ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | Q&A answers.ros.org

# 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