ROS action

自定义 action 文件

int32 input
---
int32 output
int32 goal_stamp
---
int32 feedback

CMakeLists.txt

add_action_files(
  FILES
  demo.action
)

generate_messages(DEPENDENCIES
  std_msgs
  std_srvs
  actionlib_msgs
)

package.xml

<build_depend>actionlib</build_depend>
<build_export_depend>actionlib</build_export_depend>
<exec_depend>actionlib</exec_depend>

服务端

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>

#include "ros_learning/demoAction.h"

int g_count = 0;
bool g_count_failure = false;

class ExampleActionServer {
private:

    ros::NodeHandle nh_; 
    actionlib::SimpleActionServer<ros_learning::demoAction> as_;

    ros_learning::demoGoal goal_; 
    ros_learning::demoResult result_; 
    ros_learning::demoFeedback feedback_;
    // 每部分都有对应的类名

public:
    ExampleActionServer();

    ~ExampleActionServer(void) {}

    void executeCB(const actionlib::SimpleActionServer<ros_learning::demoAction>::GoalConstPtr& goal);
};

ExampleActionServer::ExampleActionServer() : as_(nh_, 
                                                "example_action", 
                                                boost::bind(&ExampleActionServer::executeCB, this, _1),
                                                false)
{
    ROS_INFO("in constructor of exampleActionServer...");

    as_.start(); // 启动服务
    // 构造函数中的false是指在初始化时先不启动，调用start()函数之后再启动
}

void ExampleActionServer::executeCB(const actionlib::SimpleActionServer<ros_learning::demoAction>::GoalConstPtr& goal) 
{
    g_count++;
    result_.output = g_count;
    result_.goal_stamp = goal->input;
    
    if (g_count != goal->input) {
        ROS_WARN("hey--mismatch!");
        ROS_INFO("g_count = %d; goal_stamp = %d", g_count, result_.goal_stamp);

        g_count_failure = true;
        ROS_WARN("informing client of aborted goal");
        as_.setAborted(); 
    }
    else {
        as_.setSucceeded(result_);
    }
}

客户端

int main(int argc, char** argv) {
    ros::init(argc, argv, "action_server_node");

    ROS_INFO("instantiating the demo action server: ");

    ExampleActionServer as_object;
    
    ROS_INFO("going into spin");

    while (!g_count_failure && ros::ok()) {
        ros::spinOnce();
    }

    return 0;
}
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>

#include "ros_learning/demoAction.h"


void doneCb(const actionlib::SimpleClientGoalState& state,
            const ros_learning::demoResultConstPtr& result) 
{
    ROS_WARN("doneCb: server responded with state [%s]", state.toString().c_str());
    int diff = result->output - result->goal_stamp;
    ROS_WARN("got result output = %d; goal_stamp = %d; diff = %d", result->output, result->goal_stamp, diff);
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "action_client_node");
    int g_count = 0;

    ros_learning::demoGoal goal;

    actionlib::SimpleActionClient<ros_learning::demoAction> action_client("example_action", true);

    ROS_INFO("waiting for server: ");
    bool server_exists = action_client.waitForServer(ros::Duration(5.0));
    //bool server_exists = action_client.waitForServer(); //wait forever

    if (!server_exists) {
        ROS_WARN("could not connect to server; halting");
        return 0;
    }


    ROS_INFO("connected to action server");

    while (true) 
    {
        g_count++;
        goal.input = g_count;

        action_client.sendGoal(goal, &doneCb);
        // action_client.sendGoal(goal); 
        // action_client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb);

        bool finished_before_timeout = action_client.waitForResult(ros::Duration(5.0));
        // action_client.waitForResult(); // wait forever...

        if (!finished_before_timeout) {
            ROS_WARN("giving up waiting on result for goal number %d", g_count);
            return 0;
        } else {
            //if here, then server returned a result to us
        }

    }

    return 0;
}