Revision history [back]

Try using tf to get the robot's yaw from the odometry message, e.g.:

#include <tf/transform_datatypes.h>
  ...
  ...
  ...
void getOdom(const nav_msgs::Odometry::ConstPtr& msg)
{
    theta_current = tf::getYaw(msg->pose.pose.orientation);
    x_current = msg->pose.pose.position.x;
    y_current = msg->pose.pose.position.y;
}

Try using tf to get the robot's yaw from the odometry message, e.g.:

#include <tf/transform_datatypes.h>
  ...
  ...
  ...
void getOdom(const nav_msgs::Odometry::ConstPtr& msg)
{
    theta_current = tf::getYaw(msg->pose.pose.orientation);
    x_current = msg->pose.pose.position.x;
    y_current = msg->pose.pose.position.y;
}

EDIT: I guess you were looking for a way not using the tf library? Regardless, you'll need a way to transform the msg->pose.pose.orientation (a Quaternion) into the robot's yaw angle. If you don't want to use tf, you could try implementing your own conversion function. Here's the math: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles