Fix time bug (thanks to Gustavo Comerlato Rodrigues) and missing frame names in ...

diff --git a/include/arc_odometry/diff_odometry.hpp b/include/arc_odometry/diff_odometry.hpp
index c30f06e..d1ae455 100644 (file)
--- a/include/arc_odometry/diff_odometry.hpp
+++ b/include/arc_odometry/diff_odometry.hpp
@@ -41,7 +41,7 @@ namespace arc_odometry
                ARC_ODOMETRY_PUBLIC
                ~DiffOdometry(void);
 
-               ARC_ODOMETRY_PUBLIC             
+               ARC_ODOMETRY_PUBLIC
                void update(double leftDisp,double rightDisp,const rclcpp::Duration &duration);
 
                ARC_ODOMETRY_PUBLIC

diff --git a/src/diff_odometry.cpp b/src/diff_odometry.cpp
index 12ef730..70e3c0c 100644 (file)
--- a/src/diff_odometry.cpp
+++ b/src/diff_odometry.cpp
@@ -42,13 +42,13 @@ namespace arc_odometry
        // deltaPhi[1] is the right wheel angular displacement
        void DiffOdometry::update(const Eigen::Vector2d &deltaPhi,const rclcpp::Duration &duration)
        {
-               double dt=duration.nanoseconds()*1e9;
+               double dt=duration.nanoseconds()*1e-9;
 
                Eigen::Vector2d deltaU;
                deltaU << (deltaPhi[0]*wheelRadius_[0]+deltaPhi[1]*wheelRadius_[1])/2.0,
                        (deltaPhi[1]*wheelRadius_[1]-deltaPhi[0]*wheelRadius_[0])/wheelBase_;
 
-               double deltaS=(fabs(deltaU[1]) > std::numeric_limits<double>::epsilon())? 
+               double deltaS=(fabs(deltaU[1]) > std::numeric_limits<double>::epsilon())?
                        deltaU[0]*sin(deltaU[1]/2)/(deltaU[1]/2):deltaU[0];
                      
                if(dt > std::numeric_limits<double>::epsilon()) u_ << deltaS/dt , deltaU[1]/dt;

diff --git a/src/odometry_publisher.cpp b/src/odometry_publisher.cpp
index 6815a72..c93a38a 100644 (file)
--- a/src/odometry_publisher.cpp
+++ b/src/odometry_publisher.cpp
@@ -65,45 +65,45 @@ OdometryPublisher::OdometryPublisher(void): Node("odometry_publisher"),
                RCLCPP_ERROR_STREAM(get_logger(),"No 'wheel_separation' parameter in node " << get_fully_qualified_name() << ".");
                return;
        }
-       
+
        declare_parameter("wheel_radius",rclcpp::PARAMETER_DOUBLE_ARRAY);
        if(!get_parameter("wheel_radius",wheelRadius_))
        {
                RCLCPP_ERROR_STREAM(get_logger(),"No 'wheel_radius' parameter in node " << get_fully_qualified_name() << ".");
                return;
        }
-       
+
        odom_.setParams(wheelBase_,wheelRadius_);
        phi_.setZero();
-       
+
        odom_frame_id_="odom";
        declare_parameter("odom_frame_id",odom_frame_id_);
        get_parameter("odom_frame_id",odom_frame_id_);
-               
+
        base_frame_id_="base_link";
        declare_parameter("base_frame_id",base_frame_id_);
        get_parameter("base_frame_id",base_frame_id_);
 
        odomPub_=create_publisher<nav_msgs::msg::Odometry>("odom",100);
-       
+
        tfBroadcaster_=std::make_unique<tf2_ros::TransformBroadcaster>(*this);
-       
+
        lastSamplingTime_=this->get_clock()->now();
 
-       using std::placeholders::_1;    
+       using std::placeholders::_1;
        jointStateSub_=create_subscription<sensor_msgs::msg::JointState>("joint_states",100,std::bind(&OdometryPublisher::jointStateCB,this,_1));
-       
+
        int rate=100;
        declare_parameter("publish_rate",rate);
        get_parameter("publish_rate",rate);
-       
+
        timer_=rclcpp::create_timer(this,this->get_clock(),rclcpp::Duration::from_seconds(1.0/rate),std::bind(&OdometryPublisher::publish,this));
 }
 
 void OdometryPublisher::jointStateCB(const sensor_msgs::msg::JointState::SharedPtr jointState)
 {
        rclcpp::Time time=jointState->header.stamp;
-       
+
        // Incremental encoders sense angular displacement and
        // not velocity
        // phi[0] is the left wheel angular displacement
@@ -122,7 +122,7 @@ void OdometryPublisher::jointStateCB(const sensor_msgs::msg::JointState::SharedP
 void OdometryPublisher::publish(void)
 {
        rclcpp::Time time=this->get_clock()->now();
-       
+
        Eigen::Vector3d x;
        Eigen::Vector2d u;
        odom_.getPose(x);
@@ -132,13 +132,13 @@ void OdometryPublisher::publish(void)
        odomMsg.header.stamp=time;
        odomMsg.header.frame_id=odom_frame_id_;
        odomMsg.child_frame_id=base_frame_id_;
-                                                       
+
        odomMsg.pose.pose.position.x=x[0];
        odomMsg.pose.pose.position.y=x[1];
-       odomMsg.pose.pose.position.z=0;
-       
+       odomMsg.pose.pose.position.z=0.0;
+
        odomMsg.pose.pose.orientation=tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0,0.0,1.0),x[2]));
-       
+
        // Fake covariance
        double pose_cov[]={1e-6, 1e-6, 1e-16,1e-16,1e-16,1e-9};
        odomMsg.pose.covariance={
@@ -155,7 +155,7 @@ void OdometryPublisher::publish(void)
        odomMsg.twist.twist.angular.x=0;
        odomMsg.twist.twist.angular.y=0;
        odomMsg.twist.twist.angular.z=u[1];
-               
+
        //Fake covariance
        double twist_cov[]={1e-6,1e-6,1e-16,1e-16,1e-16,1e-9};
        odomMsg.twist.covariance={
@@ -170,10 +170,13 @@ void OdometryPublisher::publish(void)
 
        geometry_msgs::msg::TransformStamped odomFrame;
        odomFrame.header.stamp=time;
+       odomFrame.header.frame_id=odom_frame_id_;
+       odomFrame.child_frame_id=base_frame_id_;
        odomFrame.transform.translation.x=x[0];
        odomFrame.transform.translation.y=x[1];
+       odomFrame.transform.translation.z=0.0;
        odomFrame.transform.rotation=tf2::toMsg(tf2::Quaternion(tf2::Vector3(0.0,0.0,1.0),x[2]));
-       
+
        tfBroadcaster_->sendTransform(odomFrame);
 }