From 52ee43e7f9e9cac02a2303747efc45f577fdbde5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 13 Dec 2024 19:23:38 +0100 Subject: [PATCH 1/3] Refactor imu demo (#637) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Refactor imu demo Signed-off-by: Carlos Agüero Signed-off-by: Alejandro Hernández Cordero --- ros_gz_sim_demos/README.md | 2 +- ros_gz_sim_demos/config/imu.yaml | 6 ++ ros_gz_sim_demos/launch/imu.launch.py | 78 --------------- ros_gz_sim_demos/launch/imu.launch.xml | 13 +++ ros_gz_sim_demos/package.xml | 1 + ros_gz_sim_demos/rviz/imu.rviz | 127 ++++++++++++++++--------- 6 files changed, 102 insertions(+), 125 deletions(-) create mode 100644 ros_gz_sim_demos/config/imu.yaml delete mode 100644 ros_gz_sim_demos/launch/imu.launch.py create mode 100644 ros_gz_sim_demos/launch/imu.launch.xml diff --git a/ros_gz_sim_demos/README.md b/ros_gz_sim_demos/README.md index cfd876ce..a6ab1cd4 100644 --- a/ros_gz_sim_demos/README.md +++ b/ros_gz_sim_demos/README.md @@ -116,7 +116,7 @@ Using Gazebo Sim plugin: Publishes IMU readings. - ros2 launch ros_gz_sim_demos imu.launch.py + ros2 launch ros_gz_sim_demos imu.launch.xml ![](images/imu_demo.png) diff --git a/ros_gz_sim_demos/config/imu.yaml b/ros_gz_sim_demos/config/imu.yaml new file mode 100644 index 00000000..9a3ddbcd --- /dev/null +++ b/ros_gz_sim_demos/config/imu.yaml @@ -0,0 +1,6 @@ +# IMU configuration. +- topic_name: "/imu" + ros_type_name: "sensor_msgs/msg/Imu" + gz_type_name: "gz.msgs.IMU" + lazy: true + direction: GZ_TO_ROS diff --git a/ros_gz_sim_demos/launch/imu.launch.py b/ros_gz_sim_demos/launch/imu.launch.py deleted file mode 100644 index cc1bf9e9..00000000 --- a/ros_gz_sim_demos/launch/imu.launch.py +++ /dev/null @@ -1,78 +0,0 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration - -from launch_ros.actions import Node - - -def generate_launch_description(): - - pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') - - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), - launch_arguments={ - 'gz_args': '-r sensors.sdf' - }.items(), - ) - - # RQt - rqt = Node( - package='rqt_topic', - executable='rqt_topic', - arguments=['-t'], - condition=IfCondition(LaunchConfiguration('rqt')) - ) - - # RViz - # FIXME: Add once there's an IMU display for RViz2 - # pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos') - # rviz = Node( - # package='rviz2', - # executable='rviz2', - # # arguments=['-d', os.path.join(pkg_ros_gz_sim_demos, 'rviz', 'imu.rviz')], - # condition=IfCondition(LaunchConfiguration('rviz')) - # ) - - # Bridge - bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=['/imu@sensor_msgs/msg/Imu@gz.msgs.IMU'], - output='screen' - ) - - return LaunchDescription([ - gz_sim, - DeclareLaunchArgument( - 'rqt', default_value='true', description='Open RQt.' - ), - DeclareLaunchArgument( - 'rviz', default_value='true', description='Open RViz.' - ), - bridge, - rqt - # rviz - ]) diff --git a/ros_gz_sim_demos/launch/imu.launch.xml b/ros_gz_sim_demos/launch/imu.launch.xml new file mode 100644 index 00000000..d7ef727c --- /dev/null +++ b/ros_gz_sim_demos/launch/imu.launch.xml @@ -0,0 +1,13 @@ + + + + + + + diff --git a/ros_gz_sim_demos/package.xml b/ros_gz_sim_demos/package.xml index 6ef1de85..414af4fe 100644 --- a/ros_gz_sim_demos/package.xml +++ b/ros_gz_sim_demos/package.xml @@ -22,6 +22,7 @@ rqt_image_view rqt_plot rqt_topic + rviz_imu_plugin rviz2 sdformat_urdf xacro diff --git a/ros_gz_sim_demos/rviz/imu.rviz b/ros_gz_sim_demos/rviz/imu.rviz index 0169390a..2d825af4 100644 --- a/ros_gz_sim_demos/rviz/imu.rviz +++ b/ros_gz_sim_demos/rviz/imu.rviz @@ -1,43 +1,41 @@ Panels: - - Class: rviz/Displays + - Class: rviz_common/Displays Help Height: 78 Name: Displays Property Tree Widget: Expanded: - /Global Options1 + - /Status1 + - /Grid1 - /Imu1 - - /Imu1/Status1 + - /Imu1/Axes properties1 + - /Imu1/Acceleration properties1 Splitter Ratio: 0.5 - Tree Height: 352 - - Class: rviz/Selection + Tree Height: 555 + - Class: rviz_common/Selection Name: Selection - - Class: rviz/Tool Properties + - Class: rviz_common/Tool Properties Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 + - /2D Goal Pose1 - /Publish Point1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views + - Class: rviz_common/Views Expanded: - /Current View1 Name: Views Splitter Ratio: 0.5 - - Class: rviz/Time + - Class: rviz_common/Time Experimental: false Name: Time SyncMode: 0 SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: - Alpha: 0.5 Cell Size: 1 - Class: rviz/Grid + Class: rviz_default_plugins/Grid Color: 160; 160; 164 Enabled: true Line Style: @@ -53,53 +51,90 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Alpha: 1 - Class: rviz_plugin_tutorials/Imu - Color: 204; 51; 204 + - Acceleration properties: + Acc. vector alpha: 1 + Acc. vector color: 255; 0; 0 + Acc. vector scale: 1 + Derotate acceleration: true + Enable acceleration: true + Axes properties: + Axes scale: 1 + Enable axes: true + Box properties: + Box alpha: 1 + Box color: 255; 0; 0 + Enable box: false + x_scale: 1 + y_scale: 1 + z_scale: 1 + Class: rviz_imu_plugin/Imu Enabled: true - History Length: 1 Name: Imu - Topic: /imu - Unreliable: false + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /imu Value: true + fixed_frame_orientation: true Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: sensors/sensors_box/link/imu + Fixed Frame: sensors_box/link/imu Frame Rate: 30 Name: root Tools: - - Class: rviz/Interact + - Class: rviz_default_plugins/Interact Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint Single click: true - Topic: /clicked_point + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: - Class: rviz/Orbit - Distance: 33.22251510620117 + Class: rviz_default_plugins/Orbit + Distance: 29.85377311706543 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -1.7206333875656128 - Y: -0.9246708154678345 - Z: 1.870512843132019 + X: 0 + Y: 0 + Z: 0 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -113,10 +148,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 575 + Height: 846 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000191000001e9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001e9000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003d70000003efc0100000002fb0000000800540069006d00650000000000000003d70000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000001a2000001e900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -125,6 +160,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 825 - X: 751 - Y: 92 + Width: 1200 + X: 2219 + Y: 69 From e1f578ae2692f4fccbcf015a5fac6985ae687273 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 13 Dec 2024 19:55:47 +0100 Subject: [PATCH 2/3] Refactor magnetometer demo (#638) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim_demos/README.md | 2 +- ros_gz_sim_demos/config/magnetometer.yaml | 6 ++ .../launch/magnetometer.launch.py | 63 ------------------- .../launch/magnetometer.launch.xml | 12 ++++ 4 files changed, 19 insertions(+), 64 deletions(-) create mode 100644 ros_gz_sim_demos/config/magnetometer.yaml delete mode 100644 ros_gz_sim_demos/launch/magnetometer.launch.py create mode 100644 ros_gz_sim_demos/launch/magnetometer.launch.xml diff --git a/ros_gz_sim_demos/README.md b/ros_gz_sim_demos/README.md index a6ab1cd4..9f1b75a6 100644 --- a/ros_gz_sim_demos/README.md +++ b/ros_gz_sim_demos/README.md @@ -126,7 +126,7 @@ Publishes IMU readings. Publishes magnetic field readings. - ros2 launch ros_gz_sim_demos magnetometer.launch.py + ros2 launch ros_gz_sim_demos magnetometer.launch.xml ![](images/magnetometer_demo.png) diff --git a/ros_gz_sim_demos/config/magnetometer.yaml b/ros_gz_sim_demos/config/magnetometer.yaml new file mode 100644 index 00000000..e5d38d2e --- /dev/null +++ b/ros_gz_sim_demos/config/magnetometer.yaml @@ -0,0 +1,6 @@ +# Magnetometer configuration. +- topic_name: "/magnetometer" + ros_type_name: "sensor_msgs/msg/MagneticField" + gz_type_name: "gz.msgs.Magnetometer" + lazy: true + direction: GZ_TO_ROS diff --git a/ros_gz_sim_demos/launch/magnetometer.launch.py b/ros_gz_sim_demos/launch/magnetometer.launch.py deleted file mode 100644 index e35b8487..00000000 --- a/ros_gz_sim_demos/launch/magnetometer.launch.py +++ /dev/null @@ -1,63 +0,0 @@ -# Copyright 2019 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument -from launch.actions import IncludeLaunchDescription -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration - -from launch_ros.actions import Node - - -def generate_launch_description(): - - pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') - - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), - launch_arguments={ - 'gz_args': '-r sensors.sdf' - }.items(), - ) - - # RQt - rqt = Node( - package='rqt_topic', - executable='rqt_topic', - arguments=['-t'], - condition=IfCondition(LaunchConfiguration('rqt')) - ) - - # Bridge - bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=['/magnetometer@sensor_msgs/msg/MagneticField@gz.msgs.Magnetometer'], - output='screen' - ) - - return LaunchDescription([ - gz_sim, - DeclareLaunchArgument('rqt', default_value='true', - description='Open RQt.'), - bridge, - rqt - ]) diff --git a/ros_gz_sim_demos/launch/magnetometer.launch.xml b/ros_gz_sim_demos/launch/magnetometer.launch.xml new file mode 100644 index 00000000..10639b43 --- /dev/null +++ b/ros_gz_sim_demos/launch/magnetometer.launch.xml @@ -0,0 +1,12 @@ + + + + + + From f9e5409dd665e34e0b554a06bc56e3e14ad5e6dc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= Date: Fri, 13 Dec 2024 20:10:34 +0100 Subject: [PATCH 3/3] Refactor tf_bridge demo (#644) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Carlos Agüero --- ros_gz_sim_demos/README.md | 2 +- ros_gz_sim_demos/config/tf_bridge.yaml | 14 +++++ ros_gz_sim_demos/launch/tf_bridge.launch.py | 59 -------------------- ros_gz_sim_demos/launch/tf_bridge.launch.xml | 12 ++++ 4 files changed, 27 insertions(+), 60 deletions(-) create mode 100644 ros_gz_sim_demos/config/tf_bridge.yaml delete mode 100644 ros_gz_sim_demos/launch/tf_bridge.launch.py create mode 100644 ros_gz_sim_demos/launch/tf_bridge.launch.xml diff --git a/ros_gz_sim_demos/README.md b/ros_gz_sim_demos/README.md index 9f1b75a6..6c612de3 100644 --- a/ros_gz_sim_demos/README.md +++ b/ros_gz_sim_demos/README.md @@ -208,6 +208,6 @@ and transforms of a robot in rviz. To try the demo launch: - ros2 launch ros_gz_sim_demos tf_bridge.launch.py + ros2 launch ros_gz_sim_demos tf_bridge.launch.xml ![](images/tf_bridge.gif) diff --git a/ros_gz_sim_demos/config/tf_bridge.yaml b/ros_gz_sim_demos/config/tf_bridge.yaml new file mode 100644 index 00000000..9debbf11 --- /dev/null +++ b/ros_gz_sim_demos/config/tf_bridge.yaml @@ -0,0 +1,14 @@ +# tf_bridge configuration. +- ros_topic_name: "/joint_states" + gz_topic_name: "/world/default/model/double_pendulum_with_base0/joint_state" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + lazy: true + direction: GZ_TO_ROS + +- ros_topic_name: "/tf" + gz_topic_name: "/model/double_pendulum_with_base0/pose" + ros_type_name: "tf2_msgs/msg/TFMessage" + gz_type_name: "gz.msgs.Pose_V" + lazy: true + direction: GZ_TO_ROS diff --git a/ros_gz_sim_demos/launch/tf_bridge.launch.py b/ros_gz_sim_demos/launch/tf_bridge.launch.py deleted file mode 100644 index 42f76cdc..00000000 --- a/ros_gz_sim_demos/launch/tf_bridge.launch.py +++ /dev/null @@ -1,59 +0,0 @@ -# Copyright 2022 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import ExecuteProcess -from launch_ros.actions import Node - - -def generate_launch_description(): - pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos') - return LaunchDescription([ - # Launch gazebo - ExecuteProcess( - cmd=[ - 'gz', 'sim', '-r', - os.path.join( - pkg_ros_gz_sim_demos, - 'models', - 'double_pendulum_model.sdf' - ) - ] - ), - # Launch a bridge to forward tf and joint states to ros2 - Node( - package='ros_gz_bridge', - executable='parameter_bridge', - arguments=[ - '/world/default/model/double_pendulum_with_base0/joint_state@' - 'sensor_msgs/msg/JointState[gz.msgs.Model', - '/model/double_pendulum_with_base0/pose@' - 'tf2_msgs/msg/TFMessage[gz.msgs.Pose_V' - ], - remappings=[ - ('/model/double_pendulum_with_base0/pose', '/tf'), - ('/world/default/model/double_pendulum_with_base0/joint_state', '/joint_states') - ] - ), - # Launch rviz - Node( - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(pkg_ros_gz_sim_demos, 'rviz', 'tf_bridge.rviz')] - ) - ]) diff --git a/ros_gz_sim_demos/launch/tf_bridge.launch.xml b/ros_gz_sim_demos/launch/tf_bridge.launch.xml new file mode 100644 index 00000000..321a3d8c --- /dev/null +++ b/ros_gz_sim_demos/launch/tf_bridge.launch.xml @@ -0,0 +1,12 @@ + + + + + +