diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..2d4331d --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,42 @@ +# Copyright 2024 eve autonomy inc. All Rights Reserved. +# +# 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. + +cmake_minimum_required(VERSION 3.5) +project(status_display_manager) + +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(status_display_manager + src/status_display_manager.cpp + src/status_display_manager_node.cpp +) + +target_link_libraries(status_display_manager ${PCL_LIBRARIES}) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..75b5248 --- /dev/null +++ b/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/README.md b/README.md index a4d7cd0..32fcc38 100644 --- a/README.md +++ b/README.md @@ -1 +1,61 @@ -# status_display_manager \ No newline at end of file +# Status display manager + +## Overview +This node controls the display patterns of an electronic display board through `/dio_ros_driver` in order to alert pedestrians and vehicle drivers. + +The display patterns of an electronic display board are defined as follows. + +| Display patterns | Description | +| :---------------------------- | :------------------------------------------------------------------------------------------------------------------------------- | +| Stop | The autonomous vehicle notifies the surrounding pedestrians and vehicle drivers that it is stopped in driving mode. | +| Running | ⁠The autonomous vehicle notifies the surrounding pedestrians and vehicle drivers that it is in autonomous driving. | +| Obstacle detection | The autonomous vehicle notifies the surrounding pedestrians and vehicle drivers that it has stopped due to obstacle detection. | +| Turn right | The autonomous vehicle notifies the surrounding pedestrians and vehicle drivers that it is turning right. | +| Turn left | The autonomous vehicle notifies the surrounding pedestrians and vehicle drivers that it is turning left. | +| Emergency occurred | The autonomous vehicle notifies the surrounding pedestrians and vehicle drivers that a system error has occurred. | +| Pressed emergency stop switch | The autonomous vehicle notifies the surrounding pedestrians and vehicle drivers that the emergency stop switch has been pressed. | +| Hidden | The autonomous vehicle notifies the surrounding pedestrians and vehicle drivers that it is in manual mode. | + +## Display patterns + +The combinations corresponding to the electronic display board status are as follows, and the corresponding DO port numbers can be found in the config file of `/dio_ros_driver`. + +| Display patterns | value0 | value1 | value2 | +| :---------------------------- | :----: | :----: | :----: | +| Stop | 0 | 0 | 0 | +| Running | 1 | 0 | 0 | +| Obstacle detection | 0 | 1 | 0 | +| Turn right | 0 | 0 | 1 | +| Turn left | 1 | 0 | 1 | +| Emergency occurred | 0 | 1 | 1 | +| Pressed emergency stop switch | 1 | 1 | 0 | +| Hidden | 1 | 1 | 1 | + +## Input and Output +- input + - `/autoware_state_machine/state` \[[autoware_state_machine_msgs/msg/StateMachine](https://github.com/eve-autonomy/autoware_state_machine_msgs/blob/main/msg/StateMachine.msg)\]:
State of the system. + - `/diagnostics_err` \[[diagnostic_msgs/msg/DiagnosticArray](https://github.com/ros/common_msgs/blob/noetic-devel/diagnostic_msgs/msg/DiagnosticArray.msg)\]:
This message is used to send diagnostic information about the state of the robot. +
"status" parameter is an array of "diagnostic_msgs/msg/DiagnosticStatus", and among the elements, those whose "status.name" is "/autoware/vehicle/obstacle_crash" are the pressed state of the emergency stop switch.
If "status.level" is ERROR, it is pressed. + - `/vehicle/status/turn_indicators_status` \[[autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport](https://github.com/tier4/autoware_auto_msgs/blob/tier4/main/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl)\]:
State of turn signal. +- output + - `/dio/dout_array` \[[dio_ros_driver/msg/DIOArray](https://github.com/kodamays/dio_ros_driver/blob/feat/dio_port_array_setting/msg/DIOArray.msg)\]:
GPIO output topic for the display patterns of electronic display board. + + +## Node graph +![node_graph](docs/node_graph.png) + +## Sequence diagram +![sequence_diagram](docs/sequence_diagram.png) + +## Lanch arguments + +| Name | Description | +| :------------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------ | +| status_display_manager_config_path | Specify the config file path for `/dio_ros_driver` that designates the dout port number used for switching the display of the electronic display board. | + +## Paramater description + +| Name | Description | +| :----------------- |:--------------------------------------------------------------------------------------------- | +| dout_ports_array | Specify the dout port numbers used for switching the display of the electronic display board.
Prohibited if no element is specified in [].| +| sleep_time_at_running_state | Specify Anti-flicker Sleep timer value for display while running.
Default:500ms | \ No newline at end of file diff --git a/config/status_display_manager.params.yaml b/config/status_display_manager.params.yaml new file mode 100644 index 0000000..4d53dcf --- /dev/null +++ b/config/status_display_manager.params.yaml @@ -0,0 +1,5 @@ +# all config data are described here. +status_display_manager: + ros__parameters: + dout_ports_array: [5, 6, 7] + sleep_time_at_running_state: 500 diff --git a/docs/node_graph.png b/docs/node_graph.png new file mode 100644 index 0000000..507013a Binary files /dev/null and b/docs/node_graph.png differ diff --git a/docs/node_graph.pu b/docs/node_graph.pu new file mode 100644 index 0000000..a700783 --- /dev/null +++ b/docs/node_graph.pu @@ -0,0 +1,14 @@ +@startuml + +usecase "/autoware_state_machine" +usecase "/status_display_manager" #LightCoral +usecase "/dio_ros_driver" +usecase "/system_error_monitor" +usecase "/g30esli_interface_awiv_adapt_receiver" + +(/autoware_state_machine) -> (/status_display_manager) : /autoware_state_machine/state +(/system_error_monitor) --> (/status_display_manager) : /diagnostics_err +(/g30esli_interface_awiv_adapt_receiver) --> (/status_display_manager) : /vehicle/status/turn_indicators_status + +(/status_display_manager) -> (/dio_ros_driver) : /dio/dout_array +@enduml diff --git a/docs/sequence.pu b/docs/sequence.pu new file mode 100644 index 0000000..da6832e --- /dev/null +++ b/docs/sequence.pu @@ -0,0 +1,96 @@ +@startuml + +Participant autoware_state_machine as ASM +Participant system_error_monitor as SEM +Participant interface_awiv_adapt_receiver as IAAR +Participant status_display_manager as SDM +Participant dio_ros_driver as DRD + +== init (launch) == + + -> SDM : node launch +activate SDM + + SDM -> SDM: ctor + activate SDM + SDM -> SDM: declare_parameter() + SDM -> SDM: create_subscription() + SDM -> SDM: create_publisher() + +== running == +activate ASM +activate SEM +activate IAAR +activate DRD + deactivate SDM + + ASM -> SDM: Topic:/autoware_state_machine/state + SDM -> SDM: callbackStateMessage() + activate SDM + SDM -> SDM: RCLCPP_INFO_THROTTLE() + deactivate SDM + + SEM -> SDM: Topic:/diagnotics_err + SDM -> SDM: callbackDiagStateMessage() + activate SDM + SDM -> SDM: RCLCPP_INFO_THROTTLE() + deactivate SDM + + IAAR -> SDM: Topic:/vehicle/status/turn_inidicators_status + SDM -> SDM: callbackVehicleTurnMessage() + activate SDM + SDM -> SDM: RCLCPP_INFO_THROTTLE() + deactivate SDM + + SDM -> SDM: statusDisplayManager() + +alt service_layer_state == MANUAL + SDM -> SDM: status_display_state = HIDDEN(7) + +else service_layer_state == AUTO + alt status_display_state == PRESS_EMERGENCY_STOP_SWITCH(6) or ERROR_DETECTION(5) + SDM -> SDM: Doing nothing + else + alt server_layer_state == STATE_DURING_RECEIVE_ROUTE or STATE_ARRIVED_GOAL or STATE_WAITING_ENGAGE_INSTRUCTION or STATE_WAITING_CALL_PERMISSION + SDM -> SDM: status_display_state = STOP(0) + else server_layer_state == STATE_INFORM_ENGAGE or STATE_INFORM_RESTART or STATE_RUNNING + SDM -> SDM: status_display_state = RUNNING(1) + alt vehicle_turn_status == ENABLE_RIGHT + SDM -> SDM: status_display_state = TURN_RIGHT(3) + else vehicle_turn_status == ENABLE_LEFT + SDM -> SDM: status_display_state = TURN_LEFT(4) + end + alt status_display_state == RUNNING(1) and emergency_switch_status == true + SDM -> SDM: status_display_state = PRESS_EMERGENCY_STOP_SWITCH(6) + end + else server_layer_state == STATE_TURNING_RIGHT + SDM -> SDM: status_display_state = TURN_RIGHT(3) + else server_layer_state == STATE_TURNING_LEFT + SDM -> SDM: status_display_state = TURN_LEFT(4) + else server_layer_state == STATE_STOP_DUETO_APPROACHING_OBSTACLE + SDM -> SDM: status_display_state = OBSTACLE_DETECTION(2) + else server_layer_state == STATE_EMERGENCY_STOP + alt emergency_switch_status == true + SDM -> SDM: status_display_state = PRESS_EMERGENCY_STOP_SWITCH(6) + else emergency_switch_status == false + SDM -> SDM: status_display_state = ERROR_DETECTION(5) + end + else server_layer_state == STATE_RUNNING_TOWARD_STOP_LINE or STATE_RUNNING_TOWARD_OBSTACLE + SDM -> SDM: status_display_state = RUNNING(1) + alt vehicle_turn_status == ENABLE_RIGHT + SDM -> SDM: status_display_state = TURN_RIGHT(3) + else vehicle_turn_status == ENABLE_LEFT + SDM -> SDM: status_display_state = TURN_LEFT(4) + end + end + end +end + +alt before_status_display_state != status_display_state + activate SDM + SDM -> DRD: Topic:/dio/dout_array +end + +== finish == + +@enduml \ No newline at end of file diff --git a/docs/sequence_diagram.png b/docs/sequence_diagram.png new file mode 100644 index 0000000..8dadc73 Binary files /dev/null and b/docs/sequence_diagram.png differ diff --git a/include/status_display_manager/status_display_manager.hpp b/include/status_display_manager/status_display_manager.hpp new file mode 100644 index 0000000..8ffcd81 --- /dev/null +++ b/include/status_display_manager/status_display_manager.hpp @@ -0,0 +1,98 @@ +// Copyright 2024 eve autonomy inc. All Rights Reserved. +// +// 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 STATUS_DISPLAY_MANAGER__STATUS_DISPLAY_MANAGER_HPP_ +#define STATUS_DISPLAY_MANAGER__STATUS_DISPLAY_MANAGER_HPP_ + +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/callback_group.hpp" +#include "rclcpp/subscription_options.hpp" +#include "autoware_state_machine_msgs/msg/state_machine.hpp" +#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp" +#include "dio_ros_driver/msg/dio_port.hpp" +#include "dio_ros_driver/msg/dio_array.hpp" +#include "dio_ros_driver/msg/dio_port_value.hpp" +#include "diagnostic_msgs/msg/diagnostic_array.hpp" + +namespace status_display_manager { + +class StatusDisplayManager : public rclcpp::Node { +public: + explicit StatusDisplayManager(const rclcpp::NodeOptions & options); + ~StatusDisplayManager(); + +private: + #define DISPLAY_DOUT_PORTS_NUM (3) + enum class DisplayStatus : int8_t { + STOP = 0, + RUNNING, + OBSTACLE_DETECTION, + TURN_RIGHT, + TURN_LEFT, + ERROR_DETECTION, + PRESS_EMERGENCY_STOP_SWITCH, + HIDDEN, + DISPLAY_STATUS_NUMS + }; + + struct display_dout_values { + bool values_[DISPLAY_DOUT_PORTS_NUM]; + } const display_dout_values_[(uint8_t)DisplayStatus::DISPLAY_STATUS_NUMS]; + + // Callback group + rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_; + rclcpp::CallbackGroup::SharedPtr callback_group_timer_; + + // Publisher + rclcpp::Publisher::SharedPtr pub_dout_array_; + + // Subscriber + rclcpp::Subscription::SharedPtr sub_state_; + rclcpp::Subscription::SharedPtr sub_dio_state_; + rclcpp::Subscription::SharedPtr + sub_turn_state_; + + // Timer callback + rclcpp::TimerBase::SharedPtr status_display_update_timer_; + + std::queue queue_autoware_state_; + autoware_state_machine_msgs::msg::StateMachine::ConstSharedPtr before_autoware_state_; + uint32_t sleep_time_at_running_state_; + bool emergency_switch_status_; + DisplayStatus before_status_display_state_; + DisplayStatus status_display_state_; + std::vector display_dout_ports_; + std::uint8_t vehicle_turn_status_; + std::mutex autoware_state_mutex_; + std::mutex indicators_mutex_; + std::mutex emergency_switch_mutex_; + + void callbackStateMessage( + const autoware_state_machine_msgs::msg::StateMachine::ConstSharedPtr &msg); + void callbackDiagStateMessage( + const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr &msg); + void ApplyTurnIndicatorsReport(); + void ApplyEmergencyStopStatus(); + void callbackVehicleTurnMessage( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport &msg); + void controlStatusDisplay(builtin_interfaces::msg::Time time_stamp); + void statusDisplayManager( + autoware_state_machine_msgs::msg::StateMachine autoware_state); + void update(); +}; + +} // namespace status_display_manager +#endif // STATUS_DISPLAY_MANAGER__STATUS_DISPLAY_MANAGER_HPP_ diff --git a/launch/status_display_manager.launch.xml b/launch/status_display_manager.launch.xml new file mode 100644 index 0000000..e6c4334 --- /dev/null +++ b/launch/status_display_manager.launch.xml @@ -0,0 +1,27 @@ + + + + + + + + + + + + + diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..634f24a --- /dev/null +++ b/package.xml @@ -0,0 +1,41 @@ + + + + + + + status_display_manager + 1.0.0 + The status_display_manager package + Yasufumi Takahashi + Apache License 2.0 + + ament_cmake + + rclcpp + dio_ros_driver + autoware_state_machine_msgs + autoware_auto_vehicle_msgs + diagnostic_msgs + rclcpp_components + + builtin_interfaces + + + ament_cmake + + diff --git a/src/status_display_manager.cpp b/src/status_display_manager.cpp new file mode 100644 index 0000000..a53c8c8 --- /dev/null +++ b/src/status_display_manager.cpp @@ -0,0 +1,283 @@ +// Copyright 2024 eve autonomy inc. All Rights Reserved. +// +// 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 +#include "status_display_manager/status_display_manager.hpp" + +namespace status_display_manager { + +StatusDisplayManager::StatusDisplayManager( + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) +: Node("status_display_manager", options), + display_dout_values_{ + // Listed in the order of DisplayStatus + {false, false, false}, // DisplayStatus::STOP + {true, false, false}, // DisplayStatus::RUNNING + {false, true, false}, // DisplayStatus::OBSTACLE_DETECTION + {false, false, true }, // DisplayStatus::TURN_RIGHT + {true, false, true }, // DisplayStatus::TURN_LEFT + {false, true, true }, // DisplayStatus::ERROR_DETECTION + {true, true, false}, // DisplayStatus::PRESS_EMERGENCY_STOP_SWITCH + {true, true, true }}, // DisplayStatus::HIDDEN + sleep_time_at_running_state_(declare_parameter("sleep_time_at_running_state", 500)), + display_dout_ports_() +{ + // read parameter + this->declare_parameter("dout_ports_array", std::vector{}); + auto tmp_display_dout_port_list = this->get_parameter("dout_ports_array").as_integer_array(); + + before_autoware_state_ = nullptr; + status_display_state_ = DisplayStatus::HIDDEN; + before_status_display_state_ = status_display_state_; + emergency_switch_status_ = false; + vehicle_turn_status_ = autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::DISABLE; + + if (tmp_display_dout_port_list.size() > 0) { + for (auto tmp_display_dout_port : tmp_display_dout_port_list) { + display_dout_ports_.push_back(static_cast(tmp_display_dout_port)); + } + + callback_group_subscribers_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + auto subscriber_option = rclcpp::SubscriptionOptions(); + subscriber_option.callback_group = callback_group_subscribers_; + + callback_group_timer_ = this->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive); + + sub_state_ = this->create_subscription( + "autoware_state_machine/state", + rclcpp::QoS{3}.transient_local(), + std::bind(&StatusDisplayManager::callbackStateMessage, this, std::placeholders::_1), + subscriber_option); + sub_dio_state_ = this->create_subscription( + "emergency_switch_input", + rclcpp::QoS{1}, + std::bind(&StatusDisplayManager::callbackDiagStateMessage, this, std::placeholders::_1), + subscriber_option); + sub_turn_state_ + = this->create_subscription( + "turn_indicators_status", + rclcpp::QoS{1}, + std::bind(&StatusDisplayManager::callbackVehicleTurnMessage, this, std::placeholders::_1), + subscriber_option); + + pub_dout_array_ = this->create_publisher( + "/dio/dout_array", + rclcpp::QoS(1)); + status_display_update_timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), + std::bind(&StatusDisplayManager::update, this), + callback_group_timer_); + } +} + +StatusDisplayManager::~StatusDisplayManager() +{ + // Do not publish state when finished + status_display_state_ = DisplayStatus::HIDDEN; + before_status_display_state_ = status_display_state_; +} + +void StatusDisplayManager::callbackStateMessage( + const autoware_state_machine_msgs::msg::StateMachine::ConstSharedPtr &msg) +{ + { + std::unique_lock lock(autoware_state_mutex_); + + RCLCPP_INFO_THROTTLE( + this->get_logger(), + *this->get_clock(), 1.0, + "[StatusDisplayManager::callbackStateMessage]" + "service_layer_state: %u, control_layer_state: %u", + msg->service_layer_state, + msg->control_layer_state); + + queue_autoware_state_.push(msg); + } +} + +void StatusDisplayManager::callbackDiagStateMessage( + const diagnostic_msgs::msg::DiagnosticArray::ConstSharedPtr &msg) +{ + for (const auto& status : msg->status) { + if (status.name == "/autoware/vehicle/obstacle_crash") { + RCLCPP_INFO_THROTTLE( + this->get_logger(), + *this->get_clock(), 1.0, + "[StatusDisplayManager::callbackDiagStateMessage]" + "name: %s, level: %u", + status.name.c_str(), + status.level); + { + std::unique_lock lock(emergency_switch_mutex_); + if (status.level == diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + emergency_switch_status_ = true; + } else { + emergency_switch_status_ = false; + } + } + break; + } else { + // Nothing + } + } +} + +void StatusDisplayManager::callbackVehicleTurnMessage( + const autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport &msg) +{ + { + std::unique_lock lock(indicators_mutex_); + RCLCPP_INFO_THROTTLE( + this->get_logger(), + *this->get_clock(), 1.0, + "[StatusDisplayManager::callbackVehicleTurnMessage]" + "vehicle_turn_status: %u", + msg.report); + vehicle_turn_status_ = msg.report; + } +} + +void StatusDisplayManager::controlStatusDisplay(builtin_interfaces::msg::Time time_stamp) +{ + dio_ros_driver::msg::DIOArray dout_msg; + dio_ros_driver::msg::DIOPortValue dio_port_value; + + dout_msg.set__stamp(time_stamp); + for (int i = 0; i < DISPLAY_DOUT_PORTS_NUM; i++) { + dio_port_value.port = display_dout_ports_[i]; + dio_port_value.value = display_dout_values_[(uint8_t)status_display_state_].values_[i]; + dout_msg.values.push_back(dio_port_value); + } + pub_dout_array_->publish(dout_msg); +} + +void StatusDisplayManager::statusDisplayManager( + autoware_state_machine_msgs::msg::StateMachine autoware_state) +{ + if (autoware_state.control_layer_state + == autoware_state_machine_msgs::msg::StateMachine::MANUAL) { + // Manual + status_display_state_ = DisplayStatus::HIDDEN; + } else { + // Auto + if (status_display_state_ == DisplayStatus::PRESS_EMERGENCY_STOP_SWITCH || + status_display_state_ == DisplayStatus::ERROR_DETECTION) { + // Nothing + } else { + switch (autoware_state.service_layer_state) { + case autoware_state_machine_msgs::msg::StateMachine::STATE_DURING_RECEIVE_ROUTE: + case autoware_state_machine_msgs::msg::StateMachine::STATE_ARRIVED_GOAL: + case autoware_state_machine_msgs::msg::StateMachine::STATE_WAITING_ENGAGE_INSTRUCTION: + case autoware_state_machine_msgs::msg::StateMachine::STATE_WAITING_CALL_PERMISSION: + status_display_state_ = DisplayStatus::STOP; + break; + case autoware_state_machine_msgs::msg::StateMachine::STATE_INFORM_ENGAGE: + case autoware_state_machine_msgs::msg::StateMachine::STATE_INFORM_RESTART: + case autoware_state_machine_msgs::msg::StateMachine::STATE_RUNNING: + status_display_state_ = DisplayStatus::RUNNING; + ApplyTurnIndicatorsReport(); + if (status_display_state_ == DisplayStatus::RUNNING) { + rclcpp::sleep_for(std::chrono::milliseconds(sleep_time_at_running_state_)); + ApplyEmergencyStopStatus(); + } + break; + case autoware_state_machine_msgs::msg::StateMachine::STATE_TURNING_RIGHT: + status_display_state_ = DisplayStatus::TURN_RIGHT; + break; + case autoware_state_machine_msgs::msg::StateMachine::STATE_TURNING_LEFT: + status_display_state_ = DisplayStatus::TURN_LEFT; + break; + case autoware_state_machine_msgs::msg::StateMachine::STATE_STOP_DUETO_APPROACHING_OBSTACLE: + status_display_state_ = DisplayStatus::OBSTACLE_DETECTION; + break; + case autoware_state_machine_msgs::msg::StateMachine::STATE_EMERGENCY_STOP: + status_display_state_ = DisplayStatus::ERROR_DETECTION; + ApplyEmergencyStopStatus(); + break; + case autoware_state_machine_msgs::msg::StateMachine::STATE_RUNNING_TOWARD_STOP_LINE: + case autoware_state_machine_msgs::msg::StateMachine::STATE_RUNNING_TOWARD_OBSTACLE: + status_display_state_ = DisplayStatus::RUNNING; + ApplyTurnIndicatorsReport(); + break; + default: + // Nothing + break; + } + } + } + + if (before_status_display_state_ != status_display_state_) { + before_status_display_state_ = status_display_state_; + // publish + controlStatusDisplay(autoware_state.stamp); + } +} + +void StatusDisplayManager::ApplyTurnIndicatorsReport() +{ + { + std::unique_lock lock(indicators_mutex_); + if (vehicle_turn_status_ == + autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_RIGHT) { + status_display_state_ = DisplayStatus::TURN_RIGHT; + } else if (vehicle_turn_status_ == + autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport::ENABLE_LEFT) { + status_display_state_ = DisplayStatus::TURN_LEFT; + } else { + // Nothing + } + } +} + +void StatusDisplayManager::ApplyEmergencyStopStatus() +{ + { + std::unique_lock lock(emergency_switch_mutex_); + if (emergency_switch_status_ == true) { + status_display_state_ = DisplayStatus::PRESS_EMERGENCY_STOP_SWITCH; + } else { + // Nothing + } + } +} + +void StatusDisplayManager::update() +{ + autoware_state_machine_msgs::msg::StateMachine autoware_state; + { + std::unique_lock lock(autoware_state_mutex_); + if (queue_autoware_state_.empty()) { + if (before_autoware_state_ == nullptr) { + return; + } + autoware_state = *before_autoware_state_; + } else { + autoware_state_machine_msgs::msg::StateMachine::ConstSharedPtr front_element = + queue_autoware_state_.front(); + autoware_state = *front_element; + queue_autoware_state_.pop(); + before_autoware_state_ = front_element; + } + } + statusDisplayManager(autoware_state); +} + +} // namespace status_display_manager + +#include "rclcpp_components/register_node_macro.hpp" + + +RCLCPP_COMPONENTS_REGISTER_NODE(status_display_manager::StatusDisplayManager) diff --git a/src/status_display_manager_node.cpp b/src/status_display_manager_node.cpp new file mode 100644 index 0000000..b2ae4b3 --- /dev/null +++ b/src/status_display_manager_node.cpp @@ -0,0 +1,32 @@ +// Copyright 2024 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 + +#include "status_display_manager/status_display_manager.hpp" + +int main(int argc, char * argv[]) +{ + rclcpp::NodeOptions options = rclcpp::NodeOptions(); + rclcpp::ExecutorOptions exe_options = rclcpp::ExecutorOptions(); + + rclcpp::init(argc, argv); + auto node = std::make_shared(options); + auto executor = std::make_shared(exe_options, 2); + executor->add_node(node); + executor->spin(); + rclcpp::shutdown(); + + return 0; +}