get initial pose of turtle in turtlesim?

Question

Rosanswers logo

I am trying to write a c++ program which sets the robot to a set orientation in the turtlesim. In order to calculate how much the turtle has to rotate I need the initial pose of the turtle but my code isn't able to get the initial value of theta, it just stays 0. My code maybe of poor quality because I'm still a beginner at this.

#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "turtlesim/Pose.h"
#include "math.h"
float x,y,yaw;
void posecallback(const turtlesim::Pose& data)
{
    x=data.x;
    y=data.y;
    yaw=data.theta;
    ROS_INFO("[the location of the robot is %f, %f] and the theta is %f", data.x, data.y, data.theta);
    std::cout<<x<<" is x "<<y<<" is y "<<yaw<<" is yaw"<<std::endl;
}
float toradians(int deg)
{
    return deg * (M_PI/180);
}
float todegrees(float rad)
{
    return rad*(180/M_PI);
}
void rotate(int speed_in_degrees, float angles_to_move_degrees, int clockwise, ros::Publisher pub)
{
    geometry_msgs::Twist velocity_message;
    velocity_message.linear.x=0;
    velocity_message.linear.y=0;
    velocity_message.linear.z=0;
    velocity_message.angular.x=0;
    velocity_message.angular.y=0;
    velocity_message.angular.z=0;
    float angular_speed = toradians(abs(speed_in_degrees));
    std::cout<<(angular_speed)<<" is the speed in radians "<<std::endl;
    if(clockwise)
        velocity_message.angular.z = - (angular_speed);
    else  velocity_message.angular.z = (angular_speed);
    ros::Rate loop_rate(1);
    double t0 = ros::Time::now().toSec();
    float current_angle_degree = -1.0;
    do
    {
        //ROS_INFO("the turtle rotates");
        pub.publish(velocity_message);
        double t1 = ros::Time::now().toSec();
        current_angle_degree = speed_in_degrees *(t1-t0);
        ros::spinOnce();
        loop_rate.sleep();
        std::cout<<"current angle is "<<current_angle_degree<<std::endl;
    } while (current_angle_degree<angles_to_move_degrees);
    velocity_message.angular.z=0.0;
    pub.publish(velocity_message); }
void set_desired_orientation(int goal_in_degrees, ros::Publisher pub)
{
    int clockwise=0;
    float goal_in_radians = toradians(goal_in_degrees);
    std::cout&lt;&lt;goal_in_radians&lt;&lt;&quot; rads&quot;&lt;&lt;std::endl;
    std::cout&lt;&lt;yaw&lt;&lt;std::endl;
    float relative_angle_radians = goal_in_radians-yaw;
    std::cout&lt;&lt;relative_angle_radians&lt;&lt;&quot; relative &quot;&lt;&lt;std::endl;
    if(relative_angle_radians&lt;0)
        clockwise=1;
    else clockwise = 0;
    std::cout&lt;&lt;&quot;the desired orientaion was &quot;&lt;&lt;goal_in_degrees&lt;&lt;std::endl;
    std::cout&lt;&lt;&quot;have to move relatively &quot;&lt;&lt;todegrees(abs(relative_angle_radians))&lt;&lt;std::endl;
    rotate(30, todegrees(abs(relative_angle_radians)),clockwise, pub);
}

int main(int argc, char**argv)
{
    ros::init(argc, argv, &quot;cleaing_bot_orientation_cpp&quot;);
    ros::NodeHandle node;
    ros::Subscriber sub = node.subscribe(&quot;turtle1/pose&quot;, 1000, posecallback);
    //initialize_pose();
    ros::Publisher pub = node.advertise&lt;geometry_msgs::Twist&gt;(&quot;turtle1/cmd_vel&quot;, 1000);
    set_desired_orientation(120, pub);
    return 0;
}

stdout

rosrun ros_ws cleaning_bot_desired_orientation
2.0944 rads
0
2.0944 relative 
the desired orientaion was 120
have to move relatively 114.592
0.523599 is the speed in radians 
current angle is 0.00027895
[ INFO] [1626512236.302227715]: [the location of the robot is 5.544445, 5.544445] and the theta is 1.047198
5.54444 is x 5.54444 is y 1.0472 is yaw
[ INFO] [1626512236.302430670]: [the location of the robot is 5.544445, 5.544445] and the theta is 1.047198
5.54444 is x 5.54444 is y 1.0472 is yaw
[ INFO] [1626512236.302472357]: [the location of the robot is 5.544445, 5.544445] and the theta is 1.047198
5.54444 is x 5.54444 is y 1.0472 is yaw
[ INFO] [1626512236.302491707]: [the location of the robot is 5.544445, 5.544445] and the theta is 1.047198
5.54444 is x 5.54444 is y 1.0472 is yaw

and so on, because the initial yaw is not set the turtle moves 120 degrees every time the code is run and no set to global desired orientation.

Originally posted by Victor_Kash on ROS Answers with karma: 50 on 2021-07-17

Post score: 0

score 0 · Accepted Answer · answered Jul 17 '21 at 05:58

Rosanswers logo

There are two issues that I can see, the first is that it looks like you only spin in the rotate function, and you use yaw in the set_desired_orientation function. You will never get the value if you don't spin the node before trying to use it.

Also, you create the subscriber for the pose and then right after that call the function for set_desired_orientation where you use the yaw variable you are listening for, the subscriber hasn't had time to setup and get data.

It would probably be a good idea to restructure this a little, but a quick fix could be initialize the yaw variable to None (it might already by default, I don't use python that often), and then throw a while loop in the top of the set_desired_orientation where you wait for it to not be None while also doing a spinOnce. I would add a print in the loop and a sleep for like half a second so it doesn't spin freely.

Originally posted by 404RobotNotFound with karma: 331 on 2021-07-17

This answer was ACCEPTED on the original site

Post score: 1

Original comments

Comment by Victor_Kash on 2021-07-18:
Ah thanks that worked perfectly.

get initial pose of turtle in turtlesim?

stdout

1 Answers1

Original comments