ROS多点巡航cpp文件

ROS多点巡航cpp文件（在网上常见的是基于python写的，有时候需要用到基于cpp的巡航文件，提供参考，亲测可行，具体方法见github）

#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

// Define a client for to send goal requests to the move_base server through a SimpleActionClient
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

int main(int argc, char** argv){
  // Initialize the pick_objects node
  ros::init(argc, argv, "pick_objects");

  //tell the action client that we want to spin a thread by default
  MoveBaseClient ac("move_base", true);
  ros::param::set("/Robot_pos", "Start_Point");

  // Wait 5 sec for move_base action server to come up
  while(!ac.waitForServer(ros::Duration(5.0))){
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  move_base_msgs::MoveBaseGoal pick_up_goal;
  move_base_msgs::MoveBaseGoal drop_off_goal;

  // set up the frame parameters
  pick_up_goal.target_pose.header.frame_id = "map";
  pick_up_goal.target_pose.header.stamp = ros::Time::now();
  drop_off_goal.target_pose.header.frame_id = "map";
  drop_off_goal.target_pose.header.stamp = ros::Time::now();

  // Define a position and orientation for the robot to reach
  pick_up_goal.target_pose.pose.position.x = 1.0;
  pick_up_goal.target_pose.pose.orientation.w = 1.0;
  drop_off_goal.target_pose.pose.position.x = -0.31;
  drop_off_goal.target_pose.pose.position.y = -0.22;
  drop_off_goal.target_pose.pose.orientation.z = 0.99;
  drop_off_goal.target_pose.pose.orientation.w = 0.009;

  // Send the goal 1
  ROS_INFO("Sending pick_up_goal");
  ac.sendGoal(pick_up_goal);
  ros::param::set("/Robot_pos", "Moving");
  // Wait an infinite time for the results
  ac.waitForResult();

  // Check if the robot reached its goal
  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
    ROS_INFO("Hooray, the base moved to the pick up goal");
    ros::param::set("/Robot_pos", "position_1");
  }
  else {
    ROS_INFO("The base failed to move to the pick up goal");
    return 0;
  }

  // wait for 5 seconds
  ros::param::set("/Robot_pos", "Moving");
  ros::Duration(5.0).sleep();
  // send the goal 2
  ROS_INFO("Sending drop_off_goal");
  ac.sendGoal(drop_off_goal);
  ac.waitForResult();

  if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
    ROS_INFO("Hooray, the base moved to the drop off goal");
    ros::param::set("/Robot_pos", "position_2");
  }
  else {
    ROS_INFO("The base failed to move to the drop off goal");
    return 0;
  }
  
  ros::param::set("/Robot_pos", "finish");


  return 0;
}

https://github.com/danibyay/pick_objects