From 44a88572942ac99d422c8bd8e722c7ef32a2be2d Mon Sep 17 00:00:00 2001 From: Michael Carroll Date: Wed, 19 Oct 2022 03:04:03 -0500 Subject: [PATCH] Fix diff_drive Signed-off-by: Michael Carroll --- .../{rviz => config}/diff_drive.rviz | 90 ++++++++++++++----- .../config/ros_gz_example_bridge.yaml | 20 +++++ .../launch/diff_drive.launch.py | 24 ++++- .../models/diff_drive/model.sdf | 1 + ros_gz_example_gazebo/worlds/diff_drive.sdf | 1 - 5 files changed, 112 insertions(+), 24 deletions(-) rename ros_gz_example_bringup/{rviz => config}/diff_drive.rviz (67%) diff --git a/ros_gz_example_bringup/rviz/diff_drive.rviz b/ros_gz_example_bringup/config/diff_drive.rviz similarity index 67% rename from ros_gz_example_bringup/rviz/diff_drive.rviz rename to ros_gz_example_bringup/config/diff_drive.rviz index 6a1723c..9c3eb9e 100644 --- a/ros_gz_example_bringup/rviz/diff_drive.rviz +++ b/ros_gz_example_bringup/config/diff_drive.rviz @@ -6,10 +6,9 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /LaserScan1 - - /LaserScan1/Status1 + - /RobotModel1 Splitter Ratio: 0.5 - Tree Height: 1079 + Tree Height: 549 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -27,7 +26,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: LaserScan + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -59,20 +58,20 @@ Visualization Manager: Channel Name: intensity Class: rviz_default_plugins/LaserScan Color: 255; 255; 255 - Color Transformer: Intensity + Color Transformer: "" Decay Time: 0 Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0 + Max Intensity: 4096 Min Color: 0; 0; 0 Min Intensity: 0 Name: LaserScan - Position Transformer: XYZ + Position Transformer: "" Selectable: true Size (Pixels): 3 Size (m): 0.009999999776482582 - Style: Points + Style: Flat Squares Topic: Depth: 5 Durability Policy: Volatile @@ -83,10 +82,61 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + caster: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + chassis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: diff_drive/lidar_link/gpu_lidar + Fixed Frame: chassis Frame Rate: 30 Name: root Tools: @@ -129,33 +179,33 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 43.2213249206543 + Distance: 10 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.38051652908325195 - Y: -1.564827799797058 - Z: 1.3577169179916382 + X: 0 + Y: 0 + Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6403982639312744 + Pitch: 0.785398006439209 Target Frame: Value: Orbit (rviz) - Yaw: 2.4303994178771973 + Yaw: 0.785398006439209 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1376 + Height: 846 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000239000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a00000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000006ac000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -164,6 +214,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2560 - X: 1440 - Y: 1147 + Width: 1200 + X: 74 + Y: 60 diff --git a/ros_gz_example_bringup/config/ros_gz_example_bridge.yaml b/ros_gz_example_bringup/config/ros_gz_example_bridge.yaml index 0c34f9d..e9aec00 100644 --- a/ros_gz_example_bringup/config/ros_gz_example_bridge.yaml +++ b/ros_gz_example_bringup/config/ros_gz_example_bridge.yaml @@ -14,4 +14,24 @@ ros_type_name: "sensor_msgs/msg/LaserScan" gz_type_name: "gz.msgs.LaserScan" direction: GZ_TO_ROS +- ros_topic_name: "/clock" + gz_topic_name: "/clock" + ros_type_name: "rosgraph_msgs/msg/clock" + gz_type_name: "gz.msgs.Clock" + direction: GZ_TO_ROS +- ros_topic_name: "/joint_states" + gz_topic_name: "/world/demo/model/diff_drive/joint_state" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + direction: GZ_TO_ROS +- ros_topic_name: "/tf" + gz_topic_name: "/model/diff_drive/pose" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: GZ_TO_ROS +- ros_topic_name: "/tf_static" + gz_topic_name: "/model/diff_drive/pose_static" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: GZ_TO_ROS diff --git a/ros_gz_example_bringup/launch/diff_drive.launch.py b/ros_gz_example_bringup/launch/diff_drive.launch.py index 4f79d93..f5bbb09 100644 --- a/ros_gz_example_bringup/launch/diff_drive.launch.py +++ b/ros_gz_example_bringup/launch/diff_drive.launch.py @@ -29,7 +29,7 @@ def generate_launch_description(): pkg_project_bringup = get_package_share_directory('ros_gz_example_bringup') pkg_project_gazebo = get_package_share_directory('ros_gz_example_gazebo') - + pkg_project_description = get_package_share_directory('ros_gz_example_description') pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') gz_sim = IncludeLaunchDescription( @@ -46,7 +46,7 @@ def generate_launch_description(): rviz = Node( package='rviz2', executable='rviz2', - arguments=['-d', os.path.join(pkg_project_bringup, 'rviz', 'diff_drive.rviz')], + arguments=['-d', os.path.join(pkg_project_bringup, 'config', 'diff_drive.rviz')], condition=IfCondition(LaunchConfiguration('rviz')) ) @@ -56,15 +56,33 @@ def generate_launch_description(): executable='parameter_bridge', parameters=[{ 'config_file': os.path.join(pkg_project_bringup, 'config', 'ros_gz_example_bridge.yaml'), - 'qos_overrides./model/diff_drive.subscriber.reliability': 'reliable', + 'qos_overrides./tf_static.publisher.durability': 'transient_local', }], output='screen' ) + sdf_file = os.path.join(pkg_project_description, 'models', 'diff_drive', 'model.sdf') + + with open(sdf_file, 'r') as infp: + robot_desc = infp.read() + + # Get the parser plugin convert sdf to urdf using robot_description topic + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[ + {'use_sim_time': True}, + {'robot_description': robot_desc}, + ] + ) + return LaunchDescription([ gz_sim, DeclareLaunchArgument('rviz', default_value='true', description='Open RViz.'), + robot_state_publisher, bridge, rviz ]) diff --git a/ros_gz_example_description/models/diff_drive/model.sdf b/ros_gz_example_description/models/diff_drive/model.sdf index 7ae6c6c..ffbc2a7 100644 --- a/ros_gz_example_description/models/diff_drive/model.sdf +++ b/ros_gz_example_description/models/diff_drive/model.sdf @@ -164,6 +164,7 @@ 0 0 0 0 0 0 scan + diff_drive/lidar_link 10 diff --git a/ros_gz_example_gazebo/worlds/diff_drive.sdf b/ros_gz_example_gazebo/worlds/diff_drive.sdf index b4c820b..48b48db 100644 --- a/ros_gz_example_gazebo/worlds/diff_drive.sdf +++ b/ros_gz_example_gazebo/worlds/diff_drive.sdf @@ -26,7 +26,6 @@ filename="gz-sim-user-commands-system" name="gz::sim::systems::UserCommands"> -