ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

get initial pose of turtle in turtlesim?

asked 2021-07-17 04:06:31 -0600

Victor_Kash gravatar image

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<<goal_in_radians<<" rads"<<std::endl;
        std::cout<<yaw<<std::endl;
        float relative_angle_radians = goal_in_radians-yaw;
        std::cout<<relative_angle_radians<<" relative "<<std::endl;
        if(relative_angle_radians<0)
            clockwise=1;
        else clockwise = 0;
        std::cout<<"the desired orientaion was "<<goal_in_degrees<<std::endl;
        std::cout<<"have to move relatively "<<todegrees(abs(relative_angle_radians))<<std::endl;
        rotate(30, todegrees(abs(relative_angle_radians)),clockwise, pub);
    }

    int main(int argc, char**argv)
    {
        ros::init(argc, argv, "cleaing_bot_orientation_cpp");
        ros::NodeHandle node;
        ros::Subscriber sub = node.subscribe("turtle1/pose", 1000, posecallback);
        //initialize_pose();
        ros::Publisher pub = node.advertise<geometry_msgs::Twist>("turtle1/cmd_vel", 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 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2021-07-17 05:58:18 -0600

404RobotNotFound gravatar image

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.

edit flag offensive delete link more

Comments

Ah thanks that worked perfectly.

Victor_Kash gravatar image Victor_Kash  ( 2021-07-18 04:07:53 -0600 )edit

Question Tools

1 follower

Stats

Asked: 2021-07-17 04:06:31 -0600

Seen: 822 times

Last updated: Jul 17 '21