ROS1入门教程6：复杂行为处理

一、新建项目

bash 复制代码

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

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

二、创建行为

bash 复制代码

# 创建行为文件夹
mkdir action && cd action

# 创建行为文件
vim Move.action

# 定义行为
uint32 goal
---
uint32 result
---
uint32 feedback

三、创建服务端

cpp 复制代码

#include <iostream>
#include <thread>
#include <atomic>

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

std::atomic<bool> g_isCanceled{false};

void done_cb(actionlib::ServerGoalHandle<demo::MoveAction> goalHandle) {
    ROS_INFO("==> done_cb");
    if (goalHandle.getGoal()->destination > 100) {
        goalHandle.setRejected(); // 拒绝请求
        return;
    }
    goalHandle.setAccepted(); // 同意请求
    ros::Rate rate(1);
    uint32_t distance = 0;
    while (ros::ok() && distance < goalHandle.getGoal()->destination) {
        if (g_isCanceled) {
            goalHandle.setCanceled(); // 取消任务
            return;
        }
        demo::MoveFeedback feedback;
        feedback.distance = distance;
        goalHandle.publishFeedback(feedback);
        ROS_INFO("==> distance: %d", distance);
        rate.sleep();
        ++distance;
    }
    demo::MoveResult result;
    result.result = distance;
    goalHandle.setSucceeded(result); // 执行成功
}

void goal_cb(actionlib::ServerGoalHandle<demo::MoveAction> goalHandle) {
    ROS_INFO("==> goal_cb, goal: %d", goalHandle.getGoal()->destination);
    g_isCanceled = false;
    std::thread t(done_cb, goalHandle);
    t.detach();
}

void cancel_cb(actionlib::ServerGoalHandle<demo::MoveAction> goalHandle) {
    ROS_INFO("==> cancel_cb");
    g_isCanceled = true;
}

int main(int argc, char* argv[]) {
    ros::init(argc, argv, "server");
    ros::NodeHandle nodeHandle;
    std::string actionName = "demo6";
    actionlib::ActionServer<demo::MoveAction> server(
        nodeHandle,
        actionName,
        boost::bind(goal_cb, _1),
        boost::bind(cancel_cb, _1),
        false
    );
    server.start();
    ros::spin();

    return EXIT_SUCCESS;
}

四、创建客户端

cpp 复制代码

#include <iostream>

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

void transition_cb(actionlib::ClientGoalHandle<demo::MoveAction> goalHandle) {
    ROS_INFO("==> transition_cb, state: %s", goalHandle.getCommState().toString().c_str());
}

void feedback_cb(actionlib::ClientGoalHandle<demo::MoveAction> goalHandle, const demo::MoveFeedbackConstPtr &feedback) {
    ROS_INFO("==> feedback_cb, distance: %d", feedback->distance);
}

int main(int argc, char* argv[]) {
    ros::init(argc, argv, "client");
    ros::AsyncSpinner spinner(1);
    spinner.start();
    ros::NodeHandle nodeHandle;
    std::string actionName = "demo6";
    actionlib::ActionClient<demo::MoveAction> client(nodeHandle, actionName); // 创建行为客户端
    client.waitForActionServerToStart(); // 等待行为服务器启动
    demo::MoveGoal goal;
    goal.destination = 5;
    // 注意：必须赋值给一个对象，否则会被释放掉
    const actionlib::ActionClient<demo::MoveAction>::GoalHandle &goalHandle = client.sendGoal( // 发送目标
        goal,
        boost::bind(transition_cb, _1),
        boost::bind(feedback_cb, _1, _2)
    );
    ros::waitForShutdown();
    
    return EXIT_SUCCESS;
}

五、修改编译配置

bash 复制代码

# 查找catkin库
find_package(catkin REQUIRED COMPONENTS
  actionlib_msgs
  message_generation
  roscpp
  rosmsg
  tf
)

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

# 编译行为文件
generate_messages(
  DEPENDENCIES
  actionlib_msgs
)

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

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

六、运行效果

服务端：

客户端：