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})

六、运行效果

服务端:

客户端:

相关推荐
kyle~13 小时前
ROS2---话题重映射
机器人·嵌入式·ros·控制·电控
城北有个混子19 小时前
【机器人】—— 1. ROS 概述与环境搭建
机器人·ros
龙猫略略略13 天前
px4仿真使用fastlio的定位数据飞行
ros·px4·xtdrone·激光slam·fastlio
qq_27866728614 天前
ros中相机话题在web页面上的显示,尝试js解析sensor_msgs/Image数据
前端·javascript·ros
小慧102418 天前
2.5 Rviz使用教程
linux·ros
小慧102420 天前
2.1话题发布
linux·ros
放羊郎21 天前
机器人坐标变换TF(ROS Transform)示例解释
机器人·ros·transform·tf_echo·tf
曹勖之1 个月前
在MATLAB中使用自定义的ROS2消息
开发语言·matlab·机器人·ros·simulink·ros2
冀晓武1 个月前
Ubuntu22.04 安装 ROS2 Humble
ubuntu·ros
干啥都是小小白1 个月前
话题通信之python实现
python·机器人·ros