Publish the target point through the program during navigation to realize circular navigation

Using the action communication method, the client submits the target point, the server subscribes for processing, and releases the feedback pose information in real time on the client. For specific communication principles, please refer to the introduction of action communication on the official website.

Server:

/*
      Requirement: The software realizes the sending of a series of target points
                  1. Include the header file;
                  2. Node initialization;
                  3. Create NodeHandle;
                  4. Create the action server object;
                  5. Process the request, generate feedback and response
                  6. The spin function spin ().
*/

The focus of this part is mainly to subscribe to the target pose and publish it.

void CallBack(const move_base_msgs::MoveBaseGoalConstPtr &goal,Movebase_Server  *movebase_server)
{
      move_base_msgs::MoveBaseGoal  aim_goal;
      aim_goal.target_pose.header.frame_id=goal->target_pose.header.frame_id;
      aim_goal.target_pose.header.stamp=goal->target_pose.header.stamp;
      aim_goal.target_pose.pose.position.x=goal->target_pose.pose.position.x;
      aim_goal.target_pose.pose.position.y=goal->target_pose.pose.position.y;
      aim_goal.target_pose.pose.position.z=goal->target_pose.pose.orientation.w;
      ROS_INFO("提交的目标位姿为:x=%.2f,y=%.2f,w=%.2f",
                              aim_goal.target_pose.pose.position.x,
                              aim_goal.target_pose.pose.position.y,
                              aim_goal.target_pose.pose.orientation.w);
      move_base_msgs::MoveBaseFeedback  fb;
      fb.base_position=aim_goal.target_pose;
      movebase_server->publishFeedback(fb);
}

client:

/*
      Requirement: The software realizes the sending of a series of target points
                  1. Include header files;
                  2. Node initialization;
                  3. Create NodeHandle;
                  4. Create action client object;
                  5. Send request;
                        a. Establish connection between server and client ; --- callback function
                        b. handles the continuous feedback; ----------------- callback function
                        c. handles the final response. -----------------Callback function
                  6. Spin function spin().
*/

// handle continuous feedback

void feedback_cb(const move_base_msgs::MoveBaseFeedbackConstPtr &feedback)
{
      ROS_INFO("机器人当前位姿为:[x=%.2f,y=%.2f,w=%.2f]",
                              feedback->base_position.pose.position.x,
                              feedback->base_position.pose.position.y,
                              feedback->base_position.pose.orientation.w);
}

 Initialize the pose, send the target point, and process it in the callback function.

void done_cb(const actionlib::SimpleClientGoalState &state, 
             const move_base_msgs::MoveBaseResultConstPtr &result);//状态反馈
void  active_cb();// 服务端与客户端建立连接
void feedback_cb(const move_base_msgs::MoveBaseFeedbackConstPtr &feedback);// 处理连续反馈

 actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>  MoveBase_Client(nh,"move_base",true);
      // 等待服务端启动
      // MoveBase_Client.waitForServer();
      while(!MoveBase_Client.waitForServer(ros::Duration(5.0)))
      {
            ROS_INFO("Waiting for the  move_base action server!");
      }
      // 5.发送请求;
      // 定义坐标数据
      int size_c=8;
      double  airfloat_coord[size_c][2]={
   
   {0,0},{1,0},{2,0},{2,1},{2,2},{1,2},{0,2},{0,1}};

      move_base_msgs::MoveBaseGoal  goal;
      goal.target_pose.header.frame_id="map";          
      goal.target_pose.header.stamp=ros::Time::now();
      // 读取数组中的目标点位置数据
      for (int pose_x = 0; pose_x <=size_c; pose_x++)
      // for (int pose_x = 0; pose_x <=GoalNums; pose_x++)
      {
            for (int pose_y = 0; pose_y <2; pose_y++)
            // for (int pose_y = 0; pose_y <=GoalNums; pose_y++)
           {
                  goal.target_pose.pose.position.x=airfloat_coord[pose_x][0];
                  if (pose_y==1)
                  {
                        goal.target_pose.pose.position.y=airfloat_coord[pose_x][pose_y];
                  }
                  // goal.target_pose.pose.position.x=pose_x;
                  // goal.target_pose.pose.position.y=pose_y;
                  goal.target_pose.pose.orientation.w=1.0;
                  // goal.target_pose.pose.orientation.w=atan2(goal.target_pose.pose.position.y,goal.target_pose.pose.position.x) ;

                  /*
                        void sendGoal(const move_base_msgs::MoveBaseGoal &goal, 
                                    boost::function<void (const actionlib::SimpleClientGoalState &state, const move_base_msgs::MoveBaseResultConstPtr &result)> done_cb,
                                    boost::function<void ()> active_cb, 
                                    boost::function<void (const move_base_msgs::MoveBaseFeedbackConstPtr &feedback)> feedback_cb)
                  */
                  ROS_INFO("发送目标!");
                  MoveBase_Client.sendGoal(goal,&done_cb,&active_cb,&feedback_cb);
                  MoveBase_Client.waitForResult();
                  ros::Duration(1.0).sleep();
           }
      }

Paste part of the program here, smart friends should be able to write it according to what I wrote, if you want the complete code, I will charge you unkindly ( http://t.csdn.cn/UUpbx ).