feat: make frames work properly, rviz is now accurate

src/core_gazebo/launch/core.gazebo.launch.py

@@ -167,7 +167,7 @@ def generate_launch_description():
     # Include Robot State Publisher launch file if enabled
     robot_state_publisher_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource([
-            os.path.join(pkg_share_description, 'launch', 'display.launch.py')
+            os.path.join(pkg_share_description, 'launch', 'robot_state_publisher.launch.py')
         ]),
         launch_arguments={
             'jsp_gui': jsp_gui,

src/core_rover_description/config/ros2_controllers.yaml

@@ -18,7 +18,7 @@ diff_controller:
     wheels_per_side: 2
     wheel_radius: 0.171
 
-    base_frame_id: "base_link"
+    base_frame_id: "dummy_base_link"
     pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
     twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
 

src/core_rover_description/config/rviz_basic_settings.rviz

@@ -10,7 +10,7 @@ Panels:
         - /TF1
         - /TF1/Frames1
       Splitter Ratio: 0.5
-    Tree Height: 895
+    Tree Height: 782
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -83,6 +83,10 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        dummy_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         fl_wheel:
           Alpha: 1
           Show Axes: false
@@ -115,7 +119,7 @@ Visualization Manager:
       Enabled: true
       Frame Timeout: 15
       Frames:
-        All Enabled: false
+        All Enabled: true
         averaging_bar:
           Value: true
         base_link:
@@ -124,12 +128,16 @@ Visualization Manager:
           Value: true
         br_wheel:
           Value: true
+        dummy_base_link:
+          Value: true
         fl_wheel:
           Value: true
         fr_wheel:
           Value: true
         left_suspension_member:
           Value: true
+        odom:
+          Value: true
         right_suspension_member:
           Value: true
       Marker Scale: 1
@@ -138,6 +146,7 @@ Visualization Manager:
       Show Axes: true
       Show Names: true
       Tree:
+        dummy_base_link:
           base_link:
             averaging_bar:
               {}
@@ -151,12 +160,14 @@ Visualization Manager:
                 {}
               fr_wheel:
                 {}
+        odom:
+          {}
       Update Interval: 0
       Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
-    Fixed Frame: base_link
+    Fixed Frame: odom
     Frame Rate: 30
   Name: root
   Tools:
@@ -206,32 +217,32 @@ Visualization Manager:
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0.1387547254562378
+        X: 0.006463103927671909
-        Y: 0.16643811762332916
+        Y: 0.03326062113046646
-        Z: -0.29218846559524536
+        Z: -0.009066490456461906
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4853976368904114
+      Pitch: 0.3403971195220947
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.9253984689712524
+      Yaw: 0.9403947591781616
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1118
+  Height: 1005
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000015600000408fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000408000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000408fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000408000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002570000040800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000397fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000397000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000408fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000408000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002c00000039700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: false
-  Width: 947
+  Width: 1052
-  X: 963
+  X: 337
-  Y: 40
+  Y: 100

src/core_rover_description/launch/robot_state_publisher.launch.py

@@ -1,14 +1,5 @@
-#!/usr/bin/env python3
+# Robot state publisher launch file for core rover
-"""
-Launch RViz visualization for the mycobot robot.
 
-This launch file sets up the complete visualization environment for the mycobot robot,
-including robot state publisher, joint state publisher, and RViz2. It handles loading
-and processing of URDF/XACRO files and controller configurations.
-
-:author: Addison Sears-Collins
-:date: November 15, 2024
-"""
 import os
 from pathlib import Path
 from launch import LaunchDescription
@@ -19,192 +10,47 @@ from launch_ros.actions import Node
 from launch_ros.parameter_descriptions import ParameterValue
 from launch_ros.substitutions import FindPackageShare
 
-
-def process_ros2_controllers_config(context):
-    """Process the ROS 2 controller configuration yaml file before loading the URDF.
-
-    This function reads a template configuration file, replaces placeholder values
-    with actual configuration, and writes the processed file to both source and
-    install directories.
-
-    Args:
-        context: Launch context containing configuration values
-
-    Returns:
-        list: Empty list as required by OpaqueFunction
-    """
-
-    # Get the configuration values
-    prefix = LaunchConfiguration('prefix').perform(context)
-    flange_link = LaunchConfiguration('flange_link').perform(context)
-    robot_name = LaunchConfiguration('robot_name').perform(context)
-
-    home = str(Path.home())
-
-    # Define both source and install paths
-    src_config_path = os.path.join(
-        home,
-        'ros2_ws/src/mycobot_ros2/mycobot_moveit_config/config',
-        robot_name
-    )
-    install_config_path = os.path.join(
-        home,
-        'ros2_ws/install/mycobot_moveit_config/share/mycobot_moveit_config/config',
-        robot_name
-    )
-
-    # Read from source template
-    template_path = os.path.join(src_config_path, 'ros2_controllers_template.yaml')
-    with open(template_path, 'r', encoding='utf-8') as file:
-        template_content = file.read()
-
-    # Create processed content (leaving template untouched)
-    processed_content = template_content.replace('${prefix}', prefix)
-    processed_content = processed_content.replace('${flange_link}', flange_link)
-
-    # Write processed content to both source and install directories
-    for config_path in [src_config_path, install_config_path]:
-        os.makedirs(config_path, exist_ok=True)
-        output_path = os.path.join(config_path, 'ros2_controllers.yaml')
-        with open(output_path, 'w', encoding='utf-8') as file:
-            file.write(processed_content)
-
-    return []
-
-
-# Define the arguments for the XACRO file
-ARGUMENTS = [
-    DeclareLaunchArgument('robot_name', default_value='mycobot_280',
-                          description='Name of the robot'),
-    DeclareLaunchArgument('prefix', default_value='',
-                          description='Prefix for robot joints and links'),
-    DeclareLaunchArgument('add_world', default_value='true',
-                          choices=['true', 'false'],
-                          description='Whether to add world link'),
-    DeclareLaunchArgument('base_link', default_value='base_link',
-                          description='Name of the base link'),
-    DeclareLaunchArgument('base_type', default_value='g_shape',
-                          description='Type of the base'),
-    DeclareLaunchArgument('flange_link', default_value='link6_flange',
-                          description='Name of the flange link'),
-    DeclareLaunchArgument('gripper_type', default_value='adaptive_gripper',
-                          description='Type of the gripper'),
-    DeclareLaunchArgument('use_camera', default_value='false',
-                          choices=['true', 'false'],
-                          description='Whether to use the RGBD Gazebo plugin for point cloud'),
-    DeclareLaunchArgument('use_gazebo', default_value='false',
-                          choices=['true', 'false'],
-                          description='Whether to use Gazebo simulation'),
-    DeclareLaunchArgument('use_gripper', default_value='true',
-                          choices=['true', 'false'],
-                          description='Whether to attach a gripper')
-]
-
-
 def generate_launch_description():
-    """Generate the launch description for the mycobot robot visualization.
 
-    This function sets up all necessary nodes and parameters for visualizing
+    # Set the path to this package.
-    the mycobot robot in RViz, including:
+    pkg_share = FindPackageShare(package='core_rover_description').find('core_rover_description')
-    - Robot state publisher for broadcasting transforms
-    - Joint state publisher for simulating joint movements
-    - RViz for visualization
 
-    Returns:
+    # Launch configuration variables specific to simulation
-        LaunchDescription: Complete launch description for the visualization setup
-    """
-    # Define filenames
-    urdf_package = 'core_rover_description'
-    urdf_filename = 'core_rover_description.xacro'
-    rviz_config_filename = 'rviz_basic_settings.rviz'
-
-    # Set paths to important files
-    pkg_share_description = FindPackageShare(urdf_package)
-    default_urdf_model_path = PathJoinSubstitution(
-        [pkg_share_description, 'urdf', urdf_filename])
-    default_rviz_config_path = PathJoinSubstitution(
-        [pkg_share_description, 'config', rviz_config_filename])
-
-    # Launch configuration variables
-    jsp_gui = LaunchConfiguration('jsp_gui')
-    rviz_config_file = LaunchConfiguration('rviz_config_file')
     urdf_model = LaunchConfiguration('urdf_model')
-    use_jsp = LaunchConfiguration('use_jsp')
+    rviz_config_file = LaunchConfiguration('rviz_config_file')
     use_rviz = LaunchConfiguration('use_rviz')
     use_sim_time = LaunchConfiguration('use_sim_time')
 
     # Declare the launch arguments
-    declare_jsp_gui_cmd = DeclareLaunchArgument(
+    declare_urdf_model_path_cmd = DeclareLaunchArgument(
-        name='jsp_gui',
+        name='urdf_model',
-        default_value='true',
+        default_value=os.path.join(pkg_share, 'urdf', 'core_rover_description.xacro'),
-        choices=['true', 'false'],
+        description='Absolute path to robot urdf file')
-        description='Flag to enable joint_state_publisher_gui')
 
     declare_rviz_config_file_cmd = DeclareLaunchArgument(
         name='rviz_config_file',
-        default_value=default_rviz_config_path,
+        default_value=os.path.join(pkg_share, 'config', 'rviz_basic_settings.rviz'),
         description='Full path to the RVIZ config file to use')
 
-    declare_urdf_model_path_cmd = DeclareLaunchArgument(
-        name='urdf_model',
-        default_value=default_urdf_model_path,
-        description='Absolute path to robot urdf file')
-
-    declare_use_jsp_cmd = DeclareLaunchArgument(
-        name='use_jsp',
-        default_value='false',
-        choices=['true', 'false'],
-        description='Enable the joint state publisher')
-
     declare_use_rviz_cmd = DeclareLaunchArgument(
         name='use_rviz',
-        default_value='true',
+        default_value='True',
         description='Whether to start RVIZ')
 
     declare_use_sim_time_cmd = DeclareLaunchArgument(
         name='use_sim_time',
-        default_value='false',
+        default_value='True',
         description='Use simulation (Gazebo) clock if true')
 
-    robot_description_content = ParameterValue(Command([
+    # Specify the actions
-        'xacro', ' ', urdf_model, ' ',
-        'robot_name:=', LaunchConfiguration('robot_name'), ' ',
-        'prefix:=', LaunchConfiguration('prefix'), ' ',
-        'add_world:=', LaunchConfiguration('add_world'), ' ',
-        'base_link:=', LaunchConfiguration('base_link'), ' ',
-        'base_type:=', LaunchConfiguration('base_type'), ' ',
-        'flange_link:=', LaunchConfiguration('flange_link'), ' ',
-        'gripper_type:=', LaunchConfiguration('gripper_type'), ' ',
-        'use_camera:=', LaunchConfiguration('use_camera'), ' ',
-        'use_gazebo:=', LaunchConfiguration('use_gazebo'), ' ',
-        'use_gripper:=', LaunchConfiguration('use_gripper')
-    ]), value_type=str)
 
     # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
     start_robot_state_publisher_cmd = Node(
         package='robot_state_publisher',
         executable='robot_state_publisher',
-        name='robot_state_publisher',
+        parameters=[{'use_sim_time': use_sim_time,
-        output='screen',
+                    'robot_description': ParameterValue(Command(['xacro ', urdf_model]), value_type=str)}],
-        parameters=[{
+        arguments=[urdf_model])
-            'use_sim_time': use_sim_time,
-            'robot_description': robot_description_content}])
-
-    # Publish the joint state values for the non-fixed joints in the URDF file.
-    start_joint_state_publisher_cmd = Node(
-        package='joint_state_publisher',
-        executable='joint_state_publisher',
-        name='joint_state_publisher',
-        parameters=[{'use_sim_time': use_sim_time}],
-        condition=IfCondition(use_jsp))
-
-    # Depending on gui parameter, either launch joint_state_publisher or joint_state_publisher_gui
-    start_joint_state_publisher_gui_cmd = Node(
-        package='joint_state_publisher_gui',
-        executable='joint_state_publisher_gui',
-        name='joint_state_publisher_gui',
-        parameters=[{'use_sim_time': use_sim_time}],
-        condition=IfCondition(jsp_gui))
 
     # Launch RViz
     start_rviz_cmd = Node(
@@ -216,22 +62,15 @@ def generate_launch_description():
         parameters=[{'use_sim_time': use_sim_time}])
 
     # Create the launch description and populate
