使用机器人操作系统ROS 2和仿真软件Gazebo 9行动进阶实战（九）- mobot区域巡逻

行动（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

#include

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

rclcpp_action::GoalResponse handle_goal(

const rclcpp_action::GoalUUID & uuid, std::shared_ptr 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 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 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();

auto & sequence = feedback->sequence;

sequence.push_back(0);

sequence.push_back(1);

auto result = std::make_shared();

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 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(

node,

"fibonacci",

handle_goal,

handle_cancel,

handle_accepted);

rclcpp::spin(node);

rclcpp::shutdown();

return 0;

}

推荐，ros2新风格

#include

#include

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

explicit MinimalActionServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())

: Node("minimal_action_server", options)

{

using namespace std::placeholders;

this->action_server_ = rclcpp_action::create_server(

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::SharedPtr action_server_;

rclcpp_action::GoalResponse handle_goal(

const rclcpp_action::GoalUUID & uuid,

std::shared_ptr 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 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 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();

auto & sequence = feedback->sequence;

sequence.push_back(0);

sequence.push_back(1);

auto result = std::make_shared();

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 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();

rclcpp::spin(action_server);

rclcpp::shutdown();

return 0;

}

行为客户端：

非推荐，ros1风格

#include

#include

#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(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::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::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

#include

#include

#include

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

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(

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::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::SharedPtr client_ptr_;

rclcpp::TimerBase::SharedPtr timer_;

bool goal_done_;

void goal_response_callback(std::shared_future 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 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();

while (!action_client->is_goal_done()) {

rclcpp::spin_some(action_client);

}

rclcpp::shutdown();

return 0;

}

机器人

版权声明：本文内容由网络用户投稿，版权归原作者所有，本站不拥有其著作权，亦不承担相应法律责任。如果您发现本站中有涉嫌抄袭或描述失实的内容，请联系我们jiasou666@gmail.com 处理，核实后本网站将在24小时内删除侵权内容。