From a8ef6aa81a5943991cfe6d164f9872d75cfe30f7 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 22 Apr 2022 16:38:34 +0900 Subject: [PATCH] feature(rtc_interface): reimprement functions to use CooperateCommands and write README.md Signed-off-by: Fumiya Watanabe --- planning/rtc_interface/README.md | 134 +++++++++++++++++- .../include/rtc_interface/rtc_interface.hpp | 19 ++- planning/rtc_interface/src/rtc_interface.cpp | 74 ++++++---- 3 files changed, 195 insertions(+), 32 deletions(-) diff --git a/planning/rtc_interface/README.md b/planning/rtc_interface/README.md index e97ed5102c2a8..3fe8b48ccb4fe 100644 --- a/planning/rtc_interface/README.md +++ b/planning/rtc_interface/README.md @@ -2,12 +2,144 @@ ## Purpose -RTC Interface is an interface to receive execution command for planning from external of an autonomous driving system. +RTC Interface is an interface to publish the decision status of behavior planning modules and receive execution command from external of an autonomous driving system. ## Inner-workings / Algorithms +### Usage example + +```c++ +// Generate instance (in this example, "intersection" is selected) +rtc_interface::RTCInterface rtc_interface(node, "intersection", tier4_rtc_msgs::msg::Module::INTERSECTION); + +// Generate UUID +const unique_identifier_msgs::msg::UUID uuid = generateUUID(getModuleId()); + +// Repeat while module is running +while (...) { + // Get safety status of the module corresponding to the module id + const bool safe = ... + + // Get distance to the object corresponding to the module id + const double distance = ... + + // Update status + rtc_interface.updateCooperateStatus(uuid, safe, distance); + + if (rtc_interface.isActivated()) { + // Execute planning + } else { + // Stop planning + } + + // Publish status topic + rtc_interface.publishCooperateStatus(); +} + +// Remove the status from array +rtc_interface.removeCooperateStatus(uuid); +``` + ## Inputs / Outputs +### RTCInterface (Constructor) + +```c++ +rtc_interface::RTCInterface(rclcpp::Node & node, const std::string & name, const tier4_rtc_msgs::msg::Module & module); +``` + +#### Description + +A constructor for `rtc_interface::RTCInterface`. + +#### Input + +- `node` : Node calling this interface +- `name` : Name of cooperate status array topic and cooperate commands service + - Cooperate status array topic name : `~/{name}/cooperate_status` + - Cooperate commands service name : `~/{name}/cooperate_commands` +- `module` : Module id defined in `tier4_rtc_msgs/Module` + +#### Output + +An instance of `RTCInterface` + +### publishCooperateStatus + +```c++ +rtc_interface::publishCooperateStatus() +``` + +#### Description + +Publish registered cooperate status. + +#### Input + +Nothing + +#### Output + +Nothing + +### updateCooperateStatus + +```c++ +rtc_interface::updateCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid, const bool safe, const double distance) +``` + +#### Description + +Update cooperate status corresponding to `uuid`. +If cooperate status corresponding to `uuid` is not registered yet, add new cooperate status. + +#### Input + +- `uuid` : UUID for requesting module +- `safe` : Safety status of requesting module +- `distance` : Distance to the object from ego vehicle + +#### Output + +Nothing + +### removeCooperateStatus + +```c++ +rtc_interface::removeCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid) +``` + +#### Description + +Remove cooperate status corresponding to `uuid` from registered statuses. + +#### Input + +- `uuid` : UUID for unregistering module + +#### Output + +Nothing + +### isActivated + +```c++ +rtc_interface::isActivated(const unique_identifier_msgs::msg::UUID & uuid) +``` + +#### Description + +Return received command status corresponding to `uuid`. + +#### Input + +- `uuid` : UUID for checking module + +#### Output + +If received command is `ACTIVATED`, return `true`. +If not, return `false`. + ## Assumptions / Known limits ## Future extensions / Unimplemented parts diff --git a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp index e15ff84a6e87c..f02484b044676 100644 --- a/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -18,23 +18,26 @@ #include "rclcpp/rclcpp.hpp" #include "tier4_rtc_msgs/msg/command.hpp" +#include "tier4_rtc_msgs/msg/cooperate_command.hpp" +#include "tier4_rtc_msgs/msg/cooperate_response.hpp" #include "tier4_rtc_msgs/msg/cooperate_status.hpp" #include "tier4_rtc_msgs/msg/cooperate_status_array.hpp" #include "tier4_rtc_msgs/msg/module.hpp" -#include "tier4_rtc_msgs/srv/cooperate_command.hpp" +#include "tier4_rtc_msgs/srv/cooperate_commands.hpp" #include #include -#include #include namespace rtc_interface { using tier4_rtc_msgs::msg::Command; +using tier4_rtc_msgs::msg::CooperateCommand; +using tier4_rtc_msgs::msg::CooperateResponse; using tier4_rtc_msgs::msg::CooperateStatus; using tier4_rtc_msgs::msg::CooperateStatusArray; using tier4_rtc_msgs::msg::Module; -using tier4_rtc_msgs::srv::CooperateCommand; +using tier4_rtc_msgs::srv::CooperateCommands; using unique_identifier_msgs::msg::UUID; class RTCInterface @@ -48,13 +51,15 @@ class RTCInterface private: void onCooperateCommandService( - const CooperateCommand::Request::SharedPtr request, - const CooperateCommand::Response::SharedPtr response); + const CooperateCommands::Request::SharedPtr request, + const CooperateCommands::Response::SharedPtr responses); + rclcpp::Logger getLogger() const; - rclcpp::Publisher::SharedPtr pub_status_; - rclcpp::Service::SharedPtr srv_command_; + rclcpp::Publisher::SharedPtr pub_statuses_; + rclcpp::Service::SharedPtr srv_commands_; mutable rclcpp::Clock clock_; + rclcpp::Logger logger_; Module module_; CooperateStatusArray registered_status_; }; diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp index ecb9f7c91283f..9737ebb099d17 100644 --- a/planning/rtc_interface/src/rtc_interface.cpp +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -14,47 +14,68 @@ #include "rtc_interface/rtc_interface.hpp" +namespace +{ +std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) +{ + std::stringstream ss; + for (auto i = 0; i < 16; ++i) { + ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i]; + } + return ss.str(); +} +} // namespace + namespace rtc_interface { RTCInterface::RTCInterface(rclcpp::Node & node, const std::string & name, const Module & module) -: clock_{*node.get_clock()}, module_(module) +: clock_{*node.get_clock()}, + logger_{node.get_logger().get_child("RTCInterface[" + name + "]")}, + module_{module} { using std::placeholders::_1; using std::placeholders::_2; // Publisher - pub_status_ = node.create_publisher(name + "/status", 1); + pub_statuses_ = node.create_publisher("~/" + name + "/cooperate_status", 1); // Service - srv_command_ = node.create_service( - name + "/command", std::bind(&RTCInterface::onCooperateCommandService, this, _1, _2)); + srv_commands_ = node.create_service( + "~/" + name + "/cooperate_commands", + std::bind(&RTCInterface::onCooperateCommandService, this, _1, _2)); } void RTCInterface::publishCooperateStatus() { registered_status_.stamp = clock_.now(); - pub_status_->publish(registered_status_); + pub_statuses_->publish(registered_status_); } void RTCInterface::onCooperateCommandService( - const CooperateCommand::Request::SharedPtr request, - const CooperateCommand::Response::SharedPtr response) + const CooperateCommands::Request::SharedPtr request, + const CooperateCommands::Response::SharedPtr responses) { - const auto itr = std::find_if( - registered_status_.statuses.begin(), registered_status_.statuses.end(), - [request](auto & s) { return s.uuid == request->uuid; }); - - // Update command if the command has been already received - if (itr != registered_status_.statuses.end()) { - itr->command_status = request->command; - response->success = true; - return; + for (const auto & command : request->commands) { + CooperateResponse response; + response.uuid = command.uuid; + response.module = command.module; + + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [command](auto & s) { return s.uuid == command.uuid; }); + + // Update command if the command has been already received + if (itr != registered_status_.statuses.end()) { + itr->command_status = command.command; + response.success = true; + } else { + RCLCPP_WARN_STREAM( + getLogger(), "[onCooperateCommandService] uuid : " << to_string(command.uuid) + << " is not found." << std::endl); + response.success = false; + } + responses->responses.push_back(response); } - - // Status with same uuid is not registered - response->success = false; - - return; } void RTCInterface::updateCooperateStatus(const UUID & uuid, const bool safe, const double distance) @@ -64,7 +85,7 @@ void RTCInterface::updateCooperateStatus(const UUID & uuid, const bool safe, con registered_status_.statuses.begin(), registered_status_.statuses.end(), [uuid](auto & s) { return s.uuid == uuid; }); - // If there is no registered status, add it. + // If there is no registered status, add it if (itr == registered_status_.statuses.end()) { CooperateStatus status; status.stamp = clock_.now(); @@ -95,7 +116,9 @@ void RTCInterface::removeCooperateStatus(const UUID & uuid) return; } - std::cout << "Something wrong" << std::endl; + RCLCPP_WARN_STREAM( + getLogger(), + "[removeCooperateStatus] uuid : " << to_string(uuid) << " is not found." << std::endl); } bool RTCInterface::isActivated(const UUID & uuid) const @@ -108,8 +131,11 @@ bool RTCInterface::isActivated(const UUID & uuid) const return itr->command_status.type == Command::ACTIVATE; } - std::cout << "Something wrong" << std::endl; + RCLCPP_WARN_STREAM( + getLogger(), "[isActivated] uuid : " << to_string(uuid) << " is not found." << std::endl); return false; } +rclcpp::Logger RTCInterface::getLogger() const { return logger_; } + } // namespace rtc_interface