aaron/rover-ros2
1
0
Fork 0
mirror of https://github.com/SHC-ASTRA/rover-ros2.git synced 2026-02-11 09:20:40 +00:00
Code Issues Packages Projects Releases Wiki Activity

feat: make frames work properly, rviz is now accurate

Browse Source
This commit is contained in:
David
2025-10-15 02:31:47 -05:00
parent 89015ee7a5
commit a58f9b6ada
6 changed files with 68 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/core_gazebo/launch/core.gazebo.launch.py
View File

@@ -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,

2
src/core_rover_description/config/ros2_controllers.yaml
View File

@@ -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]

61
src/core_rover_description/config/rviz_basic_settings.rviz
View File

@@ -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: <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
X: 963
Y: 40
Width: 1052
X: 337
Y: 100

197
src/core_rover_description/launch/robot_state_publisher.launch.py
View File

@@ -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)

9
src/core_rover_description/urdf/core_rover.urdf
View File

@@ -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>

3
src/core_rover_description/urdf/ros2_control.xacro
View File

@@ -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"/>