robot_pose_ekf usage-- how to subscribe /odom_combined

asked 2019-05-10 22:47:45 -0500

macleonsh gravatar image

updated 2019-05-11 04:20:49 -0500

gvdhoorn gravatar image

Dear all, I wonder if someone have the similar puzzle as a ROS newbie. I have a TB3 burger, and trying to move it around (like patroling) -- it works now but after few round moving/rotation the angular / position will accumulate quite a lot error. Search around it looks I may able to leverage the robot_pose_ekf package. Initially I simply modify the subscribe as below--it pass the compile but when runing error happen similar as this post described :[here:](https://answers.ros.org/question/117541/problem-with-subscribing-to-robot_pose_ekfodom_combined/)

// sub = hd.subscribe("/odom",1, &velControl::odomCallback,this);
   sub = hd.subscribe("/robot_pose_ekf/odom_combined",1, &velControl::odomCallback,this);

So following that post, I modify the subscribe as below--

sub = hd.subscribe<geometry_msgs::PoseWithCovarianceStamped>("/robot_pose_ekf/odom_combined",10, &velControl::odomCallback,this);

But this time it can not even pass compile, error reported something like: no matching function for call to ‘ros::NodeHandle::subscribe(const char [30], int, void (velControl::)(const ConstPtr&), velControl)’

I already include the relevant header:

#include <geometry_msgs/PoseWithCovarianceStamped.h>

Anyone can help point me the error--looks like some silly error but I cjust can not figure out..

Thanks so much!


Edit: OK I figure out the silly error I made. I did not change the &velControl::odomCallback function to be aligned with geometry_msgs::PoseWithCovarianceStamped So after change as below--the compiling issue was gone.

//void velControl::odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
 void velControl::odomCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)

But now when I run my code, the Turtlebot burger does not move in gazebo simulation enviroment... before change it can run perfect..

Is there any TF transforming working also need to be changed? below are my old why--do I need change that and how??

geometry_msgs::Quaternion orientation = msg->pose.pose.orientation;  //obtain Quaternion info.
tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));  //transfer 

double yaw, pitch, roll;  

curr_pos_.x = msg->pose.pose.position.x;  
curr_pos_.y = msg->pose.pose.position.y;  
//Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR.
//Yaw around Z axis; Pitch around Y axis; Roll around X axis
mat.getEulerYPR (yaw, pitch, roll);
curr_pos_.theta = yaw;
edit retag flag offensive close merge delete

Comments

velControl::odomCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)

are you sure the odom_combined topic carries geometry_msgs/PoseWithCovarianceStamped messages instead of nav_msgs/Odometry?

What is the output of rostopic info /robot_pose_ekf/odom_combined when you have everything running?

gvdhoorn gravatar image gvdhoorn  ( 2019-05-11 04:21:56 -0500 )edit

Sorry, I am not quite understand the logic behind your question actually, I just follow up the tutorail and kill some issue and move to here-- I may still need learn something behind all these topics and TF stuff.. Anyway, for your question, here is the my feedback

  $ rostopic info /robot_pose_ekf/odom_combined
Type: geometry_msgs/PoseWithCovarianceStamped
Publishers: 
 * /robot_pose_ekf (http://10.0.0.13:36557/)
Subscribers: 
 * /odompub (http://10.0.0.13:36477/)
macleonsh gravatar image macleonsh  ( 2019-05-11 05:18:08 -0500 )edit

BTW, I just fall-back to my old code (which turtlebot3 works OK in Gazebo--mayb I shall mention that?) Here is the output after checking /odom:

So here actually using gazebo as the publisher (and in real test this should be turtlebot3?) but as soon as I am using /robot_pose_ekf package as above , the gazebo disapear, publisher is /robot_pose_ekf? Is this the issue?

$ rostopic info /odom
Type: nav_msgs/Odometry

Publishers: 
 * /gazebo (http://10.0.0.13:40851/)

Subscribers: 
 * /rostopic_15667_1557568637853 (http://10.0.0.13:42133/)
 * /odompub (http://10.0.0.13:43735/)
macleonsh gravatar image macleonsh  ( 2019-05-11 05:37:55 -0500 )edit