get initial pose of turtle in turtlesim?
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 ...