Ros2 devel (#4)

* modify ros2 pacmod3 pacmod_game_control to work with pacmod2 GEM

* try to fix CI

* increment version number

* update launch file

* update launch file again

.circleci/config.yml

@@ -1,41 +1,43 @@
-version: 2
-jobs:
-  noetic:
-    docker:
-      - image: autonomoustuff/docker-builds:noetic-ros-base
+version: 2.1
+
+commands:
+  ros_build:
     steps:
       - checkout
       - run:
           name: Set Up Container
           command: |
-            apt-get update -qq
+            mkdir $CIRCLE_PROJECT_REPONAME
+            mv `find -maxdepth 1 -not -name . -not -name $CIRCLE_PROJECT_REPONAME` $CIRCLE_PROJECT_REPONAME/
+      - run:
+          name: Install Dependencies
+          command: |
             source /opt/ros/*/setup.bash
-            mkdir pacmod2_game_control && mv `find -maxdepth 1 -not -name . -not -name pacmod2_game_control` pacmod2_game_control/
-            git clone https://github.com/astuff/astuff_sensor_msgs astuff_sensor_msgs
+            apt update
+            rosdep update
             rosdep install --from-paths . --ignore-src -y
-            cd ..
-            catkin init
-            catkin config --extend /opt/ros/$ROS_DISTRO
       - run:
           name: Build
           command: |
+            source /opt/ros/*/setup.bash
             cd ..
-            catkin build
-      - run:
-          name: Lint
-          command: |
-            catkin build pacmod2_game_control --no-deps --make-args roslint
+            colcon build
       - run:
           name: Run Tests
           command: |
             source /opt/ros/*/setup.bash
             cd ..
-            catkin run_tests pacmod2_game_control --no-deps
-            catkin_test_results
+            colcon test --packages-select $CIRCLE_PROJECT_REPONAME
+            colcon test-result --verbose
+jobs:
+  foxy:
+    docker:
+      - image: autonomoustuff/docker-builds:foxy-ros-base
+    steps:
+      - ros_build
     working_directory: ~/src
 
 workflows:
-  version: 2
   ros_build:
     jobs:
-      - noetic
+      - foxy

CMakeLists.txt

@@ -1,58 +1,34 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(pacmod2_game_control)
 
-add_definitions(-std=c++11)
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  roslint
-  roscpp
-  std_msgs
-  sensor_msgs
-  pacmod_msgs
-  joy
-)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
 
-catkin_package(CATKIN_DEPENDS
-  std_msgs
-  sensor_msgs
-  pacmod_msgs
-  joy
-)
+find_package(ament_cmake_auto REQUIRED)
 
-include_directories(include
-  ${catkin_INCLUDE_DIRS}
-)
+ament_auto_find_build_dependencies()
 
-add_executable(pacmod2_game_control_node
+# Build
+ament_auto_add_library(${PROJECT_NAME}_node SHARED
   src/pacmod_game_control_node.cpp
   src/startup_checks.cpp
-  src/publish_control_board_rev2.cpp
-  src/publish_control_board_rev3.cpp
-  src/publish_control.cpp
-  src/publish_control_factory.cpp
+  src/controllers.cpp
 )
 
-add_dependencies(pacmod2_game_control_node
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
+ament_auto_add_executable(${PROJECT_NAME}_exec
+  src/pacmod_game_control_exec.cpp
 )
-target_link_libraries(pacmod2_game_control_node
-  ${catkin_LIBRARIES}
-)
-
-set(ROSLINT_CPP_OPTS "--filter=-build/namespaces,-build/c++11")
-roslint_cpp()
 
-install(TARGETS pacmod2_game_control_node
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
-
-install(DIRECTORY launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# Install
+install(FILES README.md
+  DESTINATION share/${PROJECT_NAME}/
 )
 
-install(FILES README.md
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ament_auto_package(INSTALL_TO_SHARE
+  launch
 )

include/pacmod_game_control/controllers.h

@@ -0,0 +1,106 @@
+/*
+ * Unpublished Copyright (c) 2009-2021 AutonomouStuff, LLC, All Rights Reserved.
+ *
+ * This file is part of the PACMod ROS 1.0 driver which is released under the MIT license.
+ * See file LICENSE included with this software or go to https://opensource.org/licenses/MIT for full license details.
+ */
+
+#ifndef PACMOD_GAME_CONTROL_CONTROLLERS_H
+#define PACMOD_GAME_CONTROL_CONTROLLERS_H
+
+#include <unordered_map>
+
+#include <sensor_msgs/msg/joy.hpp>
+
+namespace controllers
+{
+
+enum class JoyAxis
+{
+  LEFT_STICK_UD,
+  LEFT_STICK_LR,
+  RIGHT_STICK_UD,
+  RIGHT_STICK_LR,
+  DPAD_UD,
+  DPAD_LR,
+  LEFT_TRIGGER_AXIS,  // Sometimes button, sometimes axis
+  RIGHT_TRIGGER_AXIS  // Sometimes button, sometimes axis
+};
+
+enum class JoyButton
+{
+  TOP_BTN,
+  LEFT_BTN,
+  BOTTOM_BTN,
+  RIGHT_BTN,
+  LEFT_BUMPER,
+  RIGHT_BUMPER,
+  BACK_SELECT_MINUS,
+  START_PLUS,
+  LEFT_TRIGGER_BTN,   // Sometimes button, sometimes axis
+  RIGHT_TRIGGER_BTN,  // Sometimes button, sometimes axis
+  LEFT_STICK_PUSH,
+  RIGHT_STICK_PUSH
+};
+
+class Controller
+{
+public:
+  virtual ~Controller() = default;
+  void set_controller_input(const sensor_msgs::msg::Joy& joy_msg);
+  virtual float accelerator_value();
+  virtual float brake_value();
+  virtual float steering_value();
+  virtual int turn_signal_cmd();
+  virtual int shift_cmd();
+  virtual bool horn_cmd();
+  virtual bool headlight_change();
+  virtual bool hazards_cmd();
+  virtual bool wiper_change();
+  virtual bool enable();
+  virtual bool disable();
+
+protected:
+  sensor_msgs::msg::Joy input_msg_;
+  sensor_msgs::msg::Joy prev_input_msg_;
+
+  // --- Generic gamepad controller (Logitech F310, XBOX)
+  std::unordered_map<JoyAxis, int> axes_ = {                                // NOLINT
+    { JoyAxis::LEFT_STICK_LR, 0 },     { JoyAxis::LEFT_STICK_UD, 1 },       // NOLINT
+    { JoyAxis::RIGHT_STICK_LR, 3 },    { JoyAxis::RIGHT_STICK_UD, 4 },      // NOLINT
+    { JoyAxis::LEFT_TRIGGER_AXIS, 2 }, { JoyAxis::RIGHT_TRIGGER_AXIS, 5 },  // NOLINT
+    { JoyAxis::DPAD_LR, 6 },           { JoyAxis::DPAD_UD, 7 }              // NOLINT
+  };
+                                                                          // NOLINT
+  std::unordered_map<JoyButton, int> btns_ = {                                                              // NOLINT
+    { JoyButton::BOTTOM_BTN, 0 },        { JoyButton::RIGHT_BTN, 1 },   { JoyButton::LEFT_BTN, 2 },         // NOLINT
+    { JoyButton::TOP_BTN, 3 },           { JoyButton::LEFT_BUMPER, 4 }, { JoyButton::RIGHT_BUMPER, 5 },     // NOLINT
+    { JoyButton::BACK_SELECT_MINUS, 6 }, { JoyButton::START_PLUS, 7 },  { JoyButton::LEFT_STICK_PUSH, 9 },  // NOLINT
+    { JoyButton::RIGHT_STICK_PUSH, 10 }                                                                     // NOLINT
+  };
+};
+
+class LogitechG29Controller : public Controller
+{
+public:
+  LogitechG29Controller();
+  float accelerator_value() override;
+  float brake_value() override;
+};
+
+class HriSafeController : public Controller
+{
+public:
+  HriSafeController();
+  float accelerator_value() override;
+  float brake_value() override;
+  int turn_signal_cmd() override;
+  bool horn_cmd() override;
+  bool wiper_change() override;
+  bool enable() override;
+  bool disable() override;
+};
+
+}  // namespace controllers
+
+#endif  // PACMOD_GAME_CONTROL_CONTROLLERS_H