feat(joy_controller): adding genesis joystick to joy controller (#683)

* feat(joy_controller): add genesis p65 converter

Signed-off-by: Berkay <berkay@leodrive.ai>

* feat(joy_controller): fix steering direction

Signed-off-by: Berkay <berkay@leodrive.ai>

* feat(joy_controller): changed map

Signed-off-by: Berkay <berkay@leodrive.ai>

* ci(pre-commit): autofix

Co-authored-by: Berkay <berkay@leodrive.ai>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

control/joy_controller/README.md

@@ -41,3 +41,24 @@
 | `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)                                    |
+
+## P65 Joystick Key Map
+
+| Acceleration         | R2                    |
+| -------------------- | --------------------- |
+| Brake                | L2                    |
+| Steering             | Left Stick Left Right |
+| Shift up             | Cursor Up             |
+| Shift down           | Cursor Down           |
+| Shift Drive          | Cursor Left           |
+| Shift Reverse        | Cursor Right          |
+| Turn Signal Left     | L1                    |
+| Turn Signal Right    | R1                    |
+| Clear Turn Signal    | A                     |
+| Gate Mode            | B                     |
+| Emergency Stop       | Select                |
+| Clear Emergency Stop | Start                 |
+| Autoware Engage      | X                     |
+| Autoware Disengage   | Y                     |
+| Vehicle Engage       | PS                    |
+| Vehicle Disengage    | Right Trigger         |

control/joy_controller/include/joy_controller/joy_converter/p65_joy_converter.hpp

@@ -0,0 +1,81 @@
+// Copyright 2022 Leo Drive Teknoloji A.Ş., 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 JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_
+#define JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_
+
+#include "joy_controller/joy_converter/joy_converter_base.hpp"
+
+#include <algorithm>
+
+namespace joy_controller
+{
+class P65JoyConverter : public JoyConverterBase
+{
+public:
+  explicit P65JoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {}
+
+  float accel() const { return std::max(0.0f, -((R2() - 1.0f) / 2.0f)); }
+
+  float brake() const { return std::max(0.0f, -((L2() - 1.0f) / 2.0f)); }
+
+  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 A(); }
+
+  bool gate_mode() const { return B(); }
+
+  bool emergency_stop() const { return Select(); }
+  bool clear_emergency_stop() const { return Start(); }
+
+  bool autoware_engage() const { return X(); }
+  bool autoware_disengage() const { return Y(); }
+
+  bool vehicle_engage() const { return PS(); }
+  bool vehicle_disengage() const { return RTrigger(); }
+
+private:
+  float LStickLeftRight() const { return j_.axes.at(0); }
+  float LStickUpDown() const { return j_.axes.at(1); }
+  float RStickLeftRight() const { return j_.axes.at(3); }
+  float RStickUpDown() const { return j_.axes.at(4); }
+  float CursorLeftRight() const { return j_.axes.at(6); }
+  float CursorUpDown() const { return j_.axes.at(7); }
+  float L2() const { return j_.axes.at(2); }
+  float R2() const { return j_.axes.at(5); }
+
+  bool A() const { return j_.buttons.at(0); }
+  bool B() const { return j_.buttons.at(1); }
+  bool X() const { return j_.buttons.at(2); }
+  bool Y() const { return j_.buttons.at(3); }
+  bool L1() const { return j_.buttons.at(4); }
+  bool R1() const { return j_.buttons.at(5); }
+  bool Select() const { return j_.buttons.at(6); }
+  bool Start() const { return j_.buttons.at(7); }
+  bool PS() const { return j_.buttons.at(8); }
+  bool LTrigger() const { return j_.buttons.at(9); }
+  bool RTrigger() const { return j_.buttons.at(10); }
+
+  const sensor_msgs::msg::Joy j_;
+};
+}  // namespace joy_controller
+
+#endif  // JOY_CONTROLLER__JOY_CONVERTER__P65_JOY_CONVERTER_HPP_

control/joy_controller/launch/joy_controller.launch.xml

@@ -1,5 +1,5 @@
 <launch>
-  <arg name="joy_type" default="DS4" description="options: DS4, G29"/>
+  <arg name="joy_type" default="DS4" description="options: DS4, G29, P65"/>
   <arg name="external_cmd_source" default="remote" description="options: local, remote"/>
 
   <arg name="input_joy" default="/joy"/>

control/joy_controller/src/joy_controller/joy_controller_node.cpp

@@ -15,6 +15,7 @@
 #include "joy_controller/joy_controller.hpp"
 #include "joy_controller/joy_converter/ds4_joy_converter.hpp"
 #include "joy_controller/joy_converter/g29_joy_converter.hpp"
+#include "joy_controller/joy_converter/p65_joy_converter.hpp"
 
 #include <tier4_api_utils/tier4_api_utils.hpp>
 
@@ -151,8 +152,10 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt
   last_joy_received_time_ = msg->header.stamp;
   if (joy_type_ == "G29") {
     joy_ = std::make_shared<const G29JoyConverter>(*msg);
-  } else {
+  } else if (joy_type_ == "DS4") {
     joy_ = std::make_shared<const DS4JoyConverter>(*msg);
+  } else {
+    joy_ = std::make_shared<const P65JoyConverter>(*msg);
   }
 
   if (joy_->shift_up() || joy_->shift_down() || joy_->shift_drive() || joy_->shift_reverse()) {