Ask Your Question
1

Problem in setting goal and monitoring (and controlling) movements using odometry

asked 2013-05-02 05:12:35 -0500

Hemu gravatar image

updated 2013-05-02 05:18:57 -0500

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

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2013-05-13 04:14:15 -0500

Hemu gravatar image

updated 2013-05-13 04:26:05 -0500

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)
    curr_head=asin(2*Z*W);
  else if (Z>0.707) // Heading>pi/2 and Heading<pi
    curr_head=3.14-asin(2*Z*W);
  else if (Z<-0.707) // Heading (-pi/2,-pi)
    curr_head=-3.14-asin(2*Z*W);
edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2013-05-02 05:12:35 -0500

Seen: 680 times

Last updated: May 13 '13