Movebase定点巡航

#include <ros/ros.h>

#include <move_base_msgs/MoveBaseAction.h>

#include <actionlib/client/simple_action_client.h>

 

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

 

int main(int argc, char** argv)

{

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

    ros::NodeHandle nh;

    MoveBaseClient ac("move_base", true);

 

    move_base_msgs::MoveBaseGoal goals[4];

 

    // 设置第一个目标点的坐标

    goals[0].target_pose.header.frame_id = "map";

    goals[0].target_pose.pose.position.x = 1.0;

    goals[0].target_pose.pose.position.y = 2.0;

    goals[0].target_pose.pose.orientation.w = 1.0;

 

    // 设置第二个目标点的坐标

    goals[1].target_pose.header.frame_id = "map";

    goals[1].target_pose.pose.position.x = 0.0;

    goals[1].target_pose.pose.position.y = 2.0;

    goals[1].target_pose.pose.orientation.w = 1.0;

 

    // 设置第三个目标点的坐标

    goals[2].target_pose.header.frame_id = "map";

    goals[2].target_pose.pose.position.x = 0.0;

    goals[2].target_pose.pose.position.y = 0.0;

    goals[2].target_pose.pose.orientation.w = 1.0;

 

    // 设置第四个目标点的坐标

    goals[3].target_pose.header.frame_id = "map";

    goals[3].target_pose.pose.position.x = 1.0;

    goals[3].target_pose.pose.position.y = 0.0;

    goals[3].target_pose.pose.orientation.w = 1.0;

    

 

    ROS_INFO("等待MoveBase服务器...");

    ac.waitForServer(ros::Duration(5));

 

    int current_goal = 0;

    while(ros::ok())

    {

        ROS_INFO("向目标 #%d 发送请求", current_goal+1);

        ac.sendGoal(goals[current_goal]);

 

        ac.waitForResult();

 

        actionlib::SimpleClientGoalState state = ac.getState();

        ROS_INFO("Goal state: %s", state.toString().c_str());

 

        if(state == actionlib::SimpleClientGoalState::SUCCEEDED)

            ROS_INFO("机器人已到达目标 #%d", current_goal+1);

        else

            ROS_INFO("机器人未能到达目标 #%d", current_goal+1);

 

        current_goal = (current_goal + 1) % 4;

 

        ros::Duration(5).sleep();

    }

 

    return 0;

}