turtlebot simulator gives wrong angle
Hi all,
I writing code to rotate the turtlebot to a given angle and after that print out the angle by subscring the "/odom" topic when I enter the angle it rotate to the angle and print out the angle but the printed angle is wrong ....
can anyone help me .....
thanks
this's my code ::
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
using namespace std;
ros::Subscriber pose_subscriber;
ros::Publisher velocity_publisher;
nav_msgs::Odometry robot_pose;
const double PI = 3.14159265;
void rotate(double angular_speed,double angle,bool clockwise);
double degrees2radians(double angle_in_degrees);
void poseCallback(const nav_msgs::Odometry::ConstPtr& pose_message);
int main(int argc,char** argv){
double angle;
bool clockwise;
ros::init(argc,argv,"GotoGoal_node");
ros::NodeHandle nh;
velocity_publisher = nh.advertise<geometry_msgs::Twist>("/cmd_vel_mux/input/navi",1000);
pose_subscriber = nh.subscribe("odom",1000,poseCallback);
cout << "enter angle : ";
cin >> angle;
cout << "clockwise : ";
cin >> clockwise;
rotate(degrees2radians(angle),degrees2radians(angle),clockwise);
ros::spin();
return 0;
}
void rotate(double angular_speed,double relative_angle,bool clockwise){
geometry_msgs::Twist vel_msg;
vel_msg.linear.x = 0;
vel_msg.linear.y = 0;
vel_msg.linear.z = 0;
vel_msg.angular.x = 0;
vel_msg.angular.y = 0;
if(clockwise)
vel_msg.angular.z = -abs(angular_speed);
else
vel_msg.angular.z = abs(angular_speed);
double t0 = ros::Time::now().toSec();
double current_angle = 0.0;
ros::Rate loop_rate(100);
do{
velocity_publisher.publish(vel_msg);
double t1 = ros::Time::now().toSec();
current_angle = angular_speed*(t1-t0);
ros::spinOnce();
loop_rate.sleep();
}while(current_angle < relative_angle);
vel_msg.angular.z = 0;
velocity_publisher.publish(vel_msg);
}
double degrees2radians(double angle_in_degrees){
return angle_in_degrees*(PI/180.0);
}
void poseCallback(const nav_msgs::Odometry::ConstPtr& pose_message){
double roll,pitch,yaw;
tf::Quaternion q;
tf::quaternionMsgToTF(pose_message->pose.pose.orientation,q);
tf::Matrix3x3(q).getRPY(roll,pitch,yaw);
cout << " yaw : " << yaw*(180.0/PI) << endl ;
}
this photo before entering the angle ::
and this after entering 90 degree angle and clockwise ::
Asked by jafar_abdi on 2016-06-29 15:16:07 UTC
Comments