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

一、新建项目

# 创建工作空间
mkdir -p demo6/src && cd demo6# 创建功能包
catkin_create_pkg demo roscpp rosmsg actionlib_msgs message_generation tf

二、创建行为

# 创建行为文件夹
mkdir action && cd action# 创建行为文件
vim Move.action# 定义行为
uint32 goal
---
uint32 result
---
uint32 feedback

三、创建服务端

#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;
}

四、创建客户端

#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;
}

五、修改编译配置

# 查找catkin库
find_package(catkin REQUIRED COMPONENTSactionlib_msgsmessage_generationroscpprosmsgtf
)# 添加行为文件
add_action_files(FILESMove.action
)# 编译行为文件
generate_messages(DEPENDENCIESactionlib_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})