ナビゲーション中にプログラムを介して目標点を公開し、循環ナビゲーションを実現

アクションコミュニケーションメソッドを使用して、クライアントはターゲットポイントを送信し、サーバーは処理のためにサブスクライブし、フィードバックポーズ情報をクライアント上でリアルタイムにリリースします. 具体的なコミュニケーション原則については、公式ウェブサイトのアクションコミュニケーションの紹介を参照してください.

サーバ：

/*
要件: ソフトウェアが一連のターゲットポイントの送信を実現する
1. ヘッダーファイルをインクルードする;
2. ノードの初期化;
3. NodeHandle を作成する;
4. アクションサーバーオブジェクトを作成する;
5. リクエストを処理し、フィードバックとレスポンスを生成する
6. スピン関数 spin()
*/

この部分の焦点は、ターゲットポーズをサブスクライブして公開することです。

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);
}

クライアント：

/*
要件: ソフトウェアが一連のターゲットポイントの送信を実現する
1. ヘッダーファイルを含める;
2. ノードの初期化;
3. NodeHandle を作成する;
4. アクションクライアントオブジェクトを作成する;
5. リクエストを送信する;
a. サーバーとサーバー間の接続を確立するクライアント ; --- コールバック関数
b. 継続的なフィードバックを処理します; ----------------- コールバック関数
c. 最終応答を処理します。----------------- コールバック関数
6. スピン関数 spin().
*/

// 継続的なフィードバックを処理する

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);
}

ポーズを初期化し、ターゲットポイントを送信し、コールバック関数で処理します。

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();
           }
      }

ここにプログラムの一部を貼り付けてください。賢い友人なら、私が書いたように書くことができるはずです。完全なコードが必要な場合は、申し訳ありませんが請求させていただきます ( http://t.csdn.cn/UUpbx )。