... | @@ -164,7 +164,7 @@ ros2 pkg create air_lab3 --build-type ament_python --dependencies rclpy air_lab_ |
... | @@ -164,7 +164,7 @@ ros2 pkg create air_lab3 --build-type ament_python --dependencies rclpy air_lab_ |
|
For C++:
|
|
For C++:
|
|
|
|
|
|
```bash
|
|
```bash
|
|
ros2 pkg create air_lab3 --build-type ament_cmake --dependencies rclcpp rclcpp_action air_lab_interfaces --node-name tst_executor
|
|
ros2 pkg create air_lab3 --build-type ament_cmake --dependencies rclcpp rclcpp_action air_lab_interfaces irobot_create_msgs --node-name tst_executor
|
|
```
|
|
```
|
|
|
|
|
|
## TST executor service server
|
|
## TST executor service server
|
... | @@ -426,7 +426,134 @@ Executor should inherit the `AbstractNodeExecutor` class, they need to implement |
... | @@ -426,7 +426,134 @@ Executor should inherit the `AbstractNodeExecutor` class, they need to implement |
|
|
|
|
|
The following show an example of implementation for `undock`:
|
|
The following show an example of implementation for `undock`:
|
|
|
|
|
|
\\inputminted\[linenos,bgcolor=light-gray\]{c++}{echo_executor.cpp}
|
|
```c++
|
|
|
|
#include <functional>
|
|
|
|
#include <future>
|
|
|
|
#include <memory>
|
|
|
|
#include <string>
|
|
|
|
#include <sstream>
|
|
|
|
#include <thread>
|
|
|
|
|
|
|
|
#include "rclcpp/rclcpp.hpp"
|
|
|
|
#include "rclcpp_action/rclcpp_action.hpp"
|
|
|
|
|
|
|
|
#include "irobot_create_msgs/action/undock.hpp"
|
|
|
|
|
|
|
|
#include <TstML/Executor/AbstractNodeExecutor.h>
|
|
|
|
#include <TstML/Executor/AbstractNodeExecutor.h>
|
|
|
|
#include <TstML/TSTNode.h>
|
|
|
|
|
|
|
|
// Ugly hack to get a new name for each node
|
|
|
|
std::string gen_name(const std::string& _name)
|
|
|
|
{
|
|
|
|
static int counter = 0;
|
|
|
|
return _name + std::to_string(++counter);
|
|
|
|
}
|
|
|
|
|
|
|
|
class UndockExecutor : public TstML::Executor::AbstractNodeExecutor
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
using Undock = irobot_create_msgs::action::Undock;
|
|
|
|
using GoalHandleUndock = rclcpp_action::ClientGoalHandle<Undock>;
|
|
|
|
|
|
|
|
UndockExecutor(const TstML::TSTNode* _node,
|
|
|
|
TstML::Executor::AbstractExecutionContext* _context) :
|
|
|
|
TstML::Executor::AbstractNodeExecutor(_node, _context)
|
|
|
|
{
|
|
|
|
m_node = rclcpp::Node::make_shared(gen_name("undock_node"));
|
|
|
|
m_executor.add_node(m_node);
|
|
|
|
m_executor_thread = std::thread([this]() { m_executor.spin(); });
|
|
|
|
|
|
|
|
m_client_ptr = rclcpp_action::create_client<Undock>(m_node, "undock");
|
|
|
|
}
|
|
|
|
~UndockExecutor()
|
|
|
|
{
|
|
|
|
m_executor.cancel();
|
|
|
|
m_executor_thread.join();
|
|
|
|
}
|
|
|
|
TstML::Executor::ExecutionStatus start() override
|
|
|
|
{
|
|
|
|
using namespace std::placeholders;
|
|
|
|
|
|
|
|
auto goal_msg = Undock::Goal();
|
|
|
|
|
|
|
|
auto send_goal_options = rclcpp_action::Client<Undock>::SendGoalOptions();
|
|
|
|
send_goal_options.goal_response_callback =
|
|
|
|
std::bind(&UndockExecutor::goal_response_callback, this, _1);
|
|
|
|
send_goal_options.feedback_callback =
|
|
|
|
std::bind(&UndockExecutor::feedback_callback, this, _1, _2);
|
|
|
|
send_goal_options.result_callback =
|
|
|
|
std::bind(&UndockExecutor::result_callback, this, _1);
|
|
|
|
m_client_ptr->async_send_goal(goal_msg, send_goal_options);
|
|
|
|
|
|
|
|
return TstML::Executor::ExecutionStatus::Started();
|
|
|
|
}
|
|
|
|
|
|
|
|
void goal_response_callback(std::shared_future<GoalHandleUndock::SharedPtr> future)
|
|
|
|
{
|
|
|
|
m_goal_handle = future.get();
|
|
|
|
if (!m_goal_handle) {
|
|
|
|
executionFinished(TstML::Executor::ExecutionStatus::Aborted());
|
|
|
|
RCLCPP_ERROR(m_node->get_logger(), "Goal was rejected by server");
|
|
|
|
} else {
|
|
|
|
RCLCPP_INFO(m_node->get_logger(), "Goal accepted by server, waiting for result");
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void feedback_callback(
|
|
|
|
GoalHandleUndock::SharedPtr goal_handle,
|
|
|
|
const std::shared_ptr<const Undock::Feedback> feedback)
|
|
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
void result_callback(const GoalHandleUndock::WrappedResult & result)
|
|
|
|
{
|
|
|
|
switch (result.code) {
|
|
|
|
case rclcpp_action::ResultCode::SUCCEEDED:
|
|
|
|
RCLCPP_INFO(m_node->get_logger(), "Goal was succeeded");
|
|
|
|
executionFinished(TstML::Executor::ExecutionStatus::Finished());
|
|
|
|
break;
|
|
|
|
case rclcpp_action::ResultCode::ABORTED:
|
|
|
|
RCLCPP_ERROR(m_node->get_logger(), "Goal was aborted");
|
|
|
|
executionFinished(TstML::Executor::ExecutionStatus::Aborted());
|
|
|
|
return;
|
|
|
|
case rclcpp_action::ResultCode::CANCELED:
|
|
|
|
RCLCPP_ERROR(m_node->get_logger(), "Goal was canceled");
|
|
|
|
executionFinished(TstML::Executor::ExecutionStatus::Aborted());
|
|
|
|
return;
|
|
|
|
default:
|
|
|
|
RCLCPP_ERROR(m_node->get_logger(), "Unknown result code");
|
|
|
|
executionFinished(TstML::Executor::ExecutionStatus::Aborted());
|
|
|
|
return;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
TstML::Executor::ExecutionStatus pause() override
|
|
|
|
{
|
|
|
|
return TstML::Executor::ExecutionStatus::Running();
|
|
|
|
}
|
|
|
|
TstML::Executor::ExecutionStatus resume() override
|
|
|
|
{
|
|
|
|
return TstML::Executor::ExecutionStatus::Running();
|
|
|
|
}
|
|
|
|
TstML::Executor::ExecutionStatus stop() override
|
|
|
|
{
|
|
|
|
m_client_ptr->async_cancel_goal(m_goal_handle);
|
|
|
|
return TstML::Executor::ExecutionStatus::Finished();
|
|
|
|
}
|
|
|
|
TstML::Executor::ExecutionStatus abort() override
|
|
|
|
{
|
|
|
|
m_client_ptr->async_cancel_goal(m_goal_handle);
|
|
|
|
return TstML::Executor::ExecutionStatus::Aborted();
|
|
|
|
}
|
|
|
|
private:
|
|
|
|
std::shared_ptr<rclcpp::Node> m_node;
|
|
|
|
rclcpp::executors::MultiThreadedExecutor m_executor;
|
|
|
|
std::thread m_executor_thread;
|
|
|
|
rclcpp_action::Client<Undock>::SharedPtr m_client_ptr;
|
|
|
|
GoalHandleUndock::SharedPtr m_goal_handle;
|
|
|
|
};
|
|
|
|
```
|
|
|
|
|
|
## Test execution
|
|
## Test execution
|
|
|
|
|
... | | ... | |