前言

   机器人在小场景下运动时，应该尽可能在运动过程中保持直线前进和后退，减少运动过程中的旋转，因此，在机器人接收到目标点后先旋转朝向目标点方向，然后沿着直线路径运动到达目标点，最后旋转进行姿态的纠正(不考虑途中的障碍物)。

一、CarrotPlanner全局路径规划

   CarrotPlanner全局规划器作为ROS自带的规划器，采用直线规划，规划出到达目标点的最短直线路径，能够满足机器人在小场景下的需要。在使用时将全局路径规划改为"carrot_planner/CarrotPlanner""即可。为了可视化显示，将CarrotPlanner规划的路径按照其他全局路径规划算法将路径发布出去，便于观察。

  void CarrotPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path)
  {
      if (!initialized_)
      {
          ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
          return;
      }

      //create a message for the plan
      nav_msgs::Path gui_path;
      gui_path.poses.resize(path.size());

      gui_path.header.frame_id = "map";
      gui_path.header.stamp = ros::Time::now();

      // Extract the plan in world co-ordinates, we assume the path is all in the same frame
      for (unsigned int i = 0; i < path.size(); i++)
      {
          gui_path.poses[i] = path[i];
      }

      plan_pub_.publish(gui_path);
  }

二、基于路径跟踪的导航实现

1.起始姿态调整

   在得到目标点后，计算当前位置与目标位置的夹角，按照夹角原地旋转即可,机器人当前位姿可根据里程计来获取。
主要代码：

            rotation_yaw = atan2(goal_y - current_y, goal_x - current_x);
            rotation_yaw = rotation_yaw - current_yaw;
            normalAngle(rotation_yaw); //角度限制在-PI 到PI
   
            if(abs(rotation_yaw) > yaw_tolerance_){
                geometry_msgs::Twist cmd_vel;
                cmd_vel.linear.x = 0;
                cmd_vel.angular.z =  rotation_yaw * 0.5;  
                cmd_vel_pub_.publish(cmd_vel);
            }
            else{
                ROS_WARN("rotation finished, start to front move");
            }

2.路径跟随

   在机器人朝向目标点后，使用CarrotPlanner规划出一条从起点到目标点的直线，之后进行跟随就好，为方便使用，可以采用局部路径插件的形式实现。

主要代码：

        std::vector<geometry_msgs::PoseStamped> global_plan_;
        geometry_msgs::PoseStamped target_pose;
        double dist;
        for(int i=target_index_;i<global_plan_.size();i++)
        {
            geometry_msgs::PoseStamped pose_base;
            global_plan_[i].header.stamp = ros::Time(0);
            tf_listener_->transformPose("base_link",global_plan_[i],pose_base);
            double dx = pose_base.pose.position.x;
            double dy = pose_base.pose.position.y;
            dist = std::sqrt(dx*dx + dy*dy);

            if (dist > 0.1) 
            {
                target_pose = pose_base;
                target_index_ = i;
                break;
            }

            if(i == global_plan_.size()-1)
                target_pose = pose_base; 
        }

        cmd_vel.linear.x = target_pose.pose.position.x * 0.5;
        cmd_vel.angular.z = target_pose.pose.position.y * 0.5;

3.终点姿态调整

   到达目标点后，根据目标点的角度旋转即可。

            double final_yaw = tf::getYaw(pose_final.pose.orientation);
            ROS_WARN("调整最终姿态，final_yaw = %.2f",final_yaw);
            cmd_vel.linear.x = pose_final.pose.position.x * 0.0;
            cmd_vel.angular.z = final_yaw *0.5; //角速度调整
            ROS_WARN("cmd_vel_x: %.2f, cmd_vel_z: %.2f",cmd_vel.linear.x,cmd_vel.angular.z);

            if(abs(final_yaw) < 0.05)
            {
                goal_reached_ = true;
                ROS_WARN("arrived goal！");
                cmd_vel.linear.x = 0;
                cmd_vel.angular.z = 0;
            }

三、仿真结果

   从动图中可看出，小场景下，给定目标点后，机器人先旋转朝向目标点，然后规划出直线，运动到目标点后再次纠正姿态，符合预期，使用路径跟随的方式，在到达目标点后可以保证较小的导航误差，对于旋转过快可以加入角速度限制进行优化，使用局部路径插件的形式和move_base接口对应，便于后续处理。