diff --git a/ros_gz_example_bringup/CMakeLists.txt b/ros_gz_example_bringup/CMakeLists.txt index 0ad1251..2d6b8e6 100644 --- a/ros_gz_example_bringup/CMakeLists.txt +++ b/ros_gz_example_bringup/CMakeLists.txt @@ -12,6 +12,8 @@ set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) find_package(gz-sim7 REQUIRED) set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR}) +find_package(sdformat_urdf) + # Install project launch files install( DIRECTORY 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..c0ae093 100644 --- a/ros_gz_example_bringup/config/ros_gz_example_bridge.yaml +++ b/ros_gz_example_bringup/config/ros_gz_example_bridge.yaml @@ -4,6 +4,11 @@ ros_type_name: "geometry_msgs/msg/Twist" gz_type_name: "gz.msgs.Twist" direction: ROS_TO_GZ +- 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: "/diff_drive/odometry" gz_topic_name: "/model/diff_drive/odometry" ros_type_name: "nav_msgs/msg/Odometry" @@ -14,4 +19,19 @@ ros_type_name: "sensor_msgs/msg/LaserScan" gz_type_name: "gz.msgs.LaserScan" direction: GZ_TO_ROS +- ros_topic_name: "/world/demo/model/diff_drive" + gz_topic_name: "/joint_state" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + direction: GZ_TO_ROS +- ros_topic_name: "/model/diff_drive/" + gz_topic_name: "/pose" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + direction: GZ_TO_ROS +- ros_topic_name: "/model/diff_drive/" + gz_topic_name: "/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..950082e 100644 --- a/ros_gz_example_bringup/launch/diff_drive.launch.py +++ b/ros_gz_example_bringup/launch/diff_drive.launch.py @@ -29,6 +29,12 @@ 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') + + sdf_file = os.path.join(pkg_project_description, 'models', 'diff_drive', 'model.sdf') + + with open(sdf_file, 'r') as infp: + robot_desc = infp.read() pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') @@ -42,12 +48,15 @@ def generate_launch_description(): ])}.items(), ) - # RViz - rviz = Node( - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(pkg_project_bringup, 'rviz', 'diff_drive.rviz')], - condition=IfCondition(LaunchConfiguration('rviz')) + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[ + {'use_sim_time': True}, + {'robot_description': robot_desc}, + ] ) # Bridge @@ -61,10 +70,20 @@ def generate_launch_description(): output='screen' ) + # RViz + rviz = Node( + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(pkg_project_bringup, 'rviz', 'diff_drive.rviz')], + condition=IfCondition(LaunchConfiguration('rviz')) + ) + + return LaunchDescription([ gz_sim, DeclareLaunchArgument('rviz', default_value='true', description='Open RViz.'), bridge, + robot_state_publisher, rviz ]) diff --git a/ros_gz_example_bringup/rviz/diff_drive.rviz b/ros_gz_example_bringup/rviz/diff_drive.rviz index 6a1723c..888efe9 100644 --- a/ros_gz_example_bringup/rviz/diff_drive.rviz +++ b/ros_gz_example_bringup/rviz/diff_drive.rviz @@ -6,10 +6,12 @@ Panels: Expanded: - /Global Options1 - /Status1 - - /LaserScan1 - - /LaserScan1/Status1 + - /Grid1 + - /RobotModel1 + - /RobotModel1/Status1 + - /RobotModel1/Links1 Splitter Ratio: 0.5 - Tree Height: 1079 + Tree Height: 555 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -27,7 +29,7 @@ Panels: Experimental: false Name: Time SyncMode: 0 - SyncSource: LaserScan + SyncSource: "" Visualization Manager: Class: "" Displays: @@ -50,43 +52,60 @@ Visualization Manager: Reference Frame: Value: true - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz_default_plugins/LaserScan - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: true - Invert Rainbow: false - Max Color: 255; 255; 255 - Max Intensity: 0 - Min Color: 0; 0; 0 - Min Intensity: 0 - Name: LaserScan - Position Transformer: XYZ - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Points - Topic: + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: Depth: 5 Durability Policy: Volatile - Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /diff_drive/scan - Use Fixed Frame: true - Use rainbow: true + 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: diff_drive + 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: diff_drive/odom Frame Rate: 30 Name: root Tools: @@ -129,33 +148,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: 000000ff00000000fd0000000400000000000002d8000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000000bd000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -164,6 +183,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 2560 - X: 1440 - Y: 1147 + Width: 1200 + X: 72 + Y: 60 diff --git a/template_workspace.yaml b/template_workspace.yaml index 1fd2112..390974c 100644 --- a/template_workspace.yaml +++ b/template_workspace.yaml @@ -8,4 +8,8 @@ repositories: ros_gz: type: git url: https://github.com/gazebosim/ros_gz.git + version: ros2 + sdformat_urdf: + type: git + url: https://github.com/ros/sdformat_urdf.git version: ros2 \ No newline at end of file