使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战（九）- mobot区域巡逻
【摘要】 行动（action）比服务更为灵活和复杂。在给出行动具体说明之前，先简要复习一下：
主题-服务-行动：
场合
具体细节
服务/行动对比
从上面可以非常明显的看出，服务和行动的差异。
自动驾驶mobot
那么实践任务如下：
用行动实现第8讲中，第三种服务的功能...
行动（action）比服务更为灵活和复杂。在给出行动具体说明之前，先简要复习一下：
主题-服务-行动：
从上面可以非常明显的看出，服务和行动的差异。
那么实践任务如下：
- 用行动实现第8讲中，第三种服务的功能，单目标点多参数；
- 用行动实现mobot在室内环境各房间的巡逻，多目标点多参数；
- 用行动实现mobot在室外跑道的巡逻，多目标点多参数不确定。
由此，需要融合OpenAI，OpenCV和BT。从机器人主题，服务过渡到行为。
然而行为的组合，不同状态行为的切换，构成了新的挑战，进一步学习：
- 行为树 Behavior Trees C++
- 安装：sudo apt install ros-dashing-behaviortree-cpp
- 文档：https://www.behaviortree.dev/
- 源码：https://github.com/BehaviorTree/BehaviorTree.CPP/tree/ros2-devel
熟练掌握相应算法进一步可扩展：
- 物流机器人（点点轨迹，任务调度）
- 清扫机器人（区域覆盖，协作协同）
行为（action）基础复习：
行为服务器端：
- 非推荐，ros1代码风格
-
#include <inttypes.h>
-
#include <memory>
-
#include "example_interfaces/action/fibonacci.hpp"
-
#include "rclcpp/rclcpp.hpp"
-
// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'
-
#include "rclcpp_action/rclcpp_action.hpp"
-
-
using Fibonacci = example_interfaces::action::Fibonacci;
-
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
-
-
rclcpp_action::GoalResponse handle_goal(
-
const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const Fibonacci::Goal> goal)
-
{
-
RCLCPP_INFO(rclcpp::get_logger("server"), "Got goal request with order %d", goal->order);
-
(void)uuid;
-
// Let's reject sequences that are over 9000
-
if (goal->order > 9000) {
-
return rclcpp_action::GoalResponse::REJECT;
-
}
-
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-
}
-
-
rclcpp_action::CancelResponse handle_cancel(
-
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
-
{
-
RCLCPP_INFO(rclcpp::get_logger("server"), "Got request to cancel goal");
-
(void)goal_handle;
-
return rclcpp_action::CancelResponse::ACCEPT;
-
}
-
-
void execute(
-
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
-
{
-
RCLCPP_INFO(rclcpp::get_logger("server"), "Executing goal");
-
rclcpp::Rate loop_rate(1);
-
const auto goal = goal_handle->get_goal();
-
auto feedback = std::make_shared<Fibonacci::Feedback>();
-
auto & sequence = feedback->sequence;
-
sequence.push_back(0);
-
sequence.push_back(1);
-
auto result = std::make_shared<Fibonacci::Result>();
-
-
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
-
// Check if there is a cancel request
-
if (goal_handle->is_canceling()) {
-
result->sequence = sequence;
-
goal_handle->canceled(result);
-
RCLCPP_INFO(rclcpp::get_logger("server"), "Goal Canceled");
-
return;
-
}
-
// Update sequence
-
sequence.push_back(sequence[i] + sequence[i - 1]);
-
// Publish feedback
-
goal_handle->publish_feedback(feedback);
-
RCLCPP_INFO(rclcpp::get_logger("server"), "Publish Feedback");
-
-
loop_rate.sleep();
-
}
-
-
// Check if goal is done
-
if (rclcpp::ok()) {
-
result->sequence = sequence;
-
goal_handle->succeed(result);
-
RCLCPP_INFO(rclcpp::get_logger("server"), "Goal Succeeded");
-
}
-
}
-
-
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
-
{
-
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
-
std::thread{execute, goal_handle}.detach();
-
}
-
-
int main(int argc, char ** argv)
-
{
-
rclcpp::init(argc, argv);
-
auto node = rclcpp::Node::make_shared("minimal_action_server");
-
-
// Create an action server with three callbacks
-
// 'handle_goal' and 'handle_cancel' are called by the Executor (rclcpp::spin)
-
// 'execute' is called whenever 'handle_goal' returns by accepting a goal
-
// Calls to 'execute' are made in an available thread from a pool of four.
-
auto action_server = rclcpp_action::create_server<Fibonacci>(
-
node,
-
"fibonacci",
-
handle_goal,
-
handle_cancel,
-
handle_accepted);
-
-
rclcpp::spin(node);
-
-
rclcpp::shutdown();
-
return 0;
-
}
- 推荐，ros2新风格
-
#include <inttypes.h>
-
#include <memory>
-
#include "example_interfaces/action/fibonacci.hpp"
-
#include "rclcpp/rclcpp.hpp"
-
// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'
-
#include "rclcpp_action/rclcpp_action.hpp"
-
-
class MinimalActionServer : public rclcpp::Node
-
{
-
public:
-
using Fibonacci = example_interfaces::action::Fibonacci;
-
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
-
-
explicit MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
-
: Node("minimal_action_server", options)
-
{
-
using namespace std::placeholders;
-
-
this->action_server_ = rclcpp_action::create_server<Fibonacci>(
-
this->get_node_base_interface(),
-
this->get_node_clock_interface(),
-
this->get_node_logging_interface(),
-
this->get_node_waitables_interface(),
-
"fibonacci",
-
std::bind(&MinimalActionServer::handle_goal, this, _1, _2),
-
std::bind(&MinimalActionServer::handle_cancel, this, _1),
-
std::bind(&MinimalActionServer::handle_accepted, this, _1));
-
}
-
-
private:
-
rclcpp_action::Server<Fibonacci>::SharedPtr action_server_;
-
-
rclcpp_action::GoalResponse handle_goal(
-
const rclcpp_action::GoalUUID & uuid,
-
std::shared_ptr<const Fibonacci::Goal> goal)
-
{
-
RCLCPP_INFO(this->get_logger(), "Received goal request with order %d", goal->order);
-
(void)uuid;
-
// Let's reject sequences that are over 9000
-
if (goal->order > 9000) {
-
return rclcpp_action::GoalResponse::REJECT;
-
}
-
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
-
}
-
-
rclcpp_action::CancelResponse handle_cancel(
-
const std::shared_ptr<GoalHandleFibonacci> goal_handle)
-
{
-
RCLCPP_INFO(this->get_logger(), "Received request to cancel goal");
-
(void)goal_handle;
-
return rclcpp_action::CancelResponse::ACCEPT;
-
}
-
-
void execute(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
-
{
-
RCLCPP_INFO(this->get_logger(), "Executing goal");
-
rclcpp::Rate loop_rate(1);
-
const auto goal = goal_handle->get_goal();
-
auto feedback = std::make_shared<Fibonacci::Feedback>();
-
auto & sequence = feedback->sequence;
-
sequence.push_back(0);
-
sequence.push_back(1);
-
auto result = std::make_shared<Fibonacci::Result>();
-
-
for (int i = 1; (i < goal->order) && rclcpp::ok(); ++i) {
-
// Check if there is a cancel request
-
if (goal_handle->is_canceling()) {
-
result->sequence = sequence;
-
goal_handle->canceled(result);
-
RCLCPP_INFO(this->get_logger(), "Goal Canceled");
-
return;
-
}
-
// Update sequence
-
sequence.push_back(sequence[i] + sequence[i - 1]);
-
// Publish feedback
-
goal_handle->publish_feedback(feedback);
-
RCLCPP_INFO(this->get_logger(), "Publish Feedback");
-
-
loop_rate.sleep();
-
}
-
-
// Check if goal is done
-
if (rclcpp::ok()) {
-
result->sequence = sequence;
-
goal_handle->succeed(result);
-
RCLCPP_INFO(this->get_logger(), "Goal Suceeded");
-
}
-
}
-
-
void handle_accepted(const std::shared_ptr<GoalHandleFibonacci> goal_handle)
-
{
-
using namespace std::placeholders;
-
// this needs to return quickly to avoid blocking the executor, so spin up a new thread
-
std::thread{std::bind(&MinimalActionServer::execute, this, _1), goal_handle}.detach();
-
}
-
}; // class MinimalActionServer
-
-
int main(int argc, char ** argv)
-
{
-
rclcpp::init(argc, argv);
-
-
auto action_server = std::make_shared<MinimalActionServer>();
-
-
rclcpp::spin(action_server);
-
-
rclcpp::shutdown();
-
return 0;
-
}
行为客户端：
- 非推荐，ros1风格
-
#include <inttypes.h>
-
#include <memory>
-
#include "example_interfaces/action/fibonacci.hpp"
-
#include "rclcpp/rclcpp.hpp"
-
// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'
-
#include "rclcpp_action/rclcpp_action.hpp"
-
-
using Fibonacci = example_interfaces::action::Fibonacci;
-
-
-
int main(int argc, char ** argv)
-
{
-
rclcpp::init(argc, argv);
-
auto node = rclcpp::Node::make_shared("minimal_action_client");
-
auto action_client = rclcpp_action::create_client<Fibonacci>(node, "fibonacci");
-
-
if (!action_client->wait_for_action_server(std::chrono::seconds(20))) {
-
RCLCPP_ERROR(node->get_logger(), "Action server not available after waiting");
-
return 1;
-
}
-
-
// Populate a goal
-
auto goal_msg = Fibonacci::Goal();
-
goal_msg.order = 10;
-
-
RCLCPP_INFO(node->get_logger(), "Sending goal");
-
// Ask server to achieve some goal and wait until it's accepted
-
auto goal_handle_future = action_client->async_send_goal(goal_msg);
-
if (rclcpp::spin_until_future_complete(node, goal_handle_future) !=
-
rclcpp::executor::FutureReturnCode::SUCCESS)
-
{
-
RCLCPP_ERROR(node->get_logger(), "send goal call failed :(");
-
return 1;
-
}
-
-
rclcpp_action::ClientGoalHandle<Fibonacci>::SharedPtr goal_handle = goal_handle_future.get();
-
if (!goal_handle) {
-
RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server");
-
return 1;
-
}
-
-
// Wait for the server to be done with the goal
-
auto result_future = goal_handle->async_result();
-
-
RCLCPP_INFO(node->get_logger(), "Waiting for result");
-
if (rclcpp::spin_until_future_complete(node, result_future) !=
-
rclcpp::executor::FutureReturnCode::SUCCESS)
-
{
-
RCLCPP_ERROR(node->get_logger(), "get result call failed :(");
-
return 1;
-
}
-
-
rclcpp_action::ClientGoalHandle<Fibonacci>::WrappedResult wrapped_result = result_future.get();
-
-
switch (wrapped_result.code) {
-
case rclcpp_action::ResultCode::SUCCEEDED:
-
break;
-
case rclcpp_action::ResultCode::ABORTED:
-
RCLCPP_ERROR(node->get_logger(), "Goal was aborted");
-
return 1;
-
case rclcpp_action::ResultCode::CANCELED:
-
RCLCPP_ERROR(node->get_logger(), "Goal was canceled");
-
return 1;
-
default:
-
RCLCPP_ERROR(node->get_logger(), "Unknown result code");
-
return 1;
-
}
-
-
RCLCPP_INFO(node->get_logger(), "result received");
-
for (auto number : wrapped_result.result->sequence) {
-
RCLCPP_INFO(node->get_logger(), "%" PRId64, number);
-
}
- 推荐：ros2风格
-
#include <inttypes.h>
-
#include <memory>
-
#include <string>
-
#include <iostream>
-
#include "example_interfaces/action/fibonacci.hpp"
-
#include "rclcpp/rclcpp.hpp"
-
// TODO(jacobperron): Remove this once it is included as part of 'rclcpp.hpp'
-
#include "rclcpp_action/rclcpp_action.hpp"
-
-
class MinimalActionClient : public rclcpp::Node
-
{
-
public:
-
using Fibonacci = example_interfaces::action::Fibonacci;
-
using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
-
-
explicit MinimalActionClient(const rclcpp::NodeOptions & node_options = rclcpp::NodeOptions())
-
: Node("minimal_action_client", node_options), goal_done_(false)
-
{
-
this->client_ptr_ = rclcpp_action::create_client<Fibonacci>(
-
this->get_node_base_interface(),
-
this->get_node_graph_interface(),
-
this->get_node_logging_interface(),
-
this->get_node_waitables_interface(),
-
"fibonacci");
-
-
this->timer_ = this->create_wall_timer(
-
std::chrono::milliseconds(500),
-
std::bind(&MinimalActionClient::send_goal, this));
-
}
-
-
bool is_goal_done() const
-
{
-
return this->goal_done_;
-
}
-
-
void send_goal()
-
{
-
using namespace std::placeholders;
-
-
this->timer_->cancel();
-
-
this->goal_done_ = false;
-
-
if (!this->client_ptr_) {
-
RCLCPP_ERROR(this->get_logger(), "Action client not initialized");
-
}
-
-
if (!this->client_ptr_->wait_for_action_server(std::chrono::seconds(10))) {
-
RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting");
-
this->goal_done_ = true;
-
return;
-
}
-
-
auto goal_msg = Fibonacci::Goal();
-
goal_msg.order = 10;
-
-
RCLCPP_INFO(this->get_logger(), "Sending goal");
-
-
auto send_goal_options = rclcpp_action::Client<Fibonacci>::SendGoalOptions();
-
send_goal_options.goal_response_callback =
-
std::bind(&MinimalActionClient::goal_response_callback, this, _1);
-
send_goal_options.feedback_callback =
-
std::bind(&MinimalActionClient::feedback_callback, this, _1, _2);
-
send_goal_options.result_callback =
-
std::bind(&MinimalActionClient::result_callback, this, _1);
-
auto goal_handle_future = this->client_ptr_->async_send_goal(goal_msg, send_goal_options);
-
}
-
-
private:
-
rclcpp_action::Client<Fibonacci>::SharedPtr client_ptr_;
-
rclcpp::TimerBase::SharedPtr timer_;
-
bool goal_done_;
-
-
void goal_response_callback(std::shared_future<GoalHandleFibonacci::SharedPtr> future)
-
{
-
auto goal_handle = future.get();
-
if (!goal_handle) {
-
RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
-
} else {
-
RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result");
-
}
-
}
-
-
void feedback_callback(
-
GoalHandleFibonacci::SharedPtr,
-
const std::shared_ptr<const Fibonacci::Feedback> feedback)
-
{
-
RCLCPP_INFO(
-
this->get_logger(),
-
"Next number in sequence received: %" PRId64,
-
feedback->sequence.back());
-
}
-
-
void result_callback(const GoalHandleFibonacci::WrappedResult & result)
-
{
-
this->goal_done_ = true;
-
switch (result.code) {
-
case rclcpp_action::ResultCode::SUCCEEDED:
-
break;
-
case rclcpp_action::ResultCode::ABORTED:
-
RCLCPP_ERROR(this->get_logger(), "Goal was aborted");
-
return;
-
case rclcpp_action::ResultCode::CANCELED:
-
RCLCPP_ERROR(this->get_logger(), "Goal was canceled");
-
return;
-
default:
-
RCLCPP_ERROR(this->get_logger(), "Unknown result code");
-
return;
-
}
-
-
RCLCPP_INFO(this->get_logger(), "Result received");
-
for (auto number : result.result->sequence) {
-
RCLCPP_INFO(this->get_logger(), "%" PRId64, number);
-
}
-
}
-
}; // class MinimalActionClient
-
-
int main(int argc, char ** argv)
-
{
-
rclcpp::init(argc, argv);
-
auto action_client = std::make_shared<MinimalActionClient>();
-
-
while (!action_client->is_goal_done()) {
-
rclcpp::spin_some(action_client);
-
}
-
-
rclcpp::shutdown();
-
return 0;
-
}
文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。
原文链接：zhangrelay.blog.csdn.net/article/details/106228468
【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱：
cloudbbs@huaweicloud.com
- 点赞
- 收藏
- 关注作者
评论（0）