run with standard twist_mux, diff_drive, use TwistStamped everywhere

Author: ottojo

curt_mini/bringup/joystick.launch.py

@@ -13,7 +13,7 @@ def launch_joystick(context, *args, **kwargs):
     filepath_config_joy = os.path.join(
         get_package_share_directory(robot), "config", "joystick.yaml"
     )
-    print(filepath_config_joy)
+
     node_joy = Node(
         namespace="joy_teleop",
         package="joy_linux",
@@ -29,9 +29,6 @@ def launch_joystick(context, *args, **kwargs):
         executable="teleop_node",
         output="screen",
         name="teleop_twist_joy_node",
-        # remappings=[
-        #     ('/joy_teleop/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped')
-        # ],
         parameters=[filepath_config_joy],
     )
 

curt_mini/bringup/robot_base.launch.py

@@ -101,11 +101,12 @@ def launch_robot():
             "topic",
             "pub",
             "--rate",
-            "50",
+            "20",
             "--print",
             "0",
             "/zero_twist/cmd_vel",
-            "geometry_msgs/msg/Twist",
+            "geometry_msgs/msg/TwistStamped",
+            "{header: {stamp: now, frame_id: 'base_link'}}",
         ],
     )
 

curt_mini/bringup/start_controller.launch.py

@@ -1,62 +1,42 @@
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-import launch.substitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 from launch.substitutions import PathJoinSubstitution
-from launch_ros.parameter_descriptions import ParameterFile
-import typing
-
-
-class ResultingParameterFilePath(launch.substitution.Substitution):
-    """
-    Substitution which resolves to the path of the temporary file created by ParameterFile
-    """
-
-    def __init__(self, parameter_file: ParameterFile) -> None:
-        super().__init__()
-        self.__parameter_file = parameter_file
-
-    def perform(self, context: launch.launch_context.LaunchContext) -> typing.Text:
-        return self.__parameter_file.evaluate(context).absolute().as_posix()
 
 
 def generate_launch_description():
 
-    param_file = ResultingParameterFilePath(
-        ParameterFile(
-            PathJoinSubstitution(
-                [FindPackageShare("curt_mini"), "config", "ros2_control.yaml"]
-            ),
-            allow_substs=True,
-        )
+    param_file = PathJoinSubstitution(
+        [FindPackageShare("curt_mini"), "config", "ros2_control.yaml"]
     )
 
     robot_controller_spawner = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["--param-file", param_file, "skid_steer_controller"],
+        arguments=[
+            "--param-file",
+            param_file,
+            "--controller-ros-args",
+            "-r diff_drive_controller/odom:=/odometry/wheel",
+            "diff_drive_controller",
+        ],
         output="screen",
         remappings=[("odom", "odometry/wheel")],
     )
 
     joint_state_broadcaster_spawner = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["--param-file", param_file, "joint_state_broadcaster"],
+        arguments=[
+            "--param-file",
+            param_file,
+            "joint_state_broadcaster",
+        ],
         output="screen",
