Move robot between points

edit

I'm trying to navigate hector_quadrotor through some points in the 3D space.
I based my implementation on this github project that moves from A to B and uses action to do so.
The server side uses the logic proposed on the github project above. To move the drone, first its high is adjusted, then it rotates on z-axis and then move forward on x. Whether , dist < 0.1 the drone reached its pose.
The code is as follows:

void NavigationServer::broadcastGoalTF()
{
  tf::Transform transform;

  // setup transformation - relative position of goal with respect to world
  transform.setOrigin( tf::Vector3(goal_point.x, goal_point.y, goal_point.z) );
  transform.setRotation(tf::Quaternion(0, 0, 0, 1));

  // publish transform
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "goal"));

}

void NavigationServer::publishTwist()
{
  geometry_msgs::Twist cmd;

  try
  {
    listener_goal.lookupTransform("/base_link", "/goal",
                             ros::Time(0), transform_goal);
  }
  catch (tf::TransformException &ex)
  {
    //catch exception
  }

  // get to desired altitude
  float heightDist = transform_goal.getOrigin().z();
  if ( heightDist > hTresh ) {
    // we need to raise up
    ROS_INFO("Adjusting high...Raising up");
    cmd.linear.z = 0.3;
    dist = sqrt(  pow(transform_goal.getOrigin().x(), 2) +
                  pow(transform_goal.getOrigin().y(), 2) +
                  pow(transform_goal.getOrigin().z(), 2)
                );

  } else if ( heightDist < -hTresh ) {
    ROS_INFO("Adjusting high...Diving Down");
    // we need to dive down
    cmd.linear.z = -0.3;

    dist = sqrt(  pow(transform_goal.getOrigin().x(), 2) +
                  pow(transform_goal.getOrigin().y(), 2) +
                  pow(transform_goal.getOrigin().z(), 2)
                );
  } else if ( abs(heightDist) < hTresh ) {
    // we are roughly in the same altitude as the goal so we stop any vertical movement
    cmd.linear.z = 0;

    ROS_INFO("Adjusting angular speed!");

    // ---- set angular speed ----
    cmd.angular.z = 3.0 * atan2(    transform_goal.getOrigin().y(),
                                    transform_goal.getOrigin().x());

    ROS_INFO("angular_speed: %f", cmd.angular.z);
    // set maximum angular speed
    if ( cmd.angular.z > 1)
    {
      ROS_INFO("angular_speed: %f", cmd.angular.z);
      cmd.angular.z = 1;
    }

    // if we are almost "facing" the goal
    if ( abs(cmd.angular.z) < 0.3) {

    ROS_INFO("Moving towards goal point (linear on x-axis)");

    cmd.linear.x = 0.4 * sqrt(  pow(transform_goal.getOrigin().x(), 2) +
                                pow(transform_goal.getOrigin().y(), 2)
                                );
      // set maximum linear speed
      if ( cmd.linear.x > 1)
      {
        cmd.linear.x = 1;
      }
    } else {
      cmd.linear.x = 0;
    }
    dist = sqrt(  pow(transform_goal.getOrigin().x(), 2) +
                  pow(transform_goal.getOrigin().y(), 2) +
                  pow(transform_goal.getOrigin().z(), 2)
                );

  }

  cmd_vel_pub_.publish(cmd);
}


void NavigationServer::executeNavigate(const move_quadcopter::NavigateGoalConstPtr& goal)
{
  ROS_INFO("move_quadcopter::navigation_server: executeNavigate");

  geometry_msgs::Twist empty;

  dist = 20.0;

  // set goal_point
  goal_point.x = goal->target_pos.x;
  goal_point.y = goal->target_pos.y;
  goal_point.z = goal->target_pos.z;

  ros::Rate loop_rate(100);

  while(ros::ok() && (dist > dTresh))
  {
    // broadcast static goal->world transformation
    broadcastGoalTF();

    // publish twist message based on robot_base -> goal transform
    publishTwist();

    ros::spinOnce();
    loop_rate.sleep();

  }

  last_goal.x = goal_point.x;
  last_goal.y = goal_point.y;
  last_goal.z = goal_point.z;

  as_.setSucceeded();
}

For moving from A(0,0,0) to B(1,5,3) its working fine. However, on moving from B to C(5,0,0) dist is always < 0,1 and it moves to D. On gazebo its like it moves from A-B-D and C doesn't exist. dist relies on transform and ... (more)