[Garden] Port WAMV-V xacro files and generation script

[iche033]

• Created a new vrx_urdf directory that consists of these packages:
  • wamv_description - Ported from main and updated xacro files to to use new gazebo systems
  • wamv_gazebo - Ported from main and updated xacro files (incl. components and thruster_layout) to use new gazebo systems and sensors
  • vrx_gazebo - ported the wam-v generation python and launch scripts to ROS2
• Updated the launch script in vrx_gz to support loading a urdf file of the wam-v
  • i.e. added urdf arg to competition.launch.py
  • The launch scripts convert urdf -> sdf before spawning the wam-v into simulation
• Gz<->ROS2 bridges for these new components / sensors should be working
  • python-sdformat is used to parse the generated sdf string in order to determine what sensors are attached to the wam-v and then launch the corresponding bridges. Note if you built sdformat from source, you will need to set the PYTHONPATH. See test instructions below.

To test:

1. Run the generate_wamv script to generate a urdf file from the provided example configuration yaml files.

   ros2 launch vrx_gazebo generate_wamv.launch.py component_yaml:=`ros2 pkg prefix vrx_gazebo`/share/vrx_gazebo/config/wamv_config/example_component_config.yaml thruster_yaml:=`ros2 pkg prefix vrx_gazebo`/share/vrx_gazebo/config/wamv_config/example_thruster_config.yaml wamv_target:=`pwd`/wamv_target.urdf wamv_locked:=False

   This should output a wamv_target.urdf in your current working directory.

2. Run the simulator with the generated wamv file

   a. If you installed sdformat from source, you will need to export your PYTHONPATH

   export PYTHONPATH=$PYTHONPATH:<path_to_colcon_ws>/install/lib/python 
   

   b. Launch sim

   ros2 launch vrx_gz competition.launch.py world:=sydney_regatta urdf:=`pwd`/wamv_target.urdf
   

ros2 topic list
...

/clock
/parameter_events
/rosout
/tf
/tf_static
/vrx/contacts
/vrx/task/info
/wamv/pose
/wamv/pose_static
/wamv/sensors/cameras/far_left_camera_sensor/camera_info
/wamv/sensors/cameras/far_left_camera_sensor/image_raw
/wamv/sensors/cameras/far_left_camera_sensor/optical/camera_info
/wamv/sensors/cameras/far_left_camera_sensor/optical/image_raw
/wamv/sensors/cameras/front_left_camera_sensor/camera_info
/wamv/sensors/cameras/front_left_camera_sensor/image_raw
/wamv/sensors/cameras/front_left_camera_sensor/optical/camera_info
/wamv/sensors/cameras/front_left_camera_sensor/optical/image_raw
/wamv/sensors/cameras/front_right_camera_sensor/camera_info
/wamv/sensors/cameras/front_right_camera_sensor/image_raw
/wamv/sensors/cameras/front_right_camera_sensor/optical/camera_info
/wamv/sensors/cameras/front_right_camera_sensor/optical/image_raw
/wamv/sensors/gps/gps/fix
/wamv/sensors/imu/imu/data
/wamv/sensors/lidars/lidar_wamv_sensor/points
/wamv/sensors/lidars/lidar_wamv_sensor/scan
/wamv/thrusters/left/pos
/wamv/thrusters/left/thrust
/wamv/thrusters/right/pos
/wamv/thrusters/right/thrust

If ground_truth_enabled is set to true, there will be an extra /wamv/sensors/position/ground_truth_odometry topic

Things to note:

• The URDF -> SDF conversion process changes the collision names. The contact sensor currently hardcodes references to these collision names
• There is no hector_gazebo_ros_imu plugin in the new Gazebo. I used an imu sensor with parameters from mbzirc / subt
• There is no hector_gazebo_ros_gps plugin in the new Gazebo. The navsat sensor is used instead.
• There is no gazebo_ros_p3d plugin in the new Gazebo. The odometry-publisher system is used instead.
• The thruster systems have hard coded xu, xuu params for computing the max total thrust.
• The pinger and ball shooter plugins are commented out as they have not been ported yet

