feat: add autoware joy controller

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
+)

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)                                    |

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

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 <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
+#include <autoware_auto_vehicle_msgs/msg/engage.hpp>
+#include <autoware_control_msgs/msg/gate_mode.hpp>
+#include <autoware_external_api_msgs/msg/control_command_stamped.hpp>
+#include <autoware_external_api_msgs/msg/gear_shift_stamped.hpp>
+#include <autoware_external_api_msgs/msg/heartbeat.hpp>
+#include <autoware_external_api_msgs/msg/turn_signal_stamped.hpp>
+#include <autoware_external_api_msgs/srv/engage.hpp>
+#include <autoware_external_api_msgs/srv/set_emergency.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <sensor_msgs/msg/joy.hpp>
+
+#include <algorithm>
+#include <memory>
+#include <string>
+
+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<sensor_msgs::msg::Joy>::SharedPtr sub_joy_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
+
+  rclcpp::Time last_joy_received_time_;
+  std::shared_ptr<const JoyConverterBase> 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<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
+    pub_control_command_;
+  rclcpp::Publisher<autoware_external_api_msgs::msg::ControlCommandStamped>::SharedPtr
+    pub_external_control_command_;
+  rclcpp::Publisher<autoware_external_api_msgs::msg::GearShiftStamped>::SharedPtr pub_shift_;
+  rclcpp::Publisher<autoware_external_api_msgs::msg::TurnSignalStamped>::SharedPtr pub_turn_signal_;
+  rclcpp::Publisher<autoware_external_api_msgs::msg::Heartbeat>::SharedPtr pub_heartbeat_;
+  rclcpp::Publisher<autoware_control_msgs::msg::GateMode>::SharedPtr pub_gate_mode_;
+  rclcpp::Publisher<autoware_auto_vehicle_msgs::msg::Engage>::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<autoware_external_api_msgs::srv::SetEmergency>::SharedPtr client_emergency_stop_;
+  rclcpp::Client<autoware_external_api_msgs::srv::Engage>::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_

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 <algorithm>
+
+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<float>(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<float>(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_