From f9155019349918f436d405092e1c3a6156e2a27a Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Fri, 20 May 2022 16:02:23 +0900 Subject: [PATCH] feat: add RTC interface (#765) * feature(rtc_interface): add files Signed-off-by: Fumiya Watanabe * feature(rtc_interface): implement functions Signed-off-by: Fumiya Watanabe * feature(rtc_interface): reimprement functions to use CooperateCommands and write README.md Signed-off-by: Fumiya Watanabe * feature(rtc_interface): fix README Signed-off-by: Fumiya Watanabe * feature(rtc_interface): add getModuleType() Signed-off-by: Fumiya Watanabe * feature(rtc_interface): fix definition of constructor Signed-off-by: Fumiya Watanabe * feature(rtc_interface): fix time stamp Signed-off-by: Fumiya Watanabe * feature(rtc_interface): fix README Signed-off-by: Fumiya Watanabe * feature(rtc_interface): add isRegistered and clearCooperateStatus Signed-off-by: Fumiya Watanabe * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- planning/rtc_interface/CMakeLists.txt | 25 +++ planning/rtc_interface/README.md | 187 +++++++++++++++++ .../include/rtc_interface/rtc_interface.hpp | 71 +++++++ planning/rtc_interface/package.xml | 23 +++ planning/rtc_interface/src/rtc_interface.cpp | 193 ++++++++++++++++++ 5 files changed, 499 insertions(+) create mode 100644 planning/rtc_interface/CMakeLists.txt create mode 100644 planning/rtc_interface/README.md create mode 100644 planning/rtc_interface/include/rtc_interface/rtc_interface.hpp create mode 100644 planning/rtc_interface/package.xml create mode 100644 planning/rtc_interface/src/rtc_interface.cpp diff --git a/planning/rtc_interface/CMakeLists.txt b/planning/rtc_interface/CMakeLists.txt new file mode 100644 index 0000000000000..fda6025b84e86 --- /dev/null +++ b/planning/rtc_interface/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.5) +project(rtc_interface) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(rtc_interface SHARED + src/rtc_interface.cpp +) + +# Test +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/planning/rtc_interface/README.md b/planning/rtc_interface/README.md new file mode 100644 index 0000000000000..45c1d4e8b9a23 --- /dev/null +++ b/planning/rtc_interface/README.md @@ -0,0 +1,187 @@ +# RTC Interface + +## Purpose + +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"); + +// 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 = ... + + // Get time stamp + const rclcpp::Time stamp = ... + + // Update status + rtc_interface.updateCooperateStatus(uuid, safe, distance, stamp); + + if (rtc_interface.isActivated(uuid)) { + // Execute planning + } else { + // Stop planning + } + // Get time stamp + const rclcpp::Time stamp = ... + + // Publish status topic + rtc_interface.publishCooperateStatus(stamp); +} + +// Remove the status from array +rtc_interface.removeCooperateStatus(uuid); +``` + +## Inputs / Outputs + +### RTCInterface (Constructor) + +```c++ +rtc_interface::RTCInterface(rclcpp::Node & node, const std::string & name); +``` + +#### 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` + +#### Output + +An instance of `RTCInterface` + +### publishCooperateStatus + +```c++ +rtc_interface::publishCooperateStatus(const rclcpp::Time & stamp) +``` + +#### Description + +Publish registered cooperate status. + +#### Input + +- `stamp` : Time stamp + +#### Output + +Nothing + +### updateCooperateStatus + +```c++ +rtc_interface::updateCooperateStatus(const unique_identifier_msgs::msg::UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp) +``` + +#### 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 +- `stamp` : Time stamp + +#### 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 expired module + +#### Output + +Nothing + +### clearCooperateStatus + +```c++ +rtc_interface::clearCooperateStatus() +``` + +#### Description + +Remove all cooperate statuses. + +#### Input + +Nothing + +#### 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`. + +### isRegistered + +```c++ +rtc_interface::isRegistered(const unique_identifier_msgs::msg::UUID & uuid) +``` + +#### Description + +Return `true` if `uuid` is registered. + +#### Input + +- `uuid` : UUID for checking module + +#### Output + +If `uuid` is registered, 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 new file mode 100644 index 0000000000000..df9dc4d1c2430 --- /dev/null +++ b/planning/rtc_interface/include/rtc_interface/rtc_interface.hpp @@ -0,0 +1,71 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RTC_INTERFACE__RTC_INTERFACE_HPP_ +#define RTC_INTERFACE__RTC_INTERFACE_HPP_ + +#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_commands.hpp" +#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::CooperateCommands; +using unique_identifier_msgs::msg::UUID; + +class RTCInterface +{ +public: + RTCInterface(rclcpp::Node & node, const std::string & name); + void publishCooperateStatus(const rclcpp::Time & stamp); + void updateCooperateStatus( + const UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp); + void removeCooperateStatus(const UUID & uuid); + void clearCooperateStatus(); + bool isActivated(const UUID & uuid) const; + bool isRegistered(const UUID & uuid) const; + +private: + void onCooperateCommandService( + const CooperateCommands::Request::SharedPtr request, + const CooperateCommands::Response::SharedPtr responses); + rclcpp::Logger getLogger() const; + + rclcpp::Publisher::SharedPtr pub_statuses_; + rclcpp::Service::SharedPtr srv_commands_; + + rclcpp::Logger logger_; + Module module_; + CooperateStatusArray registered_status_; +}; + +} // namespace rtc_interface + +#endif // RTC_INTERFACE__RTC_INTERFACE_HPP_ diff --git a/planning/rtc_interface/package.xml b/planning/rtc_interface/package.xml new file mode 100644 index 0000000000000..96293f6b48ff3 --- /dev/null +++ b/planning/rtc_interface/package.xml @@ -0,0 +1,23 @@ + + + rtc_interface + 0.1.0 + The rtc_interface package + + Fumiya Watanabe + + Apache License 2.0 + + ament_cmake_auto + + rclcpp + tier4_rtc_msgs + unique_identifier_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/rtc_interface/src/rtc_interface.cpp b/planning/rtc_interface/src/rtc_interface.cpp new file mode 100644 index 0000000000000..c4e0ed0bc374d --- /dev/null +++ b/planning/rtc_interface/src/rtc_interface.cpp @@ -0,0 +1,193 @@ +// Copyright 2022 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "rtc_interface/rtc_interface.hpp" + +namespace +{ +using tier4_rtc_msgs::msg::Module; + +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(); +} + +Module getModuleType(const std::string & module_name) +{ + Module module; + if (module_name == "blind_spot") { + module.type = Module::BLIND_SPOT; + } else if (module_name == "crosswalk") { + module.type = Module::CROSSWALK; + } else if (module_name == "detection_area") { + module.type = Module::DETECTION_AREA; + } else if (module_name == "intersection") { + module.type = Module::INTERSECTION; + } else if (module_name == "no_stopping_area") { + module.type = Module::NO_STOPPING_AREA; + } else if (module_name == "occlusion_spot") { + module.type = Module::OCCLUSION_SPOT; + } else if (module_name == "stop_line") { + module.type = Module::NONE; + } else if (module_name == "traffic_light") { + module.type = Module::TRAFFIC_LIGHT; + } else if (module_name == "virtual_traffic_light") { + module.type = Module::TRAFFIC_LIGHT; + } else if (module_name == "lane_change_left") { + module.type = Module::LANE_CHANGE_LEFT; + } else if (module_name == "lane_change_right") { + module.type = Module::LANE_CHANGE_RIGHT; + } else if (module_name == "avoidance_left") { + module.type = Module::AVOIDANCE_LEFT; + } else if (module_name == "avoidance_right") { + module.type = Module::AVOIDANCE_RIGHT; + } else if (module_name == "pull_over") { + module.type = Module::PULL_OVER; + } else if (module_name == "pull_out") { + module.type = Module::PULL_OUT; + } + return module; +} + +} // namespace + +namespace rtc_interface +{ +RTCInterface::RTCInterface(rclcpp::Node & node, const std::string & name) +: logger_{node.get_logger().get_child("RTCInterface[" + name + "]")} +{ + using std::placeholders::_1; + using std::placeholders::_2; + + // Publisher + pub_statuses_ = node.create_publisher("~/" + name + "/cooperate_status", 1); + + // Service + srv_commands_ = node.create_service( + "~/" + name + "/cooperate_commands", + std::bind(&RTCInterface::onCooperateCommandService, this, _1, _2)); + + // Module + module_ = getModuleType(name); +} + +void RTCInterface::publishCooperateStatus(const rclcpp::Time & stamp) +{ + registered_status_.stamp = stamp; + pub_statuses_->publish(registered_status_); +} + +void RTCInterface::onCooperateCommandService( + const CooperateCommands::Request::SharedPtr request, + const CooperateCommands::Response::SharedPtr responses) +{ + 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); + } +} + +void RTCInterface::updateCooperateStatus( + const UUID & uuid, const bool safe, const double distance, const rclcpp::Time & stamp) +{ + // Find registered status which has same uuid + auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](auto & s) { return s.uuid == uuid; }); + + // If there is no registered status, add it + if (itr == registered_status_.statuses.end()) { + CooperateStatus status; + status.stamp = stamp; + status.uuid = uuid; + status.module = module_; + status.safe = safe; + status.command_status.type = Command::DEACTIVATE; + status.distance = distance; + registered_status_.statuses.push_back(status); + return; + } + + // If the registered status is found, update status + itr->stamp = stamp; + itr->safe = safe; + itr->distance = distance; +} + +void RTCInterface::removeCooperateStatus(const UUID & uuid) +{ + // Find registered status which has same uuid and erase it + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](auto & s) { return s.uuid == uuid; }); + + if (itr != registered_status_.statuses.end()) { + registered_status_.statuses.erase(itr); + return; + } + + RCLCPP_WARN_STREAM( + getLogger(), + "[removeCooperateStatus] uuid : " << to_string(uuid) << " is not found." << std::endl); +} + +void RTCInterface::clearCooperateStatus() { registered_status_.statuses.clear(); } + +bool RTCInterface::isActivated(const UUID & uuid) const +{ + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](auto & s) { return s.uuid == uuid; }); + + if (itr != registered_status_.statuses.end()) { + return itr->command_status.type == Command::ACTIVATE; + } + + RCLCPP_WARN_STREAM( + getLogger(), "[isActivated] uuid : " << to_string(uuid) << " is not found." << std::endl); + return false; +} + +bool RTCInterface::isRegistered(const UUID & uuid) const +{ + const auto itr = std::find_if( + registered_status_.statuses.begin(), registered_status_.statuses.end(), + [uuid](auto & s) { return s.uuid == uuid; }); + return itr != registered_status_.statuses.end(); +} + +rclcpp::Logger RTCInterface::getLogger() const { return logger_; } + +} // namespace rtc_interface