-        remappings=[("odom", "odometry/wheel")],
-    )
-
-    odom_topic_argument = DeclareLaunchArgument(
-        "odom_topic",
-        default_value="/odometry/imu",
-        description="Odometry topic used as feedback for closed loop angular twist base controller",
     )
 
     return LaunchDescription(
         [
-            odom_topic_argument,
             joint_state_broadcaster_spawner,
             robot_controller_spawner,
         ]

curt_mini/config/joystick.yaml

@@ -57,6 +57,7 @@ joy_teleop/teleop_twist_joy_node:
       yaw: 3.6
     enable_button: 4
     enable_turbo_button: 5
+    publish_stamped_twist: true
 joy_teleop/joy_node:
   ros__parameters:
     deadzone: 0.1

curt_mini/config/ros2_control.yaml

@@ -3,8 +3,8 @@ controller_manager:
     update_rate: 100
     use_sim_time: false
 
-    skid_steer_controller:
-      type: skid_steer_controller/SkidSteerController
+    diff_drive_controller:
+      type: diff_drive_controller/DiffDriveController
 
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
@@ -13,69 +13,45 @@ joint_state_broadcaster:
   ros__parameters:
     use_sim_time: false
 
-skid_steer_controller:
+diff_drive_controller:
   ros__parameters:
-    use_sim_time: false
-
     left_wheel_names: ["front_left_motor", "back_left_motor"]
     right_wheel_names: ["front_right_motor", "back_right_motor"]
 
     wheel_separation: 0.44
-    wheels_per_side: 1  # actually 2, but both are controlled by 1 signal
     wheel_radius: 0.13
 
     wheel_separation_multiplier: 1.0
-    right_wheel_radius_multiplier: 1.0
     left_wheel_radius_multiplier: 1.0
+    right_wheel_radius_multiplier: 1.0
 
-    publish_rate: 50.0
     odom_frame_id: odom
     base_frame_id: base_link
+    pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+    twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
 
+    position_feedback: true
     open_loop: false
     enable_odom_tf: false
 
-    cmd_vel_timeout: 0.25
-    #publish_limited_velocity: true
-    use_stamped_vel: false
-    #velocity_rolling_window_size: 10
-
-    # Preserve turning radius when limiting speed/acceleration/jerk
-    preserve_turning_radius: true
-
-    # Publish limited velocity
-    publish_cmd: true
+    cmd_vel_timeout: 0.25 # seconds
+    publish_limited_velocity: true
+    velocity_rolling_window_size: 10
 
-    # Publish wheel data
-    publish_wheel_data: true
-
-    linear.x.has_velocity_limits: true
-    linear.x.min_velocity: -1.5
     linear.x.max_velocity: 1.5
-    linear.x.has_acceleration_limits: true
-    linear.x.min_acceleration: -2.5
+    linear.x.min_velocity: -1.5
     linear.x.max_acceleration: 2.5
+    linear.x.max_deceleration: -2.5
+    linear.x.max_acceleration_reverse: 2.5
+    linear.x.max_deceleration_reverse: -2.5
+    linear.x.max_jerk: .NAN
+    linear.x.min_jerk: .NAN
 
-    angular.z.has_velocity_limits: true
-    angular.z.min_velocity: -12.0
     angular.z.max_velocity: 12.0
-    angular.z.has_acceleration_limits: true
-    angular.z.min_acceleration: -15.0
+    angular.z.min_velocity: -12.0
     angular.z.max_acceleration: 15.0
-
-    angular_vel_pid.p: 0.0
-    angular_vel_pid.i: 1.0
-    angular_vel_pid.d: 0.0
-    angular_vel_pid.i_clamp_max: 0.7
-    angular_vel_pid.i_clamp_min: -0.7
-    angular_vel_pid.antiwindup: true
-    angular_vel_pid.max_time_diff: 1.0
-    control_loop_odometry_topic: "$(var odom_topic)"
-    filter_odometry: true
-    cut_off_frequency: 0.5
-    linear_vel_pid.p: 0.0
-    linear_vel_pid.i: 0.5
-    linear_vel_pid.d: 0.0
-    linear_vel_pid.i_clamp_max: 0.5
-    linear_vel_pid.i_clamp_min: -0.5
-    linear_vel_pid.antiwindup: true
+    angular.z.max_deceleration: -15.0
+    angular.z.max_acceleration_reverse: 15.0
+    angular.z.max_deceleration_reverse: -15.0
+    angular.z.max_jerk: .NAN
+    angular.z.min_jerk: .NAN

curt_mini/config/twist_mux.yaml

@@ -1,19 +1,16 @@
 twist_mux:
   ros__parameters:
-    output_stamped: false
+    use_stamped: true
     topics:
       navigation:
         topic: cmd_vel
         priority: 20
         timeout: 0.1
-        stamped : false
       joystick:
         topic: joy_teleop/cmd_vel
         priority: 50
         timeout: 0.1
-        stamped : false
       zero_twist:
         topic: zero_twist/cmd_vel
         priority: 10
         timeout: 0.1
-        stamped : false

curt_mini/package.xml

@@ -15,13 +15,8 @@
   <depend>robot_state_publisher</depend>
   <depend>rclcpp</depend>
   <depend>rclpy</depend>
-  <depend>velodyne_description</depend>
-  <depend>velodyne</depend>
   <depend>xacro</depend>
-  <depend>ouster_ros</depend>
-  <depend>realsense2_camera</depend>
-  <depend>livox_ros2_driver</depend>
-  <depend>rslidar_sdk</depend>
+  <exec_depend>twist_mux</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

ipa_ros2_control/launch/ros2_control.launch.py

@@ -8,23 +8,18 @@
 from launch_ros.actions import Node
 from launch.actions import OpaqueFunction
 
+
 def launch_ros2_control(context, *args, **kwargs):
     # initialize arguments
-    robot = LaunchConfiguration('robot')
-    gear_ratio = LaunchConfiguration('gear_ratio')
-    interface = LaunchConfiguration('interface')
+    robot = "curt_mini"
 
-    robot_dir = get_package_share_directory(robot.perform(context))
-    
-    ros2_control_yaml_path = os.path.join(robot_dir, 'config', 'ros2_control.yaml')
+    robot_dir = get_package_share_directory(robot)
 
-    urdf_path = os.path.join(robot_dir, 'models', robot.perform(context), 'robot.urdf.xacro')
+    ros2_control_yaml_path = os.path.join(robot_dir, "config", "ros2_control.yaml")
 
-    robot_description = Command(['xacro ', urdf_path, ' gear_ratio:=', gear_ratio, ' interface:=', interface])
-    
     controller_manager_node = Node(
-        package='controller_manager',
-        executable='ros2_control_node',
+        package="controller_manager",
+        executable="ros2_control_node",
         parameters=[
             ros2_control_yaml_path,
         ],
@@ -35,47 +30,21 @@ def launch_ros2_control(context, *args, **kwargs):
     )
 
     md80_manager_node = Node(
-        package='candle_ros2',
-        executable='candle_ros2_node',
+        package="candle_ros2",
+        executable="candle_ros2_node",
         output={
             "stdout": "screen",
             "stderr": "screen",
         },
         arguments=["USB", "1M"],
     )
-    return [controller_manager_node,
-            md80_manager_node]
+    return [controller_manager_node, md80_manager_node]
+
 
 def generate_launch_description():
 
     declared_arguments = []
 
-    # robot argument
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            'robot',
-            default_value='curt_mini',
-            description="Set the robot.",
-            choices=['curt_mvp', 'curt_mini'],
-        )
-    )
-
-    # set the interface for the ethercat communication
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            'interface',
-            default_value='enp0s31f6',
-            description='Ethercat interface to the motor controllers',
-        )
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_ros2_control)]
     )
-
-    # set the gear_ratio
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            'gear_ratio',
-            default_value='1',
-            description='Set the gear ratio of the motors',
-        )
-    )
-
-    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_ros2_control)])