From 2e0a8377c464a201e95fbc6e587fafc9771f456e Mon Sep 17 00:00:00 2001
From: Alejandro Lopez Gestoso <alopez@iri.upc.edu>
Date: Thu, 23 Nov 2023 17:04:41 +0100
Subject: [PATCH] Added odom_from_pose parameter and the corresponding
calculation
---
include/subscriber_odom2d.h | 2 ++
src/subscriber_odom2d.cpp | 31 +++++++++++++++++++++++++++++--
2 files changed, 31 insertions(+), 2 deletions(-)
diff --git a/include/subscriber_odom2d.h b/include/subscriber_odom2d.h
index 0773663..11eea5b 100644
--- a/include/subscriber_odom2d.h
+++ b/include/subscriber_odom2d.h
@@ -46,7 +46,9 @@ class SubscriberOdom2d : public Subscriber
{
protected:
ros::Time last_odom_stamp_;
+ Eigen::Vector3d last_pose_2d_;
SensorOdom2dPtr sensor_odom_;
+ bool get_odom_from_pose_;
public:
diff --git a/src/subscriber_odom2d.cpp b/src/subscriber_odom2d.cpp
index b5a0c8b..3429455 100644
--- a/src/subscriber_odom2d.cpp
+++ b/src/subscriber_odom2d.cpp
@@ -35,6 +35,7 @@
**************************/
#include <ros/ros.h>
#include <nav_msgs/Odometry.h>
+#include <tf2/utils.h>
/**************************
* STD includes *
@@ -42,6 +43,7 @@
#include <iostream>
#include <iomanip>
#include <queue>
+#include <cmath>
namespace wolf
{
@@ -51,8 +53,11 @@ SubscriberOdom2d::SubscriberOdom2d(const std::string& _unique_name,
: Subscriber(_unique_name, _server, _sensor_ptr)
, last_odom_stamp_(ros::Time(0))
, sensor_odom_(std::static_pointer_cast<SensorOdom2d>(_sensor_ptr))
+ , last_pose_2d_(0.0, 0.0, 0.0)
+ , get_odom_from_pose_(false)
{
assert(std::dynamic_pointer_cast<SensorOdom2d>(_sensor_ptr) != nullptr && "SubscriberOdom2d: sensor provided is not of type SensorOdom2d!");
+ get_odom_from_pose_ = _server.getParam<bool>(prefix_ + "/odom_from_pose");
}
void SubscriberOdom2d::initialize(ros::NodeHandle& nh, const std::string& topic)
@@ -68,8 +73,27 @@ void SubscriberOdom2d::callback(const nav_msgs::Odometry::ConstPtr& msg)
if (last_odom_stamp_ != ros::Time(0))
{
- double dt = (msg->header.stamp - last_odom_stamp_).toSec();
- Eigen::Vector2d data(msg->twist.twist.linear.x * dt, msg->twist.twist.angular.z * dt);
+ Eigen::Vector2d data;
+ if (get_odom_from_pose_)
+ {
+ double dx, dy, sign;
+ dx = msg->pose.pose.position.x - last_pose_2d_(0);
+ dy = msg->pose.pose.position.y - last_pose_2d_(1);
+ data(0) = std::sqrt(std::pow(dx,2)+std::pow(dy,2));
+ data(1) = tf2::getYaw(msg->pose.pose.orientation) - last_pose_2d_(2);
+ while (data(1) > M_PI)
+ data(1) -= 2*M_PI;
+ while (data(1) < -M_PI)
+ data(1) += 2*M_PI;
+ sign = dx*std::cos(last_pose_2d_(2)) + dy*std::sin(last_pose_2d_(2))/data(0);
+ data(0) *= (sign > 0.0? 1.0: -1.0);
+ }
+ else
+ {
+ double dt = (msg->header.stamp - last_odom_stamp_).toSec();
+ data(0) = msg->twist.twist.linear.x * dt;
+ data(1) = msg->twist.twist.angular.z * dt;
+ }
CaptureOdom2dPtr new_capture = std::make_shared<CaptureOdom2d>(
TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
sensor_ptr_,
@@ -77,6 +101,9 @@ void SubscriberOdom2d::callback(const nav_msgs::Odometry::ConstPtr& msg)
sensor_odom_->computeCovFromMotion(data));
sensor_ptr_->process(new_capture);
}
+ last_pose_2d_(0) = msg->pose.pose.position.x;
+ last_pose_2d_(1) = msg->pose.pose.position.y;
+ last_pose_2d_(2) = tf2::getYaw(msg->pose.pose.orientation);
last_odom_stamp_ = msg->header.stamp;
ROS_DEBUG("WolfNodePolyline::odomCallback: end");
--
GitLab