From 5d99c015e62c44b47cabfc319fc5d06171b78333 Mon Sep 17 00:00:00 2001 From: Dharini Dutia Date: Thu, 1 Sep 2022 19:33:48 -0400 Subject: [PATCH] sdformat_urdf parser demo (#265) * parser compatible model and launch framework Signed-off-by: Dharini Dutia * added ground plane, common gz plugins, demo commands and cleaned install paths Signed-off-by: Dharini Dutia * unique collision names and cleared flake Signed-off-by: Dharini Dutia * updating model config Signed-off-by: Dharini Dutia * building parser from source Signed-off-by: Dharini Dutia * fix flake and update deb dependency for garden Signed-off-by: Dharini Dutia * Move packages and files to gz Signed-off-by: methylDragon * feedback and ign->gz Signed-off-by: Dharini Dutia * Support ros_ign migration Clean up shared libraries, and tick-tock RosGzPointCloud Tick-tock launch args Hard-tock ign_ in sources Migrate ign, ign_, IGN_ for sources, launch, and test files Migrate IGN_XXX_VER, IGN_T, header guards Migrate launchfile, launchfile args, and test source references Migrate ros_ign_XXX and gz_gazebo -> gz_sim Migrate ros_ign_XXX project names Migrate Ign, ign-, IGN_DEPS, ign-gazebo Migrate ignitionrobotics, ignitionrobotics/ros_ign, osrf/ros_ign Migrate ignition-version, IGNITION_VERSION, Ignition , ros_ign_ci Signed-off-by: methylDragon * renaming and flake Signed-off-by: Dharini Dutia * added ros commands Signed-off-by: Dharini Dutia * gz-version Signed-off-by: Louise Poubel * feedback and ci trial Signed-off-by: Dharini Dutia * removing garden condition Signed-off-by: Dharini Dutia Signed-off-by: Dharini Dutia Signed-off-by: Michael Carroll Signed-off-by: methylDragon Signed-off-by: Louise Poubel Co-authored-by: Michael Carroll Co-authored-by: methylDragon Co-authored-by: Louise Poubel --- .github/workflows/build-and-test.sh | 4 +- .github/workflows/ros2-ci.yml | 6 + ros_gz_sim_demos/CMakeLists.txt | 8 + .../hooks/ros_gz_sim_demos.dsv.in | 1 + ros_gz_sim_demos/launch/sdf_parser.launch.py | 109 ++++++++++ ros_gz_sim_demos/models/vehicle/model.config | 16 ++ ros_gz_sim_demos/models/vehicle/model.sdf | 188 ++++++++++++++++++ ros_gz_sim_demos/package.xml | 1 + ros_gz_sim_demos/rviz/vehicle.rviz | 148 ++++++++++++++ ros_gz_sim_demos/worlds/vehicle.sdf | 90 +++++++++ 10 files changed, 570 insertions(+), 1 deletion(-) create mode 100644 ros_gz_sim_demos/hooks/ros_gz_sim_demos.dsv.in create mode 100644 ros_gz_sim_demos/launch/sdf_parser.launch.py create mode 100644 ros_gz_sim_demos/models/vehicle/model.config create mode 100644 ros_gz_sim_demos/models/vehicle/model.sdf create mode 100644 ros_gz_sim_demos/rviz/vehicle.rviz create mode 100644 ros_gz_sim_demos/worlds/vehicle.sdf diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh index 1ac4206d..aa516d17 100755 --- a/.github/workflows/build-and-test.sh +++ b/.github/workflows/build-and-test.sh @@ -17,6 +17,8 @@ if [ "$GZ_VERSION" == "garden" ]; then wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - GZ_DEPS="libgz-sim7-dev" + + ROSDEP_ARGS="--skip-keys='sdformat-urdf'" fi # Fortress comes through rosdep for Focal and Jammy @@ -31,7 +33,7 @@ apt-get install -y $GZ_DEPS \ rosdep init rosdep update -rosdep install --from-paths ./ -i -y -r --rosdistro $ROS_DISTRO +rosdep install --from-paths ./ -i -y -r --rosdistro $ROS_DISTRO $ROSDEP_ARGS # Build. source /opt/ros/$ROS_DISTRO/setup.bash diff --git a/.github/workflows/ros2-ci.yml b/.github/workflows/ros2-ci.yml index 571be23a..4c628914 100644 --- a/.github/workflows/ros2-ci.yml +++ b/.github/workflows/ros2-ci.yml @@ -30,3 +30,9 @@ jobs: DOCKER_IMAGE: ${{ matrix.docker-image }} GZ_VERSION: ${{ matrix.gz-version }} ROS_DISTRO: ${{ matrix.ros-distro }} + - name: Build sdformat_urdf from source + uses: actions/checkout@v2 + if: ${{ matrix.gz-version }} == "garden" + with: + repository: ros/sdformat_urdf + ref: ros2 diff --git a/ros_gz_sim_demos/CMakeLists.txt b/ros_gz_sim_demos/CMakeLists.txt index 38754563..8e0f580a 100644 --- a/ros_gz_sim_demos/CMakeLists.txt +++ b/ros_gz_sim_demos/CMakeLists.txt @@ -27,4 +27,12 @@ install( DESTINATION share/${PROJECT_NAME}/models ) +install( + DIRECTORY + worlds/ + DESTINATION share/${PROJECT_NAME}/worlds +) + +ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in") + ament_package() diff --git a/ros_gz_sim_demos/hooks/ros_gz_sim_demos.dsv.in b/ros_gz_sim_demos/hooks/ros_gz_sim_demos.dsv.in new file mode 100644 index 00000000..a0c6d4fc --- /dev/null +++ b/ros_gz_sim_demos/hooks/ros_gz_sim_demos.dsv.in @@ -0,0 +1 @@ +prepend-non-duplicate;IGN_GAZEBO_RESOURCE_PATH;@CMAKE_INSTALL_PREFIX@/share diff --git a/ros_gz_sim_demos/launch/sdf_parser.launch.py b/ros_gz_sim_demos/launch/sdf_parser.launch.py new file mode 100644 index 00000000..f760ca06 --- /dev/null +++ b/ros_gz_sim_demos/launch/sdf_parser.launch.py @@ -0,0 +1,109 @@ +# 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 DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node + + +def generate_launch_description(): + + pkg_ros_gz_sim_demos = get_package_share_directory('ros_gz_sim_demos') + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') + + sdf_file = os.path.join(pkg_ros_gz_sim_demos, 'models', 'vehicle', 'model.sdf') + + with open(sdf_file, 'r') as infp: + robot_desc = infp.read() + + rviz_launch_arg = DeclareLaunchArgument( + 'rviz', default_value='true', + description='Open RViz.' + ) + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py'), + ), + launch_arguments={'gz_args': PathJoinSubstitution([ + pkg_ros_gz_sim_demos, + 'worlds', + 'vehicle.sdf' + ])}.items(), + ) + + # Bridge to forward tf and joint states to ros2 + gz_topic = '/model/vehicle' + joint_state_gz_topic = '/world/demo' + gz_topic + '/joint_state' + link_pose_gz_topic = gz_topic + '/pose' + bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=[ + # Clock (Gazebo -> ROS2) + '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock', + # Joint states (Gazebo -> ROS2) + joint_state_gz_topic + '@sensor_msgs/msg/JointState[ignition.msgs.Model', + # Link poses (Gazebo -> ROS2) + link_pose_gz_topic + '@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V', + link_pose_gz_topic + '_static@tf2_msgs/msg/TFMessage[ignition.msgs.Pose_V', + # Velocity and odometry (Gazebo -> ROS2) + gz_topic + '/cmd_vel@geometry_msgs/msg/Twist@ignition.msgs.Twist', + gz_topic + '/odometry@nav_msgs/msg/Odometry@ignition.msgs.Odometry', + ], + remappings=[ + (joint_state_gz_topic, 'joint_states'), + (link_pose_gz_topic, '/tf'), + (link_pose_gz_topic + '_static', '/tf_static'), + ], + parameters=[{'qos_overrides./tf_static.publisher.durability': 'transient_local'}], + output='screen' + ) + + # 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='both', + parameters=[ + {'use_sim_time': True}, + {'robot_description': robot_desc}, + ] + ) + + # Launch rviz + rviz = Node( + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(pkg_ros_gz_sim_demos, 'rviz', 'vehicle.rviz')], + condition=IfCondition(LaunchConfiguration('rviz')), + parameters=[ + {'use_sim_time': True}, + ] + ) + + return LaunchDescription([ + rviz_launch_arg, + gazebo, + bridge, + robot_state_publisher, + rviz + ]) diff --git a/ros_gz_sim_demos/models/vehicle/model.config b/ros_gz_sim_demos/models/vehicle/model.config new file mode 100644 index 00000000..a24466b1 --- /dev/null +++ b/ros_gz_sim_demos/models/vehicle/model.config @@ -0,0 +1,16 @@ + + + vehicle + 1.0 + model.sdf + + + Dharini Dutia + dharini@openrobotics.org + + + + Parser plugin `sdformat_urdf` compatible differential drive vehicle model. Used to read + models described in SDFormat and outputs URDF C++ DOM structures. + + diff --git a/ros_gz_sim_demos/models/vehicle/model.sdf b/ros_gz_sim_demos/models/vehicle/model.sdf new file mode 100644 index 00000000..8dd8656a --- /dev/null +++ b/ros_gz_sim_demos/models/vehicle/model.sdf @@ -0,0 +1,188 @@ + + + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + left_wheel_joint + right_wheel_joint + 1.25 + 0.3 + 1 + 1 + -1 + 2 + -2 + 0.5 + -0.5 + 1 + -1 + + + + diff --git a/ros_gz_sim_demos/package.xml b/ros_gz_sim_demos/package.xml index 22d56c10..a330904a 100644 --- a/ros_gz_sim_demos/package.xml +++ b/ros_gz_sim_demos/package.xml @@ -24,6 +24,7 @@ rqt_plot rqt_topic rviz2 + sdformat_urdf xacro ament_lint_auto diff --git a/ros_gz_sim_demos/rviz/vehicle.rviz b/ros_gz_sim_demos/rviz/vehicle.rviz new file mode 100644 index 00000000..c01b4af8 --- /dev/null +++ b/ros_gz_sim_demos/rviz/vehicle.rviz @@ -0,0 +1,148 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /RobotModel1/Status1 + Splitter Ratio: 0.5 + Tree Height: 683 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +Visualization Manager: + Class: "" + Displays: + - 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: "" + Name: RobotModel + TF Prefix: vehicle + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: vehicle/odom + Frame Rate: 30 + Name: root + Tools: + - 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: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + 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_default_plugins/Orbit + Distance: 68.66040802001953 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -1.2689772844314575 + Y: 10.203336715698242 + Z: -2.8907392024993896 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5697963237762451 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.8253979682922363 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000023d000002ecfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006b00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000044000002ec000000eb00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f8fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002f8000000b900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000026c000002ec00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 299 + Y: 108 diff --git a/ros_gz_sim_demos/worlds/vehicle.sdf b/ros_gz_sim_demos/worlds/vehicle.sdf new file mode 100644 index 00000000..745c3174 --- /dev/null +++ b/ros_gz_sim_demos/worlds/vehicle.sdf @@ -0,0 +1,90 @@ + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + true + 0 0 0 0 0 0 + + package://ros_gz_sim_demos/models/vehicle + + + + + + + true + true + true + 1 + + + + vehicle/odom + vehicle + + + + +