Ask Your Question
0

How to process geometry messages of type PoseWithCovarianceStamped and PoseStampe

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

RB gravatar image

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

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 :

WHOLE TOPIC.jpg

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
0

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

Tom Moore gravatar image

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

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:

while(ros::ok())
{
  ros::spinOnce(); // Callbacks are processed here

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

Comments

Thanks for the answer @Tom Moore

RB gravatar imageRB ( 2014-04-30 03:54:33 -0600 )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

Stats

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

Seen: 2,353 times

Last updated: Apr 30 '14