Merge pull request #63 from clubcapra/add_twist_mux

Implemented twist_mux
clubcapra · Jun 22, 2024 · 0f207fd · 0f207fd
2 parents 09a9453 + 5c190c9
commit 0f207fd
Show file tree

Hide file tree

Showing 9 changed files with 35 additions and 15 deletions.
diff --git a/src/rove_navigation/config/twist_mux.yaml → src/rove_bringup/config/twist_mux.yaml b/src/rove_navigation/config/twist_mux.yaml → src/rove_bringup/config/twist_mux.yaml
@@ -1,11 +1,15 @@
-twist_mux:
-  ros_parameters:
+/twist_mux:
+  ros__parameters:
     topics:
       navigation:
-        topic: cmd_vel
+        topic: nav_vel
         timeout: 0.5
         priority: 10
       joystick:
-        topic: cmd_vel_joy
+        topic: joy_vel
         timeout: 0.5
         priority: 20
+      others:
+        topic: cmd_vel
+        timeout: 0.5
+        priority: 1
diff --git a/src/rove_bringup/launch/common.launch.py b/src/rove_bringup/launch/common.launch.py
@@ -97,12 +97,22 @@ def generate_launch_description():
             "use_slam3d": "true",
         }.items(),
     )
+
+    twist_mux = Node(
+      package='twist_mux',
+      executable='twist_mux',
+      output='screen',
+      parameters=[os.path.join(pkg_rove_bringup, 'config/twist_mux.yaml')],
+      remappings={
+          ('/cmd_vel_out', '/diff_drive_controller/cmd_vel_unstamped')
+      })
 
     return LaunchDescription([
             robot_state_publisher,
             robot_localization_node_local,
-            #robot_localization_node_global,
+            # robot_localization_node_global,
             #navsat_transform,
+            twist_mux,
             rviz,
             teleop,
             autonomy,

diff --git a/src/rove_bringup/launch/rove_controller_bluetooth.launch.py b/src/rove_bringup/launch/rove_controller_bluetooth.launch.py
@@ -25,6 +25,7 @@ def generate_launch_description():
             parameters=[teleop_joy_params_file],
             remappings=[
                 ('/joy', '/joy'),
+                ('/cmd_vel', '/joy_vel'),
             ],
         ),
         Node(

diff --git a/src/rove_bringup/launch/rove_controller_usb.launch.py b/src/rove_bringup/launch/rove_controller_usb.launch.py
@@ -25,6 +25,7 @@ def generate_launch_description():
             parameters=[teleop_joy_params_file],
             remappings=[
                 ('/joy', '/joy'),
+                ('/cmd_vel', '/joy_vel'),
             ],
         ),
         Node(

diff --git a/src/rove_bringup/package.xml b/src/rove_bringup/package.xml
@@ -9,12 +9,14 @@
 
   <build_depend>joy</build_depend>
   <build_depend>teleop_twist_joy</build_depend>
+  <build_depend>twist_mux</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>control_msgs</build_depend>
 
   <exec_depend>joy</exec_depend>
   <exec_depend>teleop_twist_joy</exec_depend>
+  <exec_depend>twist_mux</exec_depend>
   <exec_depend>control_msgs</exec_depend>
   <exec_depend>foxglove_bridge</exec_depend>
 

diff --git a/src/rove_description/config/default_bridge.yaml b/src/rove_description/config/default_bridge.yaml
@@ -6,7 +6,7 @@
   direction: GZ_TO_ROS
 
 # gz topic subscribed to by Tracked Vehicle plugin
-- ros_topic_name: "cmd_vel"
+- ros_topic_name: "diff_drive_controller/cmd_vel_unstamped"
   gz_topic_name: "cmd_vel"
   ros_type_name: "geometry_msgs/msg/Twist"
   gz_type_name: "gz.msgs.Twist"

diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro
@@ -44,6 +44,7 @@
     <!-- <xacro:rove_ros2_control/> -->
      <!-- Put these at -1 to mock the motors if working without the motors -->
     <xacro:odrive_control fl_id="21" rl_id="22" fr_id="23" rr_id="24"/>
+    <!-- <xacro:odrive_control fl_id="-1" rl_id="-1" fr_id="-1" rr_id="-1"/> -->
 
 
     <!-- Insert ovis on top of rove -->

diff --git a/src/rove_navigation/config/rove_nav_params.yaml b/src/rove_navigation/config/rove_nav_params.yaml
@@ -224,7 +224,6 @@ global_costmap:
         origin_z: 0.0
         z_resolution: 0.05
         z_voxels: 16
-        max_obstacle_height: 2.0
         unknown_threshold: 15
         mark_threshold: 0
         observation_sources: pointcloud
@@ -268,7 +267,6 @@ local_costmap:
       height: 3
       resolution: 0.05
       robot_radius: 0.22 # radius set and used, so no footprint points
-      resolution: 0.05
       plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
       obstacle_layer:
         plugin: "nav2_costmap_2d::ObstacleLayer"
@@ -310,7 +308,6 @@ local_costmap:
         origin_z: 0.0
         z_resolution: 0.05
         z_voxels: 16
-        max_obstacle_height: 2.0
         unknown_threshold: 15
         mark_threshold: 0
         observation_sources: pointcloud

diff --git a/src/rove_navigation/launch/navigation.launch.py b/src/rove_navigation/launch/navigation.launch.py
@@ -2,14 +2,13 @@
 
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription
+from launch.actions import IncludeLaunchDescription, GroupAction
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.substitutions import FindPackageShare
-
+from launch_ros.actions import SetRemap
 
 def generate_launch_description():
-
     pkg_rove_navigation = get_package_share_directory('rove_navigation')
 
     navigation_launch_path = PathJoinSubstitution(
@@ -24,9 +23,14 @@ def generate_launch_description():
         launch_arguments={
             'use_sim_time': 'true',
             'params_file': nav2_config_path
-        }.items()
+        }.items(),
     )
-
+    
     return LaunchDescription([
-        nav
+        GroupAction(
+            actions=[
+                SetRemap(src='cmd_vel', dst='nav_vel'),
+                nav,
+            ]
+        )
     ])