actionlib movebase多点导航

#include "ros/ros.h"

#include "iostream"

#include "vector"

#include "move_base_msgs/MoveBaseAction.h"

#include "actionlib/client/simple_action_client.h"


typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

//客户端类型


std::vector<std::vector<float>> pts = {
  
  {0.0, 0.0}, {-1.0, -1.0}, {0.0, 0.0},{-1.0, -1.0}};


// void pts_Callback(const geometry_msgs::PointStamped::ConstPtr& msg){

// pts.push_back({msg->point.x, msg->point.y});

// }


int main(int argc, char **argv)

{

ros::init(argc, argv, "navi_action");

ros::NodeHandle nh;

int i = 1;

// ros::Subscriber sub = nh.subscribe("/clicked_points", 10, pts_Callback);

MoveBaseClient ac("move_base", true);

// "move_base" is the name of the action server：服务器

// true -> don't need ros::spin()

while (!ac.waitForServer(ros::Duration(3.0)))

{

ROS_INFO("Waiting for action server");

}

for(auto pt : pts){

move_base_msgs::MoveBaseGoal goal;

goal.target_pose.header.frame_id = "map";

goal.target_pose.header.stamp = ros::Time::now();

goal.target_pose.pose.position.x = pt[0];

goal.target_pose.pose.position.y = pt[1];

goal.target_pose.pose.orientation.w = 1.0;

ac.sendGoal(goal);

ac.waitForResult();

if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)

{

ROS_INFO("Arrived %d pts",i);

}else{

ROS_INFO("Failed");

}

i++;

}

}

第二版:在rviz点击4个目标点来设置目标点:

#include "ros/ros.h"
#include "iostream"
#include "vector"
#include "geometry_msgs/PointStamped.h"
#include "move_base_msgs/MoveBaseAction.h"
#include "actionlib/client/simple_action_client.h"

//客户端类型
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

//目标点{x,y,w=1}
std::vector<std::vector<float>> pts = {
  
  {0.0, 0.0}, {-1.0, -1.0}, {0.0, 0.0},{-1.0, -1.0}};
//控制回调次数
int counter = 0;


void pts_Callback(const geometry_msgs::PointStamped::ConstPtr& msg){
    counter++;
    pts[counter-1] = {msg->point.x, msg->point.y};
}

int main(int argc, char **argv) 
{
    ros::init(argc, argv, "navi_action");
    ros::NodeHandle nh;
    //显示第几个点
    int num = 1;
    ros::Subscriber sub = nh.subscribe("/clicked_point", 10, pts_Callback);
    while (counter < 4) {
        ros::spinOnce();  // 处理一次回调函数
    }
    MoveBaseClient ac("move_base", true); 
    // "move_base" is the name of the action server：服务器
    // true -> don't need ros::spin()
    while (!ac.waitForServer(ros::Duration(3.0)))
    {
        ROS_INFO("Waiting for action server");
    }
    for(auto pt : pts){
        move_base_msgs::MoveBaseGoal goal; 
        goal.target_pose.header.frame_id = "map";
        goal.target_pose.header.stamp = ros::Time::now();
        goal.target_pose.pose.position.x = pt[0];
        goal.target_pose.pose.position.y = pt[1];
        goal.target_pose.pose.orientation.w = 1.0;
        ac.sendGoal(goal);
        ac.waitForResult();
        if (ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("Arrived in %d pts",num);
        }else{
            ROS_INFO("Failed in %d pts",num);
        }
        num++;
    }
}