diff --git a/src/core_gazebo/launch/core.gazebo.launch.py b/src/core_gazebo/launch/core.gazebo.launch.py index 746bf0c..97aa3c3 100644 --- a/src/core_gazebo/launch/core.gazebo.launch.py +++ b/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, diff --git a/src/core_rover_description/config/ros2_controllers.yaml b/src/core_rover_description/config/ros2_controllers.yaml index a0e158a..a58088a 100644 --- a/src/core_rover_description/config/ros2_controllers.yaml +++ b/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] diff --git a/src/core_rover_description/config/rviz_basic_settings.rviz b/src/core_rover_description/config/rviz_basic_settings.rviz index 8a27eae..6ce350b 100644 --- a/src/core_rover_description/config/rviz_basic_settings.rviz +++ b/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,25 +146,28 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - base_link: - averaging_bar: - {} - left_suspension_member: - bl_wheel: - {} - fl_wheel: - {} - right_suspension_member: - br_wheel: - {} - fr_wheel: + dummy_base_link: + base_link: + averaging_bar: {} + left_suspension_member: + bl_wheel: + {} + fl_wheel: + {} + right_suspension_member: + br_wheel: + {} + 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 - Y: 0.16643811762332916 - Z: -0.29218846559524536 + X: 0.006463103927671909 + Y: 0.03326062113046646 + 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: 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 - X: 963 - Y: 40 + Width: 1052 + X: 337 + Y: 100 diff --git a/src/core_rover_description/launch/robot_state_publisher.launch.py b/src/core_rover_description/launch/robot_state_publisher.launch.py index af917f6..18c470d 100644 --- a/src/core_rover_description/launch/robot_state_publisher.launch.py +++ b/src/core_rover_description/launch/robot_state_publisher.launch.py @@ -1,14 +1,5 @@ -#!/usr/bin/env python3 -""" -Launch RViz visualization for the mycobot robot. +# Robot state publisher launch file for core rover -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 - the mycobot robot in RViz, including: - - Robot state publisher for broadcasting transforms - - Joint state publisher for simulating joint movements - - RViz for visualization + # Set the path to this package. + pkg_share = FindPackageShare(package='core_rover_description').find('core_rover_description') - Returns: - 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') + # Launch configuration variables specific to simulation 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( - name='jsp_gui', - default_value='true', - choices=['true', 'false'], - description='Flag to enable joint_state_publisher_gui') + declare_urdf_model_path_cmd = DeclareLaunchArgument( + name='urdf_model', + default_value=os.path.join(pkg_share, 'urdf', 'core_rover_description.xacro'), + description='Absolute path to robot urdf file') 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([ - '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) + # Specify the actions # 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', - output='screen', - parameters=[{ - '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)) + parameters=[{'use_sim_time': use_sim_time, + 'robot_description': ParameterValue(Command(['xacro ', urdf_model]), value_type=str)}], + arguments=[urdf_model]) # 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) - - # Process the controller configuration before starting nodes - # ld.add_action(OpaqueFunction(function=process_ros2_controllers_config)) + ld = LaunchDescription() # 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) diff --git a/src/core_rover_description/urdf/core_rover.urdf b/src/core_rover_description/urdf/core_rover.urdf index 97f35a5..37d4393 100644 --- a/src/core_rover_description/urdf/core_rover.urdf +++ b/src/core_rover_description/urdf/core_rover.urdf @@ -5,6 +5,15 @@ + + + + + + + + + diff --git a/src/core_rover_description/urdf/ros2_control.xacro b/src/core_rover_description/urdf/ros2_control.xacro index 32ca293..a2a411b 100644 --- a/src/core_rover_description/urdf/ros2_control.xacro +++ b/src/core_rover_description/urdf/ros2_control.xacro @@ -5,6 +5,9 @@ gz_ros2_control/GazeboSimSystem + + +