Question

I want to create function turn where I will enter some angle and the robot would turn using the odometry message. I have a problem with waiting for odometry message, because the fuction will be called with the theta_current = 0, which is initial value. I thought that if I use function waitForMessage and afterwards spinOnce, I should get the current value. But I am wrong and I have no idea how to solve that problem. And yes I did a lot of googling but I found only the examples using tf.

//global variables
    double x_current = 0; //used to move
    double y_current = 0; //used to move
    double theta_current = 0; //used to move

    int main(int argc, char **argv)
    {
      ros::init(argc, argv, "robot_node");
      ros::NodeHandle n;

      velocity_pub = n.advertise<geometry_msgs::Twist>("/robot1/cmd_vel", 1000);



        ros::topic::waitForMessage<nav_msgs::Odometry>("/robot1/odom");
        ros::Subscriber sub = n.subscribe("/robot1/odom",1000,getOdom);

        ros::spinOnce();

        turn(90.0, 0.2, 1);

      ros::Rate r(10);
      while(ros::ok())
      {
          ros::spinOnce();
          r.sleep();
      }

      return 0;
    }


    void set_velocities(float lin_vel, float ang_vel)
    {
        geometry_msgs::Twist msg;
        msg.linear.x = lin_vel;
        msg.angular.z = ang_vel;

        velocity_pub.publish(msg);
    }

    void turn(float angleDeg, double angularSpeed, int direction)
    {
        float currentAngleDeg = theta_current*M_PI/180; //convert current angle in radians to degrees
        float endAngle = currentAngleDeg + currentAngleDeg;
        ros::Rate rate(10);
        while(currentAngleDeg < endAngle)
        {
            set_velocities(0.0, 0.2);
            ros::spinOnce();
            rate.sleep();

        }
        set_velocities(0.0, 0.0);
    }


    void getOdom(const nav_msgs::Odometry::ConstPtr& msg)
    {
        theta_current = msg->twist.twist.angular.z;
        x_current = msg->pose.pose.position.x;
        y_current = msg->pose.pose.position.y;
    }

Asked by horczech on 2016-05-24 13:01:50 UTC

Answer 1

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

Asked by jacobperron on 2016-05-25 17:30:40 UTC

Turn the robot using the odometry message

Comments

Answers

Comments