actionlib的应用：STDR仿真器定点巡航

关于STDR仿真器的定点巡航，ROS小课堂提供了非常详细的教程http://www.corvin.cn/892.html，作者是用python写的巡航脚本，下面提供一下C++版本的巡航代码。若有错误欢迎批评指正！

安装STDR仿真器

$ cd catkin_ws/src
$ git clone https://github.com/stdr-simulator-ros-pkg/stdr_simulator.git
$ cd ..
$ catkin_make

修改CMakeList.txt文件

add_executable(patrot_action src/patrot_action.cpp)
target_link_libraries( patrot_action ${catkin_LIBRARIES})
add_dependencies(patrot_action ${${PROJECT_NAME}_EXPORTED_TARGETS})

创建名为parbot_action的巡航代码

#include <ros/ros.h>  
#include <move_base_msgs/MoveBaseAction.h>  
#include <actionlib/client/simple_action_client.h> 
#include <iostream>
#include "geometry_msgs/PoseWithCovarianceStamped.h"

using namespace std;

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

// 当action完成后会调用次回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state, const move_base_msgs::MoveBaseResultConstPtr& result)
{
    ROS_INFO("Goal succeeded!");
    //ros::shutdown();
}

// 当action激活后会调用次回调函数一次
void activeCb()
{
    ROS_INFO("Goal just went active");
}

// 收到feedback后调用的回调函数
void feedbackCb(const move_base_msgs::MoveBaseFeedbackConstPtr& feedback)
{

}

// 设置机器人的初始位置
void setInitial_pose( ros::Publisher pub)
{
    geometry_msgs::PoseWithCovarianceStamped msg_poseinit;
    msg_poseinit.header.frame_id = "map";
    msg_poseinit.header.stamp = ros::Time::now();
    msg_poseinit.pose.pose.position.x = 1.0127655236;
    msg_poseinit.pose.pose.position.y = 1.99814241133;
    msg_poseinit.pose.pose.position.z = 0.000;
    msg_poseinit.pose.pose.orientation.x = 0.000;
    msg_poseinit.pose.pose.orientation.y = 0.000;
    msg_poseinit.pose.pose.orientation.z = 8.56034501716e-05;
    msg_poseinit.pose.pose.orientation.w = 0.999999996336;
    pub.publish(msg_poseinit);
    ros::Duration(1.0).sleep();
}

typedef struct _POSE
{
  double X;
  double Y;
  double Z;
  double or_x;
  double or_y;
  double or_z;
  double or_w;
} POSE;

//设置导航的定位点
POSE pose1 = {13.2, 1.590, 0.000,  0.000, 0.000, 0.006, 0.999};
POSE pose2 = {13.8, 12.33, 0.000,  0.000, 0.000, 0.990, 0.038};
POSE pose3 = {5.53,  12.48, 0.000,  0.000, 0.000, 0, 1.0};
POSE pose4 = {1.0, 2.0, 0.0,  0.0, 0.0, 0.0, 0.1};


void setGoal(POSE pose)
{
     // 定义客户端 
    MoveBaseClient ac("move_base", true);  
      
    // 等待服务器
    while(!ac.waitForServer(ros::Duration(5.0))){  
        ROS_WARN("Waiting for the move_base action server to come up");  
    }  
    
    // 创建action的goal
    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 = pose.X; 
    goal.target_pose.pose.position.y = pose.Y;  
    goal.target_pose.pose.position.z = pose.Z;   
    goal.target_pose.pose.orientation.x = pose.or_x;
    goal.target_pose.pose.orientation.y = pose.or_y;
    goal.target_pose.pose.orientation.z = pose.or_z;
    goal.target_pose.pose.orientation.w = pose.or_w;   
    
    ROS_INFO("Sending goal");  
    
    // 发送action的goal给服务器端，并且设置回调函数
    ac.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);

    // 等待结果 
    ac.waitForResult();  
      
    if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) 
    {
        ROS_INFO("it is successful"); 
    } 
       
     else  
     {
         ROS_ERROR("The base failed  move to goal!!!");
     }
       
}

int main(int argc, char** argv)
{  
  ros::init(argc, argv, "base_pose_control");  
  ros::NodeHandle n;
  ros::Publisher pub_initialpose = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 10);
  
  setInitial_pose(pub_initialpose);//发送初始位置
  MoveBaseClient ac("move_base", true);  

  int locations_cnt = 2; //设定巡航圈数
  int loop_cnt=0;//当前巡航圈数
  
  while(ros::ok())
  { 

    if(loop_cnt<locations_cnt)
    {
        loop_cnt += 1;
        ROS_INFO("Left loop cnt:%d",locations_cnt-loop_cnt);
        setGoal(pose1);
        setGoal(pose2);
        setGoal(pose3);
    }
    // 巡航结束后返回原点
    else
    {
        setGoal(pose4);
        ROS_INFO("Navigation test finished");
        ros::shutdown();
    }
    ros::spinOnce();
    //rate_loop.sleep();
  }
  return 0;  
}  
 

运行launch和巡航节点

launch文件包括启动STDR仿真器、move_base和amcl（按照ROS小课堂教程创建即可）。

$ roslaunch stdr_navigation stdr_navigation.launch
$ rosrun action_test patrot_action