Added the ability to switch between cmd_vel control and induvidual motor control

toid_control/launch/toid.launch.py

@@ -35,18 +35,11 @@ def generate_launch_description():
         description="Whether to use mock controller" 
     )
 
-    # odom_broadcast = Node(
-    #     package='tf2_ros',
-    #     executable='static_transform_publisher',
-    #     name='map_to_odom_broadcaster',
-    #     arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'],
-    #     condition=IfCondition(LaunchConfiguration('visualize'))
-    # )
-
     odom_broadcast = Node(
         package='toid_odometry',
         executable='toid_odometry',
         name='map_to_odom_broadcaster',
+        emulate_tty=True,
         parameters=[{'mock_odom': use_mock}],
         condition=IfCondition(LaunchConfiguration('visualize'))
     )
@@ -57,6 +50,7 @@ def generate_launch_description():
         executable='robot_state_publisher',
         name='robot_state_publisher',
         output='screen',
+        emulate_tty=True,
         parameters=[{'robot_description': Command(['xacro ', default_model_path, ' use_mock:=', use_mock])}]
     )
 
@@ -64,20 +58,42 @@ def generate_launch_description():
         package='controller_manager',
         executable='ros2_control_node',
         output='screen',
-        parameters=[params]
+        emulate_tty=True,
+        parameters=[params],
+        arguments=['--ros-args', '--log-level', 'warn']
     )
 
     joint_state_broadcaster = Node(
         package='controller_manager',
         executable='spawner',
         output='screen',
-        arguments=["joint_state_broadcaster"]
+        emulate_tty=True,
+        arguments=[
+            "joint_state_broadcaster",
+            '--ros-args', '--log-level', 'warn'
+            ]
     )
 
+    velocity_controller = Node(
+        package='controller_manager',
+        executable='spawner',
+        output='screen',
+        emulate_tty=True,
+        arguments=[
+            "velocity_controller",
+            "--inactive",
+            "-p",
+            params,
+            '--ros-args', '--log-level', 'warn'
+            ],
+    )
+
+
     diffbot_base_controller = Node(
         package='controller_manager',
         executable='spawner',
         output='both',
+        emulate_tty=True,
         arguments=[
             "diffdrive_controller",
             "-p",
@@ -96,6 +112,7 @@ def generate_launch_description():
                 "--param enable_odom_tf:=true", 
                 "--param enable_odom_tf:=false"
             ),
+            '--ros-args', '--log-level', 'warn'
         ]
     )
 
@@ -104,7 +121,10 @@ def generate_launch_description():
         executable='rviz2',
         name='rviz2',
         output='screen',
-        arguments=['-d', default_rviz_config_path],
+        emulate_tty=True,
+        arguments=['-d', default_rviz_config_path, 
+                   '--ros-args', '--log-level', 'warn'
+                   ],
         condition=IfCondition(visualize)
     )
 
@@ -116,5 +136,6 @@ def generate_launch_description():
         controller_manager,
         joint_state_broadcaster,
         diffbot_base_controller,
+        velocity_controller,
         rviz_node
     ])

toid_control/package.xml

@@ -21,6 +21,7 @@
 
   <depend>controller_manager</depend>
   <depend>diff_drive_controller</depend>
+  <depend>forward_command_controller</depend>
   <depend>robot_state_publisher</depend>
   <depend>joint_state_broadcaster</depend>
 

toid_control/params/toid_general_params.yaml

@@ -6,6 +6,15 @@ controller_manager:
       type: diff_drive_controller/DiffDriveController
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
+    velocity_controller:
+      type: forward_command_controller/ForwardCommandController
+
+velocity_controller:
+  ros__parameters:
+    joints:
+     - drivewhl_l_joint
+     - drivewhl_r_joint
+    interface_name: velocity
 
 diffdrive_controller:
   ros__parameters: