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

edit

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