diff --git a/control/autoware_joy_controller/CMakeLists.txt b/control/autoware_joy_controller/CMakeLists.txt new file mode 100644 index 0000000000000..55bd83ac16158 --- /dev/null +++ b/control/autoware_joy_controller/CMakeLists.txt @@ -0,0 +1,32 @@ +cmake_minimum_required(VERSION 3.5) +project(autoware_joy_controller) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +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(autoware_joy_controller_node SHARED + src/autoware_joy_controller/autoware_joy_controller_node.cpp +) + +rclcpp_components_register_node(autoware_joy_controller_node + PLUGIN "autoware_joy_controller::AutowareJoyControllerNode" + EXECUTABLE autoware_joy_controller +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/control/autoware_joy_controller/README.md b/control/autoware_joy_controller/README.md new file mode 100644 index 0000000000000..3ff48c3cc3f82 --- /dev/null +++ b/control/autoware_joy_controller/README.md @@ -0,0 +1,43 @@ +# autoware_joy_controller + +## Role + +`autoware_joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle. + +## Input / Output + +### Input topics + +| Name | Type | Description | +| ------------------ | ----------------------- | --------------------------------- | +| `~/input/joy` | sensor_msgs::msg::Joy | joy controller command | +| `~/input/odometry` | nav_msgs::msg::Odometry | ego vehicle odometry to get twist | + +### Output topics + +| Name | Type | Description | +| ----------------------------------- | -------------------------------------------------------- | ---------------------------------------- | +| `~/output/control_command` | autoware_auto_control_msgs::msg::AckermannControlCommand | lateral and longitudinal control command | +| `~/output/external_control_command` | autoware_external_api_msgs::msg::ControlCommandStamped | lateral and longitudinal control command | +| `~/output/shift` | autoware_external_api_msgs::msg::GearShiftStamped | gear command | +| `~/output/turn_signal` | autoware_external_api_msgs::msg::TurnSignalStamped | turn signal command | +| `~/output/gate_mode` | autoware_control_msgs::msg::GateMode | gate mode (Auto or External) | +| `~/output/heartbeat` | autoware_external_api_msgs::msg::Heartbeat | heartbeat | +| `~/output/vehicle_engage` | autoware_auto_vehicle_msgs::msg::Engage | vehicle engage | + +## Parameters + +| Parameter | Type | Description | +| ------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------ | +| `joy_type` | string | joy controller type (default: DS4) | +| `update_rate` | double | update rate to publish control commands | +| `accel_ratio` | double | ratio to calculate acceleration (commanded acceleration is ratio \* operation) | +| `brake_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | +| `steer_ratio` | double | ratio to calculate deceleration (commanded steer is ratio \* operation) | +| `steering_angle_velocity` | double | steering angle velocity for operation | +| `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | +| `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) | +| `velocity_gain` | double | ratio to calculate velocity by acceleration | +| `max_forward_velocity` | double | absolute max velocity to go forward | +| `max_backward_velocity` | double | absolute max velocity to go backward | +| `backward_accel_ratio` | double | ratio to calculate deceleration (commanded acceleration is -ratio \* operation) | diff --git a/control/autoware_joy_controller/config/autoware_joy_controller.param.yaml b/control/autoware_joy_controller/config/autoware_joy_controller.param.yaml new file mode 100644 index 0000000000000..8796e026b0e69 --- /dev/null +++ b/control/autoware_joy_controller/config/autoware_joy_controller.param.yaml @@ -0,0 +1,14 @@ +/**: + ros__parameters: + update_rate: 10.0 + accel_ratio: 3.0 + brake_ratio: 5.0 + steer_ratio: 0.5 + steering_angle_velocity: 0.1 + accel_sensitivity: 1.0 + brake_sensitivity: 1.0 + control_command: + velocity_gain: 3.0 + max_forward_velocity: 20.0 + max_backward_velocity: 3.0 + backward_accel_ratio: 1.0 diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp new file mode 100644 index 0000000000000..23ee25f4127ff --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/autoware_joy_controller.hpp @@ -0,0 +1,123 @@ +// 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 AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_ + +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace autoware_joy_controller +{ +using GearShiftType = autoware_external_api_msgs::msg::GearShift::_data_type; +using TurnSignalType = autoware_external_api_msgs::msg::TurnSignal::_data_type; +using GateModeType = autoware_control_msgs::msg::GateMode::_data_type; + +class AutowareJoyControllerNode : public rclcpp::Node +{ +public: + explicit AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options); + +private: + // Parameter + std::string joy_type_; + double update_rate_; + double accel_ratio_; + double brake_ratio_; + double steer_ratio_; + double steering_angle_velocity_; + double accel_sensitivity_; + double brake_sensitivity_; + + // ControlCommand Parameter + double velocity_gain_; + double max_forward_velocity_; + double max_backward_velocity_; + double backward_accel_ratio_; + + // CallbackGroups + rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_; + rclcpp::CallbackGroup::SharedPtr callback_group_services_; + + // Subscriber + rclcpp::Subscription::SharedPtr sub_joy_; + rclcpp::Subscription::SharedPtr sub_odom_; + + rclcpp::Time last_joy_received_time_; + std::shared_ptr joy_; + geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_; + + void onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg); + void onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg); + + // Publisher + rclcpp::Publisher::SharedPtr + pub_control_command_; + rclcpp::Publisher::SharedPtr + pub_external_control_command_; + rclcpp::Publisher::SharedPtr pub_shift_; + rclcpp::Publisher::SharedPtr pub_turn_signal_; + rclcpp::Publisher::SharedPtr pub_heartbeat_; + rclcpp::Publisher::SharedPtr pub_gate_mode_; + rclcpp::Publisher::SharedPtr pub_vehicle_engage_; + + void publishControlCommand(); + void publishExternalControlCommand(); + void publishShift(); + void publishTurnSignal(); + void publishGateMode(); + void publishHeartbeat(); + void publishAutowareEngage(); + void publishVehicleEngage(); + void sendEmergencyRequest(bool emergency); + + // Service Client + rclcpp::Client::SharedPtr client_emergency_stop_; + rclcpp::Client::SharedPtr client_autoware_engage_; + + // Previous State + autoware_auto_control_msgs::msg::AckermannControlCommand prev_control_command_; + autoware_external_api_msgs::msg::ControlCommand prev_external_control_command_; + GearShiftType prev_shift_ = autoware_external_api_msgs::msg::GearShift::NONE; + TurnSignalType prev_turn_signal_ = autoware_external_api_msgs::msg::TurnSignal::NONE; + GateModeType prev_gate_mode_ = autoware_control_msgs::msg::GateMode::AUTO; + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + void initTimer(double period_s); + + bool isDataReady(); + void onTimer(); +}; +} // namespace autoware_joy_controller + +#endif // AUTOWARE_JOY_CONTROLLER__AUTOWARE_JOY_CONTROLLER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp new file mode 100644 index 0000000000000..0a35696808d94 --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/ds4_joy_converter.hpp @@ -0,0 +1,95 @@ +// 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 AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ + +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" + +#include + +namespace autoware_joy_controller +{ +class DS4JoyConverter : public JoyConverterBase +{ +public: + explicit DS4JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} + + float accel() const + { + const auto button = static_cast(Cross()); + const auto stick = std::max(0.0f, RStickUpDown()); + const auto trigger = std::max(0.0f, -RTrigger()); + return std::max({button, stick, trigger}); + } + + float brake() const + { + const auto button = static_cast(Square()); + const auto stick = std::max(0.0f, -RStickUpDown()); + const auto trigger = std::max(0.0f, -LTrigger()); + return std::max({button, stick, trigger}); + } + + float steer() const { return LStickLeftRight(); } + + bool shift_up() const { return CursorUpDown() == 1.0f; } + bool shift_down() const { return CursorUpDown() == -1.0f; } + bool shift_drive() const { return CursorLeftRight() == 1.0f; } + bool shift_reverse() const { return CursorLeftRight() == -1.0f; } + + bool turn_signal_left() const { return L1(); } + bool turn_signal_right() const { return R1(); } + bool clear_turn_signal() const { return Share(); } + + bool gate_mode() const { return Options(); } + + bool emergency_stop() const { return !reverse() && PS(); } + bool clear_emergency_stop() const { return reverse() && PS(); } + + bool autoware_engage() const { return !reverse() && Circle(); } + bool autoware_disengage() const { return reverse() && Circle(); } + + bool vehicle_engage() const { return !reverse() && Triangle(); } + bool vehicle_disengage() const { return reverse() && Triangle(); } + +private: + float LStickLeftRight() const { return j_.axes.at(0); } + float LStickUpDown() const { return j_.axes.at(1); } + float LTrigger() const { return j_.axes.at(2); } + float RStickLeftRight() const { return j_.axes.at(3); } + float RStickUpDown() const { return j_.axes.at(4); } + float RTrigger() const { return j_.axes.at(5); } + float CursorLeftRight() const { return j_.axes.at(6); } + float CursorUpDown() const { return j_.axes.at(7); } + + bool Cross() const { return j_.buttons.at(0); } + bool Circle() const { return j_.buttons.at(1); } + bool Triangle() const { return j_.buttons.at(2); } + bool Square() const { return j_.buttons.at(3); } + bool L1() const { return j_.buttons.at(4); } + bool R1() const { return j_.buttons.at(5); } + bool L2() const { return j_.buttons.at(6); } + bool R2() const { return j_.buttons.at(7); } + bool Share() const { return j_.buttons.at(8); } + bool Options() const { return j_.buttons.at(9); } + bool PS() const { return j_.buttons.at(10); } + + const sensor_msgs::msg::Joy j_; + + bool reverse() const { return Share(); } +}; +} // namespace autoware_joy_controller + +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__DS4_JOY_CONVERTER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp new file mode 100644 index 0000000000000..22039cdf6f95f --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/g29_joy_converter.hpp @@ -0,0 +1,93 @@ +// 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 AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ + +#include "autoware_joy_controller/joy_converter/joy_converter_base.hpp" + +namespace autoware_joy_controller +{ +class G29JoyConverter : public JoyConverterBase +{ +public: + explicit G29JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {} + + float accel() const + { + constexpr float eps = 0.0000001; + if (std::fabs(AccelPedal()) < eps) { + return 0.0f; + } + return (AccelPedal() + 1.0f) / 2; + } + + float brake() const + { + constexpr float eps = 0.0000001; + if (std::fabs(BrakePedal()) < eps) { + return 0.0f; + } + return (BrakePedal() + 1.0f) / 2; + } + + float steer() const { return Steer(); } + + bool shift_up() const { return CursorUpDown() == 1.0f; } + bool shift_down() const { return CursorUpDown() == -1.0f; } + bool shift_drive() const { return CursorLeftRight() == 1.0f; } + bool shift_reverse() const { return CursorLeftRight() == -1.0f; } + + bool turn_signal_left() const { return L1(); } + bool turn_signal_right() const { return R1(); } + bool clear_turn_signal() const { return Share(); } + + bool gate_mode() const { return Options(); } + + bool emergency_stop() const { return !reverse() && PS(); } + bool clear_emergency_stop() const { return reverse() && PS(); } + + bool autoware_engage() const { return !reverse() && Circle(); } + bool autoware_disengage() const { return reverse() && Circle(); } + + bool vehicle_engage() const { return !reverse() && Triangle(); } + bool vehicle_disengage() const { return reverse() && Triangle(); } + +private: + float Steer() const { return j_.axes.at(0); } + float AccelPedal() const { return j_.axes.at(2); } + float BrakePedal() const { return j_.axes.at(3); } + + float CursorLeftRight() const { return j_.axes.at(4); } + float CursorUpDown() const { return j_.axes.at(5); } + + bool Cross() const { return j_.buttons.at(0); } + bool Circle() const { return j_.buttons.at(1); } + bool Triangle() const { return j_.buttons.at(2); } + bool Square() const { return j_.buttons.at(3); } + bool L1() const { return j_.buttons.at(5); } + bool R1() const { return j_.buttons.at(4); } + bool L2() const { return j_.buttons.at(7); } + bool R2() const { return j_.buttons.at(6); } + bool Share() const { return j_.buttons.at(8); } + bool Options() const { return j_.buttons.at(9); } + bool PS() const { return j_.buttons.at(24); } + + const sensor_msgs::msg::Joy j_; + + bool reverse() const { return Share(); } +}; +} // namespace autoware_joy_controller + +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__G29_JOY_CONVERTER_HPP_ diff --git a/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp new file mode 100644 index 0000000000000..3da69c915e21a --- /dev/null +++ b/control/autoware_joy_controller/include/autoware_joy_controller/joy_converter/joy_converter_base.hpp @@ -0,0 +1,55 @@ +// 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 AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ +#define AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ + +#include + +#include + +namespace autoware_joy_controller +{ +class JoyConverterBase +{ +public: + virtual float accel() const = 0; + + virtual float brake() const = 0; + + virtual float steer() const = 0; + + virtual bool shift_up() const = 0; + virtual bool shift_down() const = 0; + virtual bool shift_drive() const = 0; + virtual bool shift_reverse() const = 0; + + virtual bool turn_signal_left() const = 0; + virtual bool turn_signal_right() const = 0; + virtual bool clear_turn_signal() const = 0; + + virtual bool gate_mode() const = 0; + + virtual bool emergency_stop() const = 0; + virtual bool clear_emergency_stop() const = 0; + + virtual bool autoware_engage() const = 0; + virtual bool autoware_disengage() const = 0; + + virtual bool vehicle_engage() const = 0; + virtual bool vehicle_disengage() const = 0; +}; +} // namespace autoware_joy_controller + +#endif // AUTOWARE_JOY_CONTROLLER__JOY_CONVERTER__JOY_CONVERTER_BASE_HPP_ diff --git a/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml new file mode 100644 index 0000000000000..62dc933a3dfbf --- /dev/null +++ b/control/autoware_joy_controller/launch/autoware_joy_controller.launch.xml @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control/autoware_joy_controller/package.xml b/control/autoware_joy_controller/package.xml new file mode 100644 index 0000000000000..1839dbab645fa --- /dev/null +++ b/control/autoware_joy_controller/package.xml @@ -0,0 +1,30 @@ + + + autoware_joy_controller + 0.1.0 + The autoware_joy_controller package + Kenji Miyake + Apache License 2.0 + + ament_cmake_auto + + autoware_api_utils + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + autoware_control_msgs + autoware_external_api_msgs + geometry_msgs + joy + nav_msgs + rclcpp + rclcpp_components + sensor_msgs + std_srvs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp new file mode 100644 index 0000000000000..feca8f12c5dea --- /dev/null +++ b/control/autoware_joy_controller/src/autoware_joy_controller/autoware_joy_controller_node.cpp @@ -0,0 +1,524 @@ +// 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 "autoware_joy_controller/autoware_joy_controller.hpp" +#include "autoware_joy_controller/joy_converter/ds4_joy_converter.hpp" +#include "autoware_joy_controller/joy_converter/g29_joy_converter.hpp" + +#include + +#include +#include +#include +#include + +namespace +{ +using autoware_joy_controller::GateModeType; +using autoware_joy_controller::GearShiftType; +using autoware_joy_controller::TurnSignalType; +using GearShift = autoware_external_api_msgs::msg::GearShift; +using TurnSignal = autoware_external_api_msgs::msg::TurnSignal; +using GateMode = autoware_control_msgs::msg::GateMode; + +GearShiftType getUpperShift(const GearShiftType & shift) +{ + if (shift == GearShift::NONE) { + return GearShift::PARKING; + } + if (shift == GearShift::PARKING) { + return GearShift::REVERSE; + } + if (shift == GearShift::REVERSE) { + return GearShift::NEUTRAL; + } + if (shift == GearShift::NEUTRAL) { + return GearShift::DRIVE; + } + if (shift == GearShift::DRIVE) { + return GearShift::LOW; + } + if (shift == GearShift::LOW) { + return GearShift::LOW; + } + + return GearShift::NONE; +} + +GearShiftType getLowerShift(const GearShiftType & shift) +{ + if (shift == GearShift::NONE) { + return GearShift::PARKING; + } + if (shift == GearShift::PARKING) { + return GearShift::PARKING; + } + if (shift == GearShift::REVERSE) { + return GearShift::PARKING; + } + if (shift == GearShift::NEUTRAL) { + return GearShift::REVERSE; + } + if (shift == GearShift::DRIVE) { + return GearShift::NEUTRAL; + } + if (shift == GearShift::LOW) { + return GearShift::DRIVE; + } + + return GearShift::NONE; +} + +const char * getShiftName(const GearShiftType & shift) +{ + if (shift == GearShift::NONE) { + return "NONE"; + } + if (shift == GearShift::PARKING) { + return "PARKING"; + } + if (shift == GearShift::REVERSE) { + return "REVERSE"; + } + if (shift == GearShift::NEUTRAL) { + return "NEUTRAL"; + } + if (shift == GearShift::DRIVE) { + return "DRIVE"; + } + if (shift == GearShift::LOW) { + return "LOW"; + } + + return "NOT_SUPPORTED"; +} + +const char * getTurnSignalName(const TurnSignalType & turn_signal) +{ + if (turn_signal == TurnSignal::NONE) { + return "NONE"; + } + if (turn_signal == TurnSignal::LEFT) { + return "LEFT"; + } + if (turn_signal == TurnSignal::RIGHT) { + return "RIGHT"; + } + if (turn_signal == TurnSignal::HAZARD) { + return "HAZARD"; + } + + return "NOT_SUPPORTED"; +} + +const char * getGateModeName(const GateModeType & gate_mode) +{ + using autoware_control_msgs::msg::GateMode; + + if (gate_mode == GateMode::AUTO) { + return "AUTO"; + } + if (gate_mode == GateMode::EXTERNAL) { + return "EXTERNAL"; + } + + return "NOT_SUPPORTED"; +} + +double calcMapping(const double input, const double sensitivity) +{ + const double exponent = 1.0 / (std::max(0.001, std::min(1.0, sensitivity))); + return std::pow(input, exponent); +} + +} // namespace + +namespace autoware_joy_controller +{ +void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPtr msg) +{ + last_joy_received_time_ = msg->header.stamp; + if (joy_type_ == "G29") { + joy_ = std::make_shared(*msg); + } else { + joy_ = std::make_shared(*msg); + } + + if (joy_->shift_up() || joy_->shift_down() || joy_->shift_drive() || joy_->shift_reverse()) { + publishShift(); + } + + if (joy_->turn_signal_left() || joy_->turn_signal_right() || joy_->clear_turn_signal()) { + publishTurnSignal(); + } + + if (joy_->gate_mode()) { + publishGateMode(); + } + + if (joy_->autoware_engage() || joy_->autoware_disengage()) { + publishAutowareEngage(); + } + + if (joy_->vehicle_engage() || joy_->vehicle_disengage()) { + publishVehicleEngage(); + } + + if (joy_->emergency_stop()) { + sendEmergencyRequest(true); + } + + if (joy_->clear_emergency_stop()) { + sendEmergencyRequest(false); + } +} + +void AutowareJoyControllerNode::onOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +{ + auto twist = std::make_shared(); + twist->header = msg->header; + twist->twist = msg->twist.twist; + + twist_ = twist; +} + +bool AutowareJoyControllerNode::isDataReady() +{ + // Joy + { + if (!joy_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "waiting for joy msg..."); + return false; + } + + constexpr auto timeout = 2.0; + const auto time_diff = this->now() - last_joy_received_time_; + if (time_diff.seconds() > timeout) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), "joy msg is timeout"); + return false; + } + } + + // Twist + { + if (!twist_) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "waiting for twist msg..."); + return false; + } + + constexpr auto timeout = 0.5; + const auto time_diff = this->now() - twist_->header.stamp; + if (time_diff.seconds() > timeout) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "twist msg is timeout"); + return false; + } + } + + return true; +} + +void AutowareJoyControllerNode::onTimer() +{ + if (!isDataReady()) { + return; + } + + publishControlCommand(); + publishExternalControlCommand(); + publishHeartbeat(); +} + +void AutowareJoyControllerNode::publishControlCommand() +{ + autoware_auto_control_msgs::msg::AckermannControlCommand cmd; + cmd.stamp = this->now(); + { + cmd.lateral.steering_tire_angle = steer_ratio_ * joy_->steer(); + cmd.lateral.steering_tire_rotation_rate = steering_angle_velocity_; + + if (joy_->accel()) { + cmd.longitudinal.acceleration = accel_ratio_ * joy_->accel(); + cmd.longitudinal.speed = + twist_->twist.linear.x + velocity_gain_ * cmd.longitudinal.acceleration; + cmd.longitudinal.speed = + std::min(cmd.longitudinal.speed, static_cast(max_forward_velocity_)); + } + + if (joy_->brake()) { + cmd.longitudinal.speed = 0.0; + cmd.longitudinal.acceleration = -brake_ratio_ * joy_->brake(); + } + + // Backward + if (joy_->accel() && joy_->brake()) { + cmd.longitudinal.acceleration = backward_accel_ratio_ * joy_->accel(); + cmd.longitudinal.speed = + twist_->twist.linear.x - velocity_gain_ * cmd.longitudinal.acceleration; + cmd.longitudinal.speed = + std::max(cmd.longitudinal.speed, static_cast(-max_backward_velocity_)); + } + } + + pub_control_command_->publish(cmd); + prev_control_command_ = cmd; +} + +void AutowareJoyControllerNode::publishExternalControlCommand() +{ + autoware_external_api_msgs::msg::ControlCommandStamped cmd_stamped; + cmd_stamped.stamp = this->now(); + { + auto & cmd = cmd_stamped.control; + + cmd.steering_angle = steer_ratio_ * joy_->steer(); + cmd.steering_angle_velocity = steering_angle_velocity_; + cmd.throttle = + accel_ratio_ * calcMapping(static_cast(joy_->accel()), accel_sensitivity_); + cmd.brake = brake_ratio_ * calcMapping(static_cast(joy_->brake()), brake_sensitivity_); + } + + pub_external_control_command_->publish(cmd_stamped); + prev_external_control_command_ = cmd_stamped.control; +} + +void AutowareJoyControllerNode::publishShift() +{ + autoware_external_api_msgs::msg::GearShiftStamped gear_shift; + gear_shift.stamp = this->now(); + + if (joy_->shift_up()) { + gear_shift.gear_shift.data = getUpperShift(prev_shift_); + } + + if (joy_->shift_down()) { + gear_shift.gear_shift.data = getLowerShift(prev_shift_); + } + + if (joy_->shift_drive()) { + gear_shift.gear_shift.data = GearShift::DRIVE; + } + + if (joy_->shift_reverse()) { + gear_shift.gear_shift.data = GearShift::REVERSE; + } + + RCLCPP_INFO(get_logger(), "GearShift::%s", getShiftName(gear_shift.gear_shift.data)); + + pub_shift_->publish(gear_shift); + prev_shift_ = gear_shift.gear_shift.data; +} + +void AutowareJoyControllerNode::publishTurnSignal() +{ + autoware_external_api_msgs::msg::TurnSignalStamped turn_signal; + turn_signal.stamp = this->now(); + + if (joy_->turn_signal_left() && joy_->turn_signal_right()) { + turn_signal.turn_signal.data = TurnSignal::HAZARD; + } else if (joy_->turn_signal_left()) { + turn_signal.turn_signal.data = TurnSignal::LEFT; + } else if (joy_->turn_signal_right()) { + turn_signal.turn_signal.data = TurnSignal::RIGHT; + } + + if (joy_->clear_turn_signal()) { + turn_signal.turn_signal.data = TurnSignal::NONE; + } + + RCLCPP_INFO(get_logger(), "TurnSignal::%s", getTurnSignalName(turn_signal.turn_signal.data)); + + pub_turn_signal_->publish(turn_signal); +} + +void AutowareJoyControllerNode::publishGateMode() +{ + autoware_control_msgs::msg::GateMode gate_mode; + + if (prev_gate_mode_ == GateMode::AUTO) { + gate_mode.data = GateMode::EXTERNAL; + } + + if (prev_gate_mode_ == GateMode::EXTERNAL) { + gate_mode.data = GateMode::AUTO; + } + + RCLCPP_INFO(get_logger(), "GateMode::%s", getGateModeName(gate_mode.data)); + + pub_gate_mode_->publish(gate_mode); + prev_gate_mode_ = gate_mode.data; +} + +void AutowareJoyControllerNode::publishHeartbeat() +{ + autoware_external_api_msgs::msg::Heartbeat heartbeat; + heartbeat.stamp = this->now(); + pub_heartbeat_->publish(heartbeat); +} + +void AutowareJoyControllerNode::sendEmergencyRequest(bool emergency) +{ + RCLCPP_INFO(get_logger(), "%s emergency stop", emergency ? "Set" : "Clear"); + + auto request = std::make_shared(); + request->emergency = emergency; + + client_emergency_stop_->async_send_request( + request, [this, emergency]( + rclcpp::Client::SharedFuture result) { + auto response = result.get(); + if (autoware_api_utils::is_success(response->status)) { + RCLCPP_INFO(get_logger(), "service succeeded"); + } else { + RCLCPP_WARN(get_logger(), "service failed: %s", response->status.message.c_str()); + } + }); +} + +void AutowareJoyControllerNode::publishAutowareEngage() +{ + auto req = std::make_shared(); + if (joy_->autoware_engage()) { + req->engage = true; + RCLCPP_INFO(get_logger(), "Autoware Engage"); + } + + if (joy_->autoware_disengage()) { + req->engage = false; + RCLCPP_INFO(get_logger(), "Autoware Disengage"); + } + + if (!client_autoware_engage_->service_is_ready()) { + RCLCPP_WARN(get_logger(), "%s is unavailable", client_autoware_engage_->get_service_name()); + return; + } + + client_autoware_engage_->async_send_request( + req, [this](rclcpp::Client::SharedFuture result) { + RCLCPP_INFO( + get_logger(), "%s: %d, %s", client_autoware_engage_->get_service_name(), + result.get()->status.code, result.get()->status.message.c_str()); + }); +} + +void AutowareJoyControllerNode::publishVehicleEngage() +{ + autoware_auto_vehicle_msgs::msg::Engage engage; + + if (joy_->vehicle_engage()) { + engage.engage = true; + RCLCPP_INFO(get_logger(), "Vehicle Engage"); + } + + if (joy_->vehicle_disengage()) { + engage.engage = false; + RCLCPP_INFO(get_logger(), "Vehicle Disengage"); + } + + pub_vehicle_engage_->publish(engage); +} + +void AutowareJoyControllerNode::initTimer(double period_s) +{ + auto timer_callback = std::bind(&AutowareJoyControllerNode::onTimer, this); + const auto period_ns = + std::chrono::duration_cast(std::chrono::duration(period_s)); + timer_ = std::make_shared>( + this->get_clock(), period_ns, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_, nullptr); +} + +AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions & node_options) +: Node("autoware_joy_controller", node_options) +{ + // Parameter + joy_type_ = declare_parameter("joy_type", std::string("DS4")); + update_rate_ = declare_parameter("update_rate", 10.0); + accel_ratio_ = declare_parameter("accel_ratio", 3.0); + brake_ratio_ = declare_parameter("brake_ratio", 5.0); + steer_ratio_ = declare_parameter("steer_ratio", 0.5); + steering_angle_velocity_ = declare_parameter("steering_angle_velocity", 0.1); + accel_sensitivity_ = declare_parameter("accel_sensitivity", 1.0); + brake_sensitivity_ = declare_parameter("brake_sensitivity", 1.0); + velocity_gain_ = declare_parameter("control_command.velocity_gain", 3.0); + max_forward_velocity_ = declare_parameter("control_command.max_forward_velocity", 20.0); + max_backward_velocity_ = declare_parameter("control_command.max_backward_velocity", 3.0); + backward_accel_ratio_ = declare_parameter("control_command.backward_accel_ratio", 1.0); + + RCLCPP_INFO(get_logger(), "Joy type: %s", joy_type_.c_str()); + + // 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_joy_ = this->create_subscription( + "input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1), + subscriber_option); + sub_odom_ = this->create_subscription( + "input/odometry", 1, + std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1), + subscriber_option); + + // Publisher + pub_control_command_ = + this->create_publisher( + "output/control_command", 1); + pub_external_control_command_ = + this->create_publisher( + "output/external_control_command", 1); + pub_shift_ = + this->create_publisher("output/shift", 1); + pub_turn_signal_ = this->create_publisher( + "output/turn_signal", 1); + pub_gate_mode_ = + this->create_publisher("output/gate_mode", 1); + pub_heartbeat_ = + this->create_publisher("output/heartbeat", 1); + pub_vehicle_engage_ = + this->create_publisher("output/vehicle_engage", 1); + + // Service Client + client_emergency_stop_ = this->create_client( + "service/emergency_stop", rmw_qos_profile_services_default, callback_group_services_); + while (!client_emergency_stop_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(get_logger(), "Interrupted while waiting for service."); + rclcpp::shutdown(); + return; + } + RCLCPP_INFO(get_logger(), "Waiting for emergency_stop service connection..."); + } + + client_autoware_engage_ = this->create_client( + "service/autoware_engage", rmw_qos_profile_services_default); + + // Timer + initTimer(1.0 / update_rate_); +} +} // namespace autoware_joy_controller + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(autoware_joy_controller::AutowareJoyControllerNode)