ROS1入门教程5：简单行为处理

一、新建项目

# 创建工作空间
mkdir -p demo5/src && cd demo5

# 初始化工作空间
catkin_make

# 创建功能包
cd src
catkin_create_pkg demo roscpp actionlib_msgs message_generation tf

二、创建行为

# 创建行为目录
mkdir action && cd action

# 创建行为文件
vim Move.action

# 定义行为内容
uint32 destination
---
bool arrived
---
uint32 distance

三、修改编译配置

# 添加行为文件
add_action_files(
  FILES
  Move.action
)

# 生成消息文件
generate_messages(
  DEPENDENCIES
  std_msgs
  actionlib_msgs
)

# 添加源文件
add_executable(server src/server.cpp)
add_executable(client src/client.cpp)

# 添加依赖
add_dependencies(server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
add_dependencies(client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)

# 链接catkin库
target_link_libraries(server ${catkin_LIBRARIES})
target_link_libraries(client ${catkin_LIBRARIES})

四、创建行为服务端

#include <iostream>

#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include "demo/MoveAction.h"

void ActionCallback(const demo::MoveGoalConstPtr &goal, actionlib::SimpleActionServer<demo::MoveAction> *server) {
    if (goal.get() != nullptr) {
        uint32_t destination = goal.get()->destination;
        ROS_INFO("destination: %d", destination);
        ros::Rate rate(1);
        for (uint32_t distance = 0; distance < destination; ++distance) {
            demo::MoveFeedback feedback;
            feedback.distance = distance;
            server->publishFeedback(feedback);
            rate.sleep();
        }
        demo::MoveResult result;
        result.arrived = destination;
        server->setSucceeded(result);
    }
}

int main(int argc, char* argv[]) {
    std::string nodeName = "Server";
    ros::init(argc, argv, nodeName);
    ros::NodeHandle nodeHandle;
    std::string actionName = "move";
    actionlib::SimpleActionServer<demo::MoveAction> actionServer(nodeHandle, actionName, boost::bind(&ActionCallback, _1, &actionServer), false);
    actionServer.start();
    ros::spin();

    return EXIT_SUCCESS;
}

五、创建行为客户端

#include <iostream>

#include "ros/ros.h"
#include "actionlib/client/simple_action_client.h"
#include "demo/MoveAction.h"

void doneCallback(const actionlib::SimpleClientGoalState &state, const demo::MoveResultConstPtr &result, actionlib::SimpleActionClient<demo::MoveAction> *client) {
    ROS_INFO("MoveAction state: %s", state.toString().c_str());
    if (state == state.SUCCEEDED) {
        ros::shutdown();
    }
}

void activeCallback() {
    ROS_INFO("MoveAction active!!!");
}

void feedbackCallback(const demo::MoveFeedbackConstPtr &feedback) {
    ROS_INFO("MoveAction feedback: %d", feedback.get()->distance);
}

int main(int argc, char* argv[]) {
    std::string nodeName = "Client";
    ros::init(argc, argv, nodeName);
    ros::NodeHandle nodeHandle;
    std::string actionName = "move";
    actionlib::SimpleActionClient<demo::MoveAction> &&actionClient = actionlib::SimpleActionClient<demo::MoveAction>(nodeHandle, actionName);
    actionClient.waitForServer();
    demo::MoveGoal goal;
    goal.destination = 5;
    actionClient.sendGoal(goal, boost::bind(&doneCallback, _1, _2, &actionClient), boost::bind(&activeCallback), boost::bind(&feedbackCallback, _1));
    ros::spin();

    return EXIT_SUCCESS;
}