[caguero]

Reminder: we need to add ros-humble-xacro to the list of packages to install in the instructions.

[caguero]

I'm getting an error trying to compile:

--- stderr: wamv_gazebo                                                                                                             
CMake Warning at /opt/ros/humble/share/xacro/cmake/xacro-extras.cmake:74 (message):
  failed to determine deps for: urdf/wamv_gazebo.urdf.xacro

  error: package not found: "package 'wamv_gazebo' not found, searching:
  ['/home/caguero/vrx2023_ws/install', '/opt/ros/humble']"

  when processing file: urdf/wamv_gazebo.urdf.xacro

Call Stack (most recent call first):
  /opt/ros/humble/share/xacro/cmake/xacro-extras.cmake:133 (xacro_add_xacro_file)
  CMakeLists.txt:9 (xacro_add_files)


package not found: "package 'wamv_gazebo' not found, searching: ['/home/caguero/vrx2023_ws/install', '/opt/ros/humble']"

[iche033]

I'm getting an error trying to compile:

I commented out the offending line in cmake that runs xacro. I think we may not need it. It should build now.

[iche033]

Alright updated the ros topic names to be consistent with the naming scheme in the main branch. I also updated the PR description with the new ROS topic names. b421c87, af5941c

[iche033]

removed rviz config and launch files in 00e836a as they do not work yet.

[caguero]

Do you recognize that red object on top of the WAM-V?

It doesn't look familiar...

[caguero]

Should we add the

<plugin
      filename="gz-sim-navsat-system"
      name="gz::sim::systems::NavSat">
    </plugin>

to the recently merged wildlife_task.sdf world?

[caguero]

All my tests work as expected. The only remaining question is whether launching the world without the urdf option should load the default WAM-V with sensors. That was the expected behavior in Gazebo classic.

[iche033]

to the recently merged wildlife_task.sdf world?

yep, added in c370dd9

Do you recognize that red object on top of the WAM-V?

that's the imu visual. I noticed that it's white in gazebo-classic but it should've been red though as that's what's specified in urdf. I can change to white to make it consistent?

The only remaining question is whether launching the world without the urdf option should load the default WAM-V with sensors. That was the expected behavior in Gazebo classic.

I can also make the behavior consistent with gazebo-classic

[iche033]

I can also make the behavior consistent with gazebo-classic

I looked at a few launch files in master and saw that they have slight different default arg values. I changed this PR to run xacro with the default from the perception_task.launch just as an example. 86fa7fe.

We can def change the default values or even exposed them as launch args if it's helpful

[caguero]

I think something isn't right because the RTF drops a lot in this branch (~5%).

[iche033]

hmm I what world / configure are you using? I tested with different worlds and default wamv configurations and I'm seeing around 5x% RTF.

[caguero]

hmm I what world / configure are you using? I tested with different worlds and default wamv configurations and I'm seeing around 5x% RTF.

I can confirm it oscillates between 50% and 80% most of the time. I think I wasn't trying with a good Nvidia setup last week.

We discussed today trying to reduce the resolution of the camera / velodine to recover the real-time behavior until we improve performance but we could tackle it in a separate PR.

[iche033]

I just remember that another potential perf improvement is to enable lazy subscription on the bridge, e.g. add the following
to the parameter_bridge nodes

parameters=[{'lazy': True}],

https://github.com/osrf/vrx/blob/gazebosim/vrx_gz/src/vrx_gz/launch.py#L128
https://github.com/osrf/vrx/blob/gazebosim/vrx_gz/src/vrx_gz/launch.py#L168
https://github.com/osrf/vrx/blob/gazebosim/vrx_gz/src/vrx_gz/model.py#L124-L152