Ask Your Question

How to process geometry messages of type PoseWithCovarianceStamped and PoseStampe

asked 2014-04-29 09:16:50 -0500

RB gravatar image

updated 2014-04-29 21:12:33 -0500

Hi, I am using ROS fuerte version on Ubuntu 10.04.

I have a screen shot below (WHOLE TOPIC.jpg) which tells about what type of messages are published on topics amcl_pose and move_base_simple/goal :


Messages of amcl_pose are passed to a callback function,called currentPosCallback

void CmdVelMux::currentPosCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
 double xPos;
 double yPos;
 double zPos;
 xPos = msg->pose.pose.position.x;
 ROS_INFO("the position value is %lf \n ",xPos);

Messages of move_base_simple/goal are passed to a callback function,called goalPosCallback

void CmdVelMux::goalPosCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
    double x;
    double y;
    double z;   
    x = msg->pose.position.x;
    y = msg->pose.position.y;
   z = msg->pose.position.z;
   ROS_INFO("the position value is %lf \n ",x);

Question 1:I want to calculate angle between current goal(from move_base_simple/goal topic) and robot position (amcl_pose).

In trigonometry, we can calculate angle between two points P1(x1,y1) and P2(x2,y2). Is there any way to do this type of thing here?

I want to store this angle in class member data, what is the guarantee that it is the most updated one?

How to do that?

Thanks for any kind of help................

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted

answered 2014-04-30 03:34:06 -0500

Tom Moore gravatar image

updated 2014-04-30 03:35:00 -0500

You could:

  1. Declare member variables, say goalX, goalY, robotX, and robotY.
  2. In the callbacks, store the values in the appropriate variables.
  3. When you want to calculate the angle, just do atan2(goalY - robotY, goalX - robotX).

As for your question regarding guaranteeing that you are working with the most recent data, every time ROS does a spin, it processes all the callbacks sequentially, so your loop goes like this:

  ros::spinOnce(); // Callbacks are processed here

  double angle = ::atan2(goalY - robotY, goalX - robotX);
edit flag offensive delete link more


Thanks for the answer @Tom Moore

RB gravatar image RB  ( 2014-04-30 03:54:33 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2014-04-29 09:16:50 -0500

Seen: 2,691 times

Last updated: Apr 30 '14