From 0d33c52174616b9ce3fc07e7027f1c4d4759fab2 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Wed, 20 Apr 2022 14:28:04 -0300 Subject: [PATCH] Fix time bug (thanks to Gustavo Comerlato Rodrigues) and missing frame names in /tf publication. --- include/arc_odometry/diff_odometry.hpp | 2 +- src/diff_odometry.cpp | 4 ++-- src/odometry_publisher.cpp | 37 ++++++++++++++++++---------------- 3 files changed, 23 insertions(+), 20 deletions(-) diff --git a/include/arc_odometry/diff_odometry.hpp b/include/arc_odometry/diff_odometry.hpp index c30f06e..d1ae455 100644 --- 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 --- 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::epsilon())? + double deltaS=(fabs(deltaU[1]) > std::numeric_limits::epsilon())? deltaU[0]*sin(deltaU[1]/2)/(deltaU[1]/2):deltaU[0]; if(dt > std::numeric_limits::epsilon()) u_ << deltaS/dt , deltaU[1]/dt; diff --git a/src/odometry_publisher.cpp b/src/odometry_publisher.cpp index 6815a72..c93a38a 100644 --- 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("odom",100); - + tfBroadcaster_=std::make_unique(*this); - + lastSamplingTime_=this->get_clock()->now(); - using std::placeholders::_1; + using std::placeholders::_1; jointStateSub_=create_subscription("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); } -- 2.12.0