Definition
As we saw on fundamentals, ROS actions are another type of communication that is intended to handle long running tasks. It is divided into three parts: a goal, a feedback and a result.
Creating an Action
We define an action by creating a file called *.action file with the format:
(request)
---
(result)
---
(feedback)
- A request message is sent from an action client to an action server initiating a new goal.
- A result message is sent from an action server to an action client when a goal is done.
- Feedback messages are periodically sent from an action server to an action client with updates about a goal.
Let's create a package called fib_action_interface that will hold the data formats for an Action that will calculate and return fibonacci sequence for a number.
ros2 pkg create --build-type ament_cmake fib_action_interface Now, make a dir called actions and create a fib.action file on it.
mkdir -p src/fib_action_interface/actions touch src/fib_action_interface/actions/fib.action
Let's edit the Fibonacci.action file and put the following format inside of it:
int32 order
---
int32[] sequence
---
int32[] partial_sequence
---
int32[] sequence
---
int32[] partial_sequence
Now, edit the CMakeList.txt file and add before ament_package() :
find_package(rosidl_default_generators REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"action/Fibonacci.action"
)
Also, add the following to package.xml:
<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
<member_of_group>rosidl_interface_packages</member_of_group>
Let's now compile and check
colcon build --packages-select fib_action_interface. install/setup.bash ros2 interface show fib_action_interface/action/Fibonacci
int32 order
---
int32[] sequence
---
int32[] partial_sequence
Nice, the Action is registered in ROS2. So, now, let's build the actual action code. A skeleton file for an action could be implemented by the following:
#include <functional>
#include <memory>
#include <thread>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include "fib_action_interface/action/fibonacci.hpp"
namespace fibonacci_action
{
using Fibonacci = fib_action_interface::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ServerGoalHandle<Fibonacci>;
using std::placeholders::_1;
using std::placeholders::_2;
class FibonacciActionServer : public rclcpp::Node
{
public:
explicit FibonacciActionServer(const rclcpp::NodeOptions &options =
rclcpp::NodeOptions()) : Node("FibonacciActionServer", options)
{
this->server = rclcpp_action::create_server<Fibonacci>(
this,
"fibonacci",
std::bind(&FibonacciActionServer::cb_goal, this, _1, _2),
std::bind(&FibonacciActionServer::cb_cancel, this, _1),
std::bind(&FibonacciActionServer::cb_accepted, this, _1));
}
private:
rclcpp_action::Server<Fibonacci>::SharedPtr server;
rclcpp_action::GoalResponse cb_goal(
const rclcpp_action::GoalUUID &uuid,
std::shared_ptr<const Fibonacci::Goal> goal)
{
//handles goal request, returning the goal response
}
rclcpp_action::CancelResponse cb_cancel(
const std::shared_ptr<GoalHandleFibonacci> goal_handler)
{
//handles action cancel, returning the cancel response
}
void cb_accepted(
const std::shared_ptr<GoalHandleFibonacci> goal_handler)
{
//handles accepted state, where we need to execute
//the heavy code, so we create another thread to
//execute it
std::thread{
std::bind(&FibonacciActionServer::cb_accepted_exec,
this, _1),
goal_handler}.detach();
}
void cb_accepted_exec(
const std::shared_ptr<GoalHandleFibonacci> goal_handler){
// background threat to execute the action
};
};
}
// Registering the server as a node
RCLCPP_COMPONENTS_REGISTER_NODE(fibonacci_action::FibonacciActionServer)
After that, we just need to make sure package.xml has all the dependencies set
<buildtool_depend>ament_cmake</buildtool_depend>
<buildtool_depend>fib_action_interface</buildtool_depend>
<buildtool_depend>rclcpp</buildtool_depend>
<buildtool_depend>rclcpp_action</buildtool_depend>
<buildtool_depend>rclcpp_components</buildtool_depend>
And then, we do configure the CMakeLists.txt with the following definitions:
cmake_minimum_required(VERSION 3.8)
project(fibonacci_action)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(fib_action_interface REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(rclcpp_components REQUIRED)
add_library(action_server SHARED
src/fibonacci.cpp)
target_include_directories(action_server PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
ament_target_dependencies(action_server
"fib_action_interface"
"rclcpp"
"rclcpp_action"
"rclcpp_components")
rclcpp_components_register_node(action_server PLUGIN
"fibonacci_action::FibonacciActionServer"
EXECUTABLE fibonacci_action_server)
install(TARGETS
action_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
ament_package()
Finally, let's compile this package:
colcon build --packages-select fibonacci_action Now, we need to create a client to consume this action. Let's create a fibonacci_client.cpp. A skeleton file for an action client could be implemented by the following:
#include <functional>
#include <memory>
#include <thread>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include "fib_action_interface/action/fibonacci.hpp"
namespace fibonacci_action
{
using Fibonacci = fib_action_interface::action::Fibonacci;
using GoalHandleFibonacci =
rclcpp_action::ClientGoalHandle<Fibonacci>;
using std::placeholders::_1;
using std::placeholders::_2;
class FibonacciActionClient : public rclcpp::Node
{
public:
explicit FibonacciActionClient(
const rclcpp::NodeOptions &options =
rclcpp::NodeOptions()) :
Node("FibonacciActionClient", options)
{
this->client = rclcpp_action::create_client<Fibonacci>(
this,
"fibonacci");
}
void request_action()
{
using namespace std::placeholders;
if (!this->client->wait_for_action_server())
{
RCLCPP_ERROR(this->get_logger(),
"Action server not available after waiting");
rclcpp::shutdown();
}
// Build options using the client
auto send_goal_options =
rclcpp_action::Client<Fibonacci>::SendGoalOptions();
// Setting the action's callbacks in the options object
send_goal_options.goal_response_callback =
std::bind(&FibonacciActionClient::cb_goal_response,
this, _1);
send_goal_options.feedback_callback =
std::bind(&FibonacciActionClient::cb_feedback,
this, _1, _2);
send_goal_options.result_callback =
std::bind(&FibonacciActionClient::cb_result,
this, _1);
// building the request message
auto goal_msg = Fibonacci::Goal();
goal_msg.order = 10; // 10 position fibonnaci
RCLCPP_INFO(this->get_logger(), "Sending goal");
// use client to call for action
this->client->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<Fibonacci>::SharedPtr client;
void cb_goal_response(
std::shared_future<GoalHandleFibonacci::SharedPtr> future)
{
// handles goal request, returning the goal response
}
void cb_feedback(GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
// handles the action cancel, returning the cancel response
}
void cb_result(const GoalHandleFibonacci::WrappedResult &result)
{
// handles the action result
}
};
}
// Registering the client as a node
RCLCPP_COMPONENTS_REGISTER_NODE(fibonacci_action::FibonacciActionClient)
A complete client node would be something like:
#include <functional>
#include <memory>
#include <thread>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include "fib_action_interface/action/fibonacci.hpp"
namespace fibonacci_action
{
using Fibonacci = fib_action_interface::action::Fibonacci;
using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle<Fibonacci>;
using std::placeholders::_1;
using std::placeholders::_2;
class FibonacciActionClient : public rclcpp::Node
{
public:
explicit FibonacciActionClient(const rclcpp::NodeOptions &options =
rclcpp::NodeOptions()) :
Node("FibonacciActionClient", options)
{
this->client = rclcpp_action::create_client<Fibonacci>(
this,
"fibonacci");
this->timer = this->create_wall_timer(
std::chrono::milliseconds(500), [this]() -> void
{ this->request_fibonacci_sequence(10); });
}
void request_fibonacci_sequence(int seq_order)
{
using namespace std::placeholders;
if (!this->client->wait_for_action_server())
{
RCLCPP_ERROR(this->get_logger(),
"Action server not available after waiting");
rclcpp::shutdown();
}
// Build options using the client
auto send_goal_options =
rclcpp_action::Client<Fibonacci>::SendGoalOptions();
// Setting the action's callbacks in the options object
send_goal_options.goal_response_callback =
std::bind(&FibonacciActionClient::cb_goal_response,
this, _1);
send_goal_options.feedback_callback =
std::bind(&FibonacciActionClient::cb_feedback,
this, _1, _2);
send_goal_options.result_callback =
std::bind(&FibonacciActionClient::cb_result,
this, _1);
// building the request message
auto goal_msg = Fibonacci::Goal();
goal_msg.order = seq_order; // 10 position fibonnaci
RCLCPP_INFO(this->get_logger(), "Sending goal");
// use client to call for action
this->client->async_send_goal(goal_msg, send_goal_options);
}
private:
rclcpp_action::Client<Fibonacci>::SharedPtr client;
rclcpp::TimerBase::SharedPtr timer;
void cb_goal_response(
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 cb_feedback(GoalHandleFibonacci::SharedPtr,
const std::shared_ptr<const Fibonacci::Feedback> feedback)
{
std::stringstream ss;
ss << "Next number in sequence received: ";
for (auto number : feedback->partial_sequence)
{
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
}
void cb_result(const GoalHandleFibonacci::WrappedResult &result)
{
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;
}
std::stringstream ss;
ss << "Result received: ";
for (auto number : result.result->sequence)
{
ss << number << " ";
}
RCLCPP_INFO(this->get_logger(), ss.str().c_str());
rclcpp::shutdown();
}
};
}
// Registering the client as a node
RCLCPP_COMPONENTS_REGISTER_NODE(fibonacci_action::FibonacciActionClient)
And we get the execution results as:
. install/setup.bash ros2 run fibonacci_action fibonacci_action_server
. install/setup.bash ros2 run fibonacci_action fibonacci_action_client
[INFO] [1649540533.111528166] [FibonacciActionClient]: Next number in sequence received: 0 1 1 2 3 5 8 13 21 34
[INFO] [1649540533.111687467] [FibonacciActionClient]: Next number in sequence received: 0 1 1 2 3 5 8 13 21 34 55
[INFO] [1649540533.113418847] [FibonacciActionClient]: Result received: 0 1 1 2 3 5 8 13 21 34 55
Nenhum comentário:
Postar um comentário