# 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;
end_x=dest_x+x;
end_y=dest_y+y;
if (!reached)
end_dist=sqrt((end_x*end_x)+(end_y*end_y));
ROS_INFO("Distance to destination %f",end_dist);
if (end_dist<=0.08)
{
reached=true;
}
facing=false;
else
facing=true;
if (!facing)
{
obj.linear.x=0;
{
obj.angular.z=-0.3;
obj.angular.z=-0.15;
obj.angular.z=-0.05;
else
obj.angular.z=-0.01;
}
else
{
obj.angular.z=0.3;
obj.angular.z=0.15;
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

edit retag close merge delete

Sort by » oldest newest most voted

The current heading of the robot can be found by using the following conditions:

  double Z=msg2->pose.pose.orientation.z;
double W=msg2->pose.pose.orientation.w;
if (Z<0.707 && Z>-0.707) // This corresponds to heading (-pi/2,pi/2)
else if (Z<-0.707) // Heading (-pi/2,-pi)

more