-    ld = LaunchDescription(ARGUMENTS)
+    ld = LaunchDescription()
-
-    # Process the controller configuration before starting nodes
-    # ld.add_action(OpaqueFunction(function=process_ros2_controllers_config))
 
     # Declare the launch options
-    ld.add_action(declare_jsp_gui_cmd)
-    ld.add_action(declare_rviz_config_file_cmd)
     ld.add_action(declare_urdf_model_path_cmd)
-    ld.add_action(declare_use_jsp_cmd) 
+    ld.add_action(declare_rviz_config_file_cmd)
     ld.add_action(declare_use_rviz_cmd)
     ld.add_action(declare_use_sim_time_cmd)
 
     # Add any actions
-    ld.add_action(start_joint_state_publisher_cmd)
-    ld.add_action(start_joint_state_publisher_gui_cmd)
     ld.add_action(start_robot_state_publisher_cmd)
     ld.add_action(start_rviz_cmd)
 

src/core_rover_description/urdf/core_rover.urdf

@@ -5,6 +5,15 @@
 <robot
   name="core_rover_description">
 
+  <link name="dummy_base_link"/>
+
+  <joint name="dummy_base_link_joint" type="fixed">
+    <parent link="dummy_base_link"/>
+    <child link="base_link"/>
+    <origin xyz="0 0 0.35" rpy="0 0 1.5707963267948966"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
   <link
     name="base_link">
     <inertial>

src/core_rover_description/urdf/ros2_control.xacro

@@ -5,6 +5,9 @@
         <hardware>
             <plugin>gz_ros2_control/GazeboSimSystem</plugin>
         </hardware>
+        <joint name="left_suspension_joint">
+            <state_interface name="position"/>
+        </joint>
         <joint name="bl_wheel_joint">
             <command_interface name="velocity"/>
             <state_interface name="velocity"/>