Upgrade/gamepad mappings

racecar_behaviors/launch/behaviors.launch.py

@@ -1,58 +1,68 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
 from launch.substitutions import LaunchConfiguration
-from launch.conditions import IfCondition
-from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.launch_context import LaunchContext
 
-def launch_setup(context, *args, **kwargs):
+
+def launch_setup(context: LaunchContext, *args, **kwargs):
     # Declare launch arguments
-    prefix = LaunchConfiguration('prefix').perform(context)
-    problematique = LaunchConfiguration('problematique').perform(context)
-    navigation = LaunchConfiguration('navigation').perform(context)
+    prefix = LaunchConfiguration("prefix").perform(context)
+    # problematique = LaunchConfiguration("problematique").perform(context)  # NOTE: Unused.
+    # navigation = LaunchConfiguration("navigation").perform(context)  # NOTE: Unused.
 
-    # Define nodes for behaviors
     path_following_node = Node(
-        package='racecar_behaviors',
-        executable='path_following',
-        name='path_following',
-        output='screen',
-        parameters=[{'max_speed': 1}],
-        remappings=[('/cmd_vel', f'/{prefix}/cmd_vel_abtr_5'), ('/scan', f'/{prefix}/scan'), (f'/{prefix}/odom', f'/{prefix}/odometry/filtered')]
+        package="racecar_behaviors",
+        executable="path_following",
+        name="path_following",
+        output="screen",
+        parameters=[{"max_speed": 1}],
+        remappings=[
+            ("/cmd_vel", f"/{prefix}/cmd_vel_abtr_5"),
+            ("/scan", f"/{prefix}/scan"),
+            (f"/{prefix}/odom", f"/{prefix}/odometry/filtered"),
+        ],
     )
 
     obstacle_detector_node = Node(
-        package='racecar_behaviors',
-        executable='obstacle_detector',
-        name='obstacle_detector',
-        output='screen',
-        remappings=[('/cmd_vel', f'/{prefix}/cmd_vel_abtr_1'), ('/scan', f'/{prefix}/scan')]
+        package="racecar_behaviors",
+        executable="obstacle_detector",
+        name="obstacle_detector",
+        output="screen",
+        remappings=[
+            ("/cmd_vel", f"/{prefix}/cmd_vel_abtr_1"),
+            ("/scan", f"/{prefix}/scan"),
+        ],
     )
 
     # Return nodes
     return [
         path_following_node,
         obstacle_detector_node,
-        ]
+    ]
 
 
-
 def generate_launch_description():
     # Declare launch arguments
-    prefix_arg = DeclareLaunchArgument('prefix', default_value='racecar')
-    problematique_arg = DeclareLaunchArgument('problematique', default_value='false')
-    navigation_arg = DeclareLaunchArgument('navigation', default_value='false')
-    uturn_x_arg = DeclareLaunchArgument('uturn_x', default_value='999')
-    uturn_y_arg = DeclareLaunchArgument('uturn_y', default_value='999')
-    fixed_frame_id_arg = DeclareLaunchArgument('fixed_frame_id', default_value='odom')
-
+    prefix_arg = DeclareLaunchArgument(
+        "prefix", default_value="racecar", description="Namespace"
+    )
+    problematique_arg = DeclareLaunchArgument(
+        "problematique", default_value="false", choices=["true", "false"]
+    )  # NOTE: Unused.
+    navigation_arg = DeclareLaunchArgument(
+        "navigation", default_value="false", choices=["true", "false"]
+    )  # NOTE: Unused.
+    # uturn_x_arg = DeclareLaunchArgument("uturn_x", default_value="999")  # NOTE: Unused.
+    # uturn_y_arg = DeclareLaunchArgument("uturn_y", default_value="999")  # NOTE: Unused.
+    # fixed_frame_id_arg = DeclareLaunchArgument("fixed_frame_id", default_value="odom")  # NOTE: Unused.
 
     # Add nodes to launch description
-    return LaunchDescription([
-        prefix_arg,
-        problematique_arg,
-        navigation_arg,
-
-        OpaqueFunction(function=launch_setup)
-    ])
-
+    return LaunchDescription(
+        [
+            prefix_arg,
+            problematique_arg,
+            navigation_arg,
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

racecar_bringup/launch/GRO830.launch.py

@@ -44,7 +44,7 @@ def generate_launch_description():
 
         Node(
             package='joy',  
-            executable='joy_node',
+            executable='game_controller_node',
             name='joy',
             output='screen',
         ),

racecar_bringup/launch/autopilot.launch.py

@@ -10,7 +10,7 @@ def generate_launch_description():
         [
             Node(
                 package="joy",
-                executable="joy_node",
+                executable="game_controller_node",
                 name="joy",
                 parameters=[{"deadzone": 0.05}],
                 arguments=["dev", "/dev/input/js0"],

racecar_bringup/launch/basic_control.launch.py

@@ -10,7 +10,7 @@ def generate_launch_description():
         [
             Node(
                 package="joy",
-                executable="joy_node",
+                executable="game_controller_node",
                 name="joy",
                 parameters=[{"deadzone": 0.05}],
                 arguments=["dev", "/dev/input/js0"],

racecar_bringup/launch/teleop.launch.py

@@ -51,7 +51,7 @@ def generate_launch_description():
             actions=[
                 Node(
                     package='joy',
-                    executable='joy_node',
+                    executable='game_controller_node',
                     name='joy',
                     parameters=[{'deadzone': 0.05}],
                     arguments=['dev', '/dev/input/js0'],

racecar_bringup/package.xml

@@ -19,6 +19,7 @@
   <exec_depend>rclpy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
 
 
   <test_depend>ament_copyright</test_depend>

racecar_bringup/racecar_bringup/cmd_vel_arbitration.py

@@ -4,35 +4,61 @@
 from rclpy.node import Node
 from geometry_msgs.msg import Twist
 from std_msgs.msg import Int32
+from sensor_msgs.msg import Joy
+
 
 class Arbitration(Node):
     def __init__(self):
-        super().__init__('arbitration')
+        super().__init__("arbitration")
 
-        self._delay_sec = self.declare_parameter('delay_sec',0.5).value
+        self._delay_sec = (
+            self.declare_parameter("delay_sec", 0.5).get_parameter_value().double_value
+        )
         self._time_called = [self.get_clock().now() for _ in range(9)]
 
-        self._cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel_output', 1)
-        self._status_pub = self.create_publisher(Int32, 'status', 1)
+        self._cmd_vel_pub = self.create_publisher(Twist, "cmd_vel_output", 1)
+        self._status_pub = self.create_publisher(Int32, "status", 1)
+
+        self.joy_subscription = self.create_subscription(
+            Joy, "joy", self.check_autopilot_status, 1
+        )
 
         self._cmd_vel_subs = []
         for i in range(8):
-            sub = self.create_subscription(Twist, f'cmd_vel_abtr_{i}', lambda msg, priority=i: self.cmd_vel_callback(msg, priority), 1)
+            sub = self.create_subscription(
+                Twist,
+                f"cmd_vel_abtr_{i}",
+                lambda msg, priority=i: self.cmd_vel_callback(msg, priority),
+                1,
+            )
             self._cmd_vel_subs.append(sub)
 
-    def cmd_vel_callback(self, msg, priority):
+        self.__autopilot_active = bool(False)
+
+    def check_autopilot_status(self, msg: Joy):
+        """Checks if `LB` + `LT` are pressed (i.e. deadman switch + autopilot-using-behaviors mode)."""
+        self.__autopilot_active = msg.buttons[4] and msg.buttons[6]
+        # NOTE: On DirectInput mode, LB is msg.buttons[6]. On XInput mode, LB is msg.axes[2] (== -1.0).
+
+    def cmd_vel_callback(self, msg, priority: int):
         self._time_called[priority] = self.get_clock().now()
         pub = True
         for i in range(1, priority + 1):
-            if (self._time_called[priority] - self._time_called[i - 1]).nanoseconds / 1e9 < self._delay_sec:
+            if (
+                self._time_called[priority] - self._time_called[i - 1]
+            ).nanoseconds / 1e9 < self._delay_sec:
                 pub = False
                 break
         if pub:
-            self._cmd_vel_pub.publish(msg)
-            status = Int32()
-            status.data = priority
+            if self.__autopilot_active:
+                self._cmd_vel_pub.publish(msg)
+            elif priority == 0:
+                self._cmd_vel_pub.publish(msg)
+
+            status = Int32(data=priority)
             self._status_pub.publish(status)
 
+
 def main(args=None):
     rclpy.init(args=args)
     arbitration = Arbitration()
@@ -44,5 +70,6 @@ def main(args=None):
         arbitration.destroy_node()
         rclpy.shutdown()
 
-if __name__ == '__main__':
+
+if __name__ == "__main__":
     main()

racecar_gazebo/launch/spawn_2_racecars.launch.py

@@ -100,8 +100,8 @@ def launch_setup(context, *args, **kwargs):
 
     joystick = Node(
         package='joy',
-        executable='joy_node',
-        name='joy_node',
+        executable='game_controller_node',
+        name='joy',
         parameters=[{'deadzone': 0.2},
                     {'autorepeat_rate': 0.0},
                     {'coalesce_interval': 0.01}],
@@ -143,4 +143,4 @@ def generate_launch_description():
     ])
 
 if __name__ == '__main__':
-    generate_launch_description()
+    generate_launch_description()

racecar_gazebo/launch/spawn_racecar.launch.py

@@ -16,7 +16,7 @@ def launch_setup(context, *args, **kwargs):
 
     # Package Directories
     racecar_description = get_package_share_directory('racecar_description')
-    racecar_gazebo = get_package_share_directory('racecar_gazebo')
+    # racecar_gazebo = get_package_share_directory('racecar_gazebo')  # NOTE: Unused.
     racecar_navigation = get_package_share_directory('racecar_navigation')
 
     # Parse robot description from xacro
@@ -74,8 +74,8 @@ def launch_setup(context, *args, **kwargs):
 
     joystick = Node(
         package='joy',
-        executable='joy_node',
-        name='joy_node',
+        executable='game_controller_node',
+        name='joy',
         parameters=[{'deadzone': 0.2},
                     {'autorepeat_rate': 0.0},
                     {'coalesce_interval': 0.01}],
@@ -92,9 +92,9 @@ def launch_setup(context, *args, **kwargs):
     )
 
 
-    gaz_control = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([os.path.join(racecar_gazebo, 'launch', 'gazebo_control.launch.py')]),
-    )
+    # gaz_control = IncludeLaunchDescription(
+    #     PythonLaunchDescriptionSource([os.path.join(racecar_gazebo, 'launch', 'gazebo_control.launch.py')]),
+    # )  # NOTE: Does nothing.
 
     kalmanFilter = IncludeLaunchDescription(
                         PythonLaunchDescriptionSource([os.path.join(racecar_navigation, 'launch', 'kalmanFilter.launch.py')]),
@@ -110,7 +110,7 @@ def launch_setup(context, *args, **kwargs):
         cmd_vel_arb,
         joystick,
         teleop,
-        gaz_control,
+        # gaz_control,
         kalmanFilter
     ]