diff --git a/control/external_cmd_selector/CMakeLists.txt b/control/external_cmd_selector/CMakeLists.txt new file mode 100644 index 0000000000000..c195671489fcc --- /dev/null +++ b/control/external_cmd_selector/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 3.5) +project(external_cmd_selector) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + 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 -Werror) +endif() + +### Dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_library(external_cmd_selector_node SHARED + src/external_cmd_selector/external_cmd_selector_node.cpp +) + +rclcpp_components_register_node(external_cmd_selector_node + PLUGIN "ExternalCmdSelector" + EXECUTABLE external_cmd_selector +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp b/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp new file mode 100644 index 0000000000000..edfaa16e0956a --- /dev/null +++ b/control/external_cmd_selector/include/external_cmd_selector/external_cmd_selector_node.hpp @@ -0,0 +1,105 @@ +// Copyright 2020 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 EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ +#define EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +class ExternalCmdSelector : public rclcpp::Node +{ +public: + explicit ExternalCmdSelector(const rclcpp::NodeOptions & node_options); + +private: + using CommandSourceSelect = autoware_control_msgs::srv::ExternalCommandSelect; + using CommandSourceMode = autoware_control_msgs::msg::ExternalCommandSelectorMode; + using InternalGearShift = autoware_auto_vehicle_msgs::msg::GearCommand; + using InternalTurnSignal = autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; + using InternalHazardSignal = autoware_auto_vehicle_msgs::msg::HazardLightsCommand; + using InternalHeartbeat = autoware_external_api_msgs::msg::Heartbeat; + using ExternalControlCommand = autoware_external_api_msgs::msg::ControlCommandStamped; + using ExternalGearShift = autoware_external_api_msgs::msg::GearShiftStamped; + using ExternalTurnSignal = autoware_external_api_msgs::msg::TurnSignalStamped; + using ExternalHeartbeat = autoware_external_api_msgs::msg::Heartbeat; + + // CallbackGroups + rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_; + rclcpp::CallbackGroup::SharedPtr callback_group_services_; + + // Publisher + rclcpp::Publisher::SharedPtr pub_current_selector_mode_; + rclcpp::Publisher::SharedPtr pub_control_cmd_; + rclcpp::Publisher::SharedPtr pub_shift_cmd_; + rclcpp::Publisher::SharedPtr pub_turn_signal_cmd_; + rclcpp::Publisher::SharedPtr pub_hazard_signal_cmd_; + rclcpp::Publisher::SharedPtr pub_heartbeat_; + + // Subscriber + rclcpp::Subscription::SharedPtr sub_local_control_cmd_; + rclcpp::Subscription::SharedPtr sub_local_shift_cmd_; + rclcpp::Subscription::SharedPtr sub_local_turn_signal_cmd_; + rclcpp::Subscription::SharedPtr sub_local_heartbeat_; + + rclcpp::Subscription::SharedPtr sub_remote_control_cmd_; + rclcpp::Subscription::SharedPtr sub_remote_shift_cmd_; + rclcpp::Subscription::SharedPtr sub_remote_turn_signal_cmd_; + rclcpp::Subscription::SharedPtr sub_remote_heartbeat_; + + void onLocalControlCmd(const ExternalControlCommand::ConstSharedPtr msg); + void onLocalShiftCmd(const ExternalGearShift::ConstSharedPtr msg); + void onLocalTurnSignalCmd(const ExternalTurnSignal::ConstSharedPtr msg); + void onLocalHeartbeat(const ExternalHeartbeat::ConstSharedPtr msg); + + void onRemoteControlCmd(const ExternalControlCommand::ConstSharedPtr msg); + void onRemoteShiftCmd(const ExternalGearShift::ConstSharedPtr msg); + void onRemoteTurnSignalCmd(const ExternalTurnSignal::ConstSharedPtr msg); + void onRemoteHeartbeat(const ExternalHeartbeat::ConstSharedPtr msg); + + // Service + rclcpp::Service::SharedPtr srv_select_external_command_; + CommandSourceMode current_selector_mode_; + + bool onSelectExternalCommandService( + const CommandSourceSelect::Request::SharedPtr req, + const CommandSourceSelect::Response::SharedPtr res); + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + void onTimer(); + + // Converter + static InternalGearShift convert(const ExternalGearShift & command); + static InternalHeartbeat convert(const ExternalHeartbeat & command); + + // Diagnostics Updater + diagnostic_updater::Updater updater_{this}; +}; + +#endif // EXTERNAL_CMD_SELECTOR__EXTERNAL_CMD_SELECTOR_NODE_HPP_ diff --git a/control/external_cmd_selector/launch/external_cmd_selector.launch.py b/control/external_cmd_selector/launch/external_cmd_selector.launch.py new file mode 100644 index 0000000000000..5e67627207ad3 --- /dev/null +++ b/control/external_cmd_selector/launch/external_cmd_selector.launch.py @@ -0,0 +1,123 @@ +# Copyright 2021 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def _create_mapping_tuple(name): + return ("~/" + name, LaunchConfiguration(name)) + + +def generate_launch_description(): + + arguments = [ + # component + DeclareLaunchArgument("use_intra_process"), + DeclareLaunchArgument("target_container"), + # settings + DeclareLaunchArgument( + "initial_selector_mode", default_value="local", choices=["local", "remote"] + ), + # service + DeclareLaunchArgument( + "service/select_external_command", default_value="~/select_external_command" + ), + # local input + DeclareLaunchArgument( + "input/local/control_cmd", default_value="/api/external/set/command/local/control" + ), + DeclareLaunchArgument( + "input/local/shift_cmd", default_value="/api/external/set/command/local/shift" + ), + DeclareLaunchArgument( + "input/local/turn_signal_cmd", + default_value="/api/external/set/command/local/turn_signal", + ), + DeclareLaunchArgument( + "input/local/heartbeat", default_value="/api/external/set/command/local/heartbeat" + ), + # remote input + DeclareLaunchArgument( + "input/remote/control_cmd", default_value="/api/external/set/command/remote/control" + ), + DeclareLaunchArgument( + "input/remote/shift_cmd", default_value="/api/external/set/command/remote/shift" + ), + DeclareLaunchArgument( + "input/remote/turn_signal_cmd", + default_value="/api/external/set/command/remote/turn_signal", + ), + DeclareLaunchArgument( + "input/remote/heartbeat", default_value="/api/external/set/command/remote/heartbeat" + ), + # output + DeclareLaunchArgument( + "output/control_cmd", default_value="/external/selected/external_control_cmd" + ), + DeclareLaunchArgument("output/gear_cmd", default_value="/external/selected/gear_cmd"), + DeclareLaunchArgument( + "output/turn_indicators_cmd", default_value="/external/selected/turn_indicators_cmd" + ), + DeclareLaunchArgument( + "output/hazard_lights_cmd", default_value="/external/selected/hazard_lights_cmd" + ), + DeclareLaunchArgument("output/heartbeat", default_value="/external/selected/heartbeat"), + DeclareLaunchArgument( + "output/current_selector_mode", default_value="~/current_selector_mode" + ), + ] + + component = ComposableNode( + package="external_cmd_selector", + plugin="ExternalCmdSelector", + name="external_cmd_selector", + remappings=[ + _create_mapping_tuple("service/select_external_command"), + _create_mapping_tuple("input/local/control_cmd"), + _create_mapping_tuple("input/local/shift_cmd"), + _create_mapping_tuple("input/local/turn_signal_cmd"), + _create_mapping_tuple("input/local/heartbeat"), + _create_mapping_tuple("input/remote/control_cmd"), + _create_mapping_tuple("input/remote/shift_cmd"), + _create_mapping_tuple("input/remote/turn_signal_cmd"), + _create_mapping_tuple("input/remote/heartbeat"), + _create_mapping_tuple("output/control_cmd"), + _create_mapping_tuple("output/gear_cmd"), + _create_mapping_tuple("output/turn_indicators_cmd"), + _create_mapping_tuple("output/hazard_lights_cmd"), + _create_mapping_tuple("output/heartbeat"), + _create_mapping_tuple("output/current_selector_mode"), + ], + parameters=[ + { + "initial_selector_mode": LaunchConfiguration("initial_selector_mode"), + } + ], + extra_arguments=[ + { + "use_intra_process_comms": LaunchConfiguration("use_intra_process"), + } + ], + ) + + loader = LoadComposableNodes( + composable_node_descriptions=[component], + target_container=LaunchConfiguration("target_container"), + ) + + return LaunchDescription(arguments + [loader]) diff --git a/control/external_cmd_selector/launch/external_cmd_selector.launch.xml b/control/external_cmd_selector/launch/external_cmd_selector.launch.xml new file mode 100644 index 0000000000000..766dc8645d89c --- /dev/null +++ b/control/external_cmd_selector/launch/external_cmd_selector.launch.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/external_cmd_selector/package.xml b/control/external_cmd_selector/package.xml new file mode 100644 index 0000000000000..047de6105f7a6 --- /dev/null +++ b/control/external_cmd_selector/package.xml @@ -0,0 +1,28 @@ + + + external_cmd_selector + 0.1.0 + The external_cmd_selector package + Kenji Miyake + Kenji Miyake + Apache License 2.0 + + ament_cmake_auto + + autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_external_api_msgs + autoware_iv_auto_msgs_converter + diagnostic_updater + geometry_msgs + rclcpp + rclcpp_components + std_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp new file mode 100644 index 0000000000000..0c4168ff245c2 --- /dev/null +++ b/control/external_cmd_selector/src/external_cmd_selector/external_cmd_selector_node.cpp @@ -0,0 +1,205 @@ +// Copyright 2020 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 "external_cmd_selector/external_cmd_selector_node.hpp" + +#include + +#include +#include +#include +#include + +ExternalCmdSelector::ExternalCmdSelector(const rclcpp::NodeOptions & node_options) +: Node("external_cmd_selector", node_options) +{ + using std::placeholders::_1; + using std::placeholders::_2; + + // Parameter + double update_rate = declare_parameter("update_rate", 10.0); + std::string initial_selector_mode = declare_parameter("initial_selector_mode", "local"); + + // Publisher + pub_current_selector_mode_ = + create_publisher("~/output/current_selector_mode", 1); + pub_control_cmd_ = create_publisher("~/output/control_cmd", 1); + pub_shift_cmd_ = create_publisher("~/output/gear_cmd", 1); + pub_turn_signal_cmd_ = create_publisher("~/output/turn_indicators_cmd", 1); + pub_hazard_signal_cmd_ = create_publisher("~/output/hazard_lights_cmd", 1); + pub_heartbeat_ = create_publisher("~/output/heartbeat", 1); + + // Callback Groups + callback_group_subscribers_ = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_services_ = + this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + auto subscriber_option = rclcpp::SubscriptionOptions(); + subscriber_option.callback_group = callback_group_subscribers_; + + // Subscriber + sub_local_control_cmd_ = create_subscription( + "~/input/local/control_cmd", 1, std::bind(&ExternalCmdSelector::onLocalControlCmd, this, _1), + subscriber_option); + sub_local_shift_cmd_ = create_subscription( + "~/input/local/shift_cmd", 1, std::bind(&ExternalCmdSelector::onLocalShiftCmd, this, _1), + subscriber_option); + sub_local_turn_signal_cmd_ = create_subscription( + "~/input/local/turn_signal_cmd", 1, + std::bind(&ExternalCmdSelector::onLocalTurnSignalCmd, this, _1), subscriber_option); + sub_local_heartbeat_ = create_subscription( + "~/input/local/heartbeat", 1, std::bind(&ExternalCmdSelector::onLocalHeartbeat, this, _1), + subscriber_option); + + sub_remote_control_cmd_ = create_subscription( + "~/input/remote/control_cmd", 1, std::bind(&ExternalCmdSelector::onRemoteControlCmd, this, _1), + subscriber_option); + sub_remote_shift_cmd_ = create_subscription( + "~/input/remote/shift_cmd", 1, std::bind(&ExternalCmdSelector::onRemoteShiftCmd, this, _1), + subscriber_option); + sub_remote_turn_signal_cmd_ = create_subscription( + "~/input/remote/turn_signal_cmd", 1, + std::bind(&ExternalCmdSelector::onRemoteTurnSignalCmd, this, _1), subscriber_option); + sub_remote_heartbeat_ = create_subscription( + "~/input/remote/heartbeat", 1, std::bind(&ExternalCmdSelector::onRemoteHeartbeat, this, _1), + subscriber_option); + + // Service + srv_select_external_command_ = create_service( + "~/service/select_external_command", + std::bind(&ExternalCmdSelector::onSelectExternalCommandService, this, _1, _2), + rmw_qos_profile_services_default, callback_group_services_); + + // Initialize mode + auto convert_selector_mode = [](const std::string & mode_text) { + if (mode_text == "local") { + return CommandSourceMode::LOCAL; + } + if (mode_text == "remote") { + return CommandSourceMode::REMOTE; + } + throw std::invalid_argument("unknown selector mode"); + }; + current_selector_mode_.data = convert_selector_mode(initial_selector_mode); + + // Diagnostics Updater + updater_.setHardwareID("external_cmd_selector"); + updater_.add("heartbeat", [](auto & stat) { + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Alive"); + }); + + // Timer + auto timer_callback = std::bind(&ExternalCmdSelector::onTimer, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(1.0 / update_rate)); + timer_ = std::make_shared>( + get_clock(), period, std::move(timer_callback), get_node_base_interface()->get_context()); + get_node_timers_interface()->add_timer(timer_, callback_group_subscribers_); +} + +void ExternalCmdSelector::onLocalControlCmd(const ExternalControlCommand::ConstSharedPtr msg) +{ + if (current_selector_mode_.data != CommandSourceMode::LOCAL) { + return; + } + pub_control_cmd_->publish(*msg); +} + +void ExternalCmdSelector::onLocalShiftCmd(const ExternalGearShift::ConstSharedPtr msg) +{ + if (current_selector_mode_.data != CommandSourceMode::LOCAL) { + return; + } + pub_shift_cmd_->publish(convert(*msg)); +} + +void ExternalCmdSelector::onLocalTurnSignalCmd(const ExternalTurnSignal::ConstSharedPtr msg) +{ + if (current_selector_mode_.data != CommandSourceMode::LOCAL) { + return; + } + auto light_signal = autoware_iv_auto_msgs_converter::convert(*msg); + pub_turn_signal_cmd_->publish(light_signal.turn_signal); + pub_hazard_signal_cmd_->publish(light_signal.hazard_signal); +} + +void ExternalCmdSelector::onLocalHeartbeat(const ExternalHeartbeat::ConstSharedPtr msg) +{ + if (current_selector_mode_.data != CommandSourceMode::LOCAL) { + return; + } + pub_heartbeat_->publish(convert(*msg)); +} + +void ExternalCmdSelector::onRemoteControlCmd(const ExternalControlCommand::ConstSharedPtr msg) +{ + if (current_selector_mode_.data != CommandSourceMode::REMOTE) { + return; + } + pub_control_cmd_->publish(*msg); +} + +void ExternalCmdSelector::onRemoteShiftCmd(const ExternalGearShift::ConstSharedPtr msg) +{ + if (current_selector_mode_.data != CommandSourceMode::REMOTE) { + return; + } + pub_shift_cmd_->publish(convert(*msg)); +} + +void ExternalCmdSelector::onRemoteTurnSignalCmd(const ExternalTurnSignal::ConstSharedPtr msg) +{ + if (current_selector_mode_.data != CommandSourceMode::REMOTE) { + return; + } + auto light_signal = autoware_iv_auto_msgs_converter::convert(*msg); + pub_turn_signal_cmd_->publish(light_signal.turn_signal); + pub_hazard_signal_cmd_->publish(light_signal.hazard_signal); +} + +void ExternalCmdSelector::onRemoteHeartbeat(const ExternalHeartbeat::ConstSharedPtr msg) +{ + if (current_selector_mode_.data != CommandSourceMode::REMOTE) { + return; + } + pub_heartbeat_->publish(convert(*msg)); +} + +bool ExternalCmdSelector::onSelectExternalCommandService( + const CommandSourceSelect::Request::SharedPtr req, + const CommandSourceSelect::Response::SharedPtr res) +{ + current_selector_mode_.data = req->mode.data; + res->success = true; + res->message = "Success."; + return true; +} + +void ExternalCmdSelector::onTimer() { pub_current_selector_mode_->publish(current_selector_mode_); } + +ExternalCmdSelector::InternalGearShift ExternalCmdSelector::convert( + const ExternalGearShift & command) +{ + return autoware_iv_auto_msgs_converter::convert(command); +} + +ExternalCmdSelector::InternalHeartbeat ExternalCmdSelector::convert( + const ExternalHeartbeat & command) +{ + return command; +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ExternalCmdSelector)