Problem in setting goal and monitoring (and controlling) movements using odometry
Hi all, I am trying to provide a goal to turtlebot (roomba base) in rectangular coordinates using /odom as the reference frame. I am trying to control and monitor the movements. The objective is to move along a straight line from initial position to final position (for the time being no obstacle avoidance required). There is a problem when I give negative destination coordinates (e.g. if robot is at (0,0) and I want it to move to (-1,-1) by first changing the yaw to the desired direction and then moving in the direction). When I calculate the yaw of the robot using its odometry (quaternion), its range is -1.57 to 1.57. However, I find the desired direction of motion using atan2(y,x) for destination (-1,-1) which requires yaw<(-1.57). So, the robot keeps rotating at its position. dest_x=x-coordinate of destination dest_y=y coordinate of destination end_dist= distance between current position and destination obj= geometry_msgs::Twist
void odomcb(const nav_msgs::Odometry::ConstPtr& msg2)
{
ROS_INFO("Odometry Callback.");
double x=msg2->pose.pose.position.x;
double y=msg2->pose.pose.position.y;
double Z=msg2->pose.pose.orientation.z;
double W=msg2->pose.pose.orientation.w;
curr_head=asin(2*Z*W); //Current Heading
ROS_INFO("Current Heading is %f",curr_head);
end_x=dest_x+x;
end_y=dest_y+y;
if (!reached)
heading=atan2(end_y,end_x); //Desired Heading
end_dist=sqrt((end_x*end_x)+(end_y*end_y));
ROS_INFO("Distance to destination %f",end_dist);
ROS_INFO("Required heading %f",heading);
if (end_dist<=0.08)
{
heading=0.0;
reached=true;
}
//curr_head=asin(2*Z*W);
if (abs(curr_head-heading)>0.05)
facing=false;
else
facing=true;
if (!facing)
{
obj.linear.x=0;
if (curr_head>heading)
{
if (curr_head-heading>0.5)
obj.angular.z=-0.3;
else if (curr_head-heading>0.2 && curr_head-heading<=0.5)
obj.angular.z=-0.15;
else if (curr_head-heading>0.05)
obj.angular.z=-0.05;
else
obj.angular.z=-0.01;
}
else
{
if (heading-curr_head>0.5)
obj.angular.z=0.3;
else if (heading-curr_head>0.2 && heading-curr_head<=0.5)
obj.angular.z=0.15;
else if (heading-curr_head>0.05)
obj.angular.z=0.05;
else
obj.angular.z=0.01;
}
}
if (facing && !reached)
{
obj.angular.z=0.0;
if (end_dist>0.5)
obj.linear.x=-0.2;
else if (end_dist>0.2)
obj.linear.x=-0.15;
else if (end_dist>0.08)
obj.linear.x=-0.05;
else
obj.linear.x=-0.00;
}
if (!facing && reached)
{
obj.linear.x=0.0;
}
if (reached && facing)
//Robot has reached destination and returns to orientation (yaw=0)
{
ROS_INFO("The robot has reached the destination.");
obj.linear.x=0.0;
obj.angular.z=0.0;
}
vel_pub.publish(obj);
}
Can anyone suggest me a solution. Please do have a look at the callback function I have created (the kinect of turtlebot is facing in reverse direction. Hence, I had to use negative linear velocity as I wish to extend this code for obstacle avoidance).
Thanks