Implement ROS bridge for calling set_rate on cameras and lidars

[peci1]

Depends on release of gazebosim/gz-sensors#95 in ign-sensors4.

This PR adds ROS services .../set_rate to cameras and lidars which can be used to call their Ignition counterparts. This is to allow users to deliberately lower the FPS of the rendering sensors, speeding up the simulation. It doesn't change the default behavior of the sensors in any way (i.e. until the ROS service is called, the sensors publish at the rate specified in SDF).

gazebosim/gz-sensors#95 makes sure it is not possible to set the rate higher than what it specified in SDF (special case 0 is only allowed to be set if SDF contains 0). Negative set_rate requests are ignored.

Each time the set_rate request is processed by the simulator, it emits a debug message like

[Dbg] [Sensor.cc:266] Setting update rate of sensor X1::base_link::camera_front to 10 Hz

It also checks whether the Ignition service is available. If it's not, it emits this kind of warning once per minute:

[ WARN] [1613162940.414493909, 60.944000000] [ros.subt_ros]: Waiting for ignition service /world/simple_cave_01/model/X1/link/base_link/sensor/camera_front/image/set_rate to appear.

Moreover, if IGN_TRANSPORT_TOPIC_STATISTICS=1, the warning will also include this text:

IGN_TRANSPORT_TOPIC_STATISTICS is set to 1. Make sure all parts of the simulator run with this setting, otherwise the parts with different values of this variable would not be able to communicate with each other.

The ROS service is only advertised after existence of the Ignition service is confirmed.

So far I added the services mostly to robots we regularly use. There's no reason not to add it to other robots, too, but it needs to be tested a bit (whether you connected the ROS service to the correct Ignition service) and that would take time, so I first want to get a thumbs up on how this is implemented, and when the implementation is agreed upon, I can try to fix the other robots.

---

There is an alternative implementation, which would not need changing all robot launchers. It would be a node that would periodically list all ignition services and create the set_rate ROS counterparts on-the-fly as the Ignition services appear. But I didn't like this solution that much because it's not really transparent (automagic) and it would create weird service names (we would need to name them after the Ignition services because there is no other hint for a name). It would also create the services for sensors where it doesn't make sense to lower the rate because generating their data is not expensive (IMUs, pressure sensors, magnetometers).

---

I also probably found a bug in

subt/subt_ros/launch/vehicle_topics.launch

Lines 140 to 166 in 6bd98e3

	<group unless="$(arg stereo_cam)">
	<node
	pkg="ros_ign_bridge"
	type="parameter_bridge"
	name="ros_ign_bridge_camera"
	args="$(arg sensor_prefix)/camera_front/camera_info@sensor_msgs/CameraInfo[ignition.msgs.CameraInfo">
	<remap from="$(arg sensor_prefix)/camera_front/camera_info" to="front/camera_info"/>
	</node>
	<node
	pkg="ros_ign_image"
	type="image_bridge"
	name="ros_ign_image_camera"
	args="$(arg sensor_prefix)/camera_front/image">
	<remap from="$(arg sensor_prefix)/camera_front/image" to="front/image_raw"/>
	</node>
	<node
	pkg="subt_ros"
	type="optical_frame_publisher"
	name="optical_frame_publisher">
	<remap from="input/image" to="front/image_raw" />
	<remap from="output/image" to="front/optical/image_raw" />
	<remap from="input/camera_info" to="front/camera_info" />
	<remap from="output/camera_info" to="front/optical/camera_info" />
	</node>
	</group>

.

It begins with a block unless="$(arg stereo_cam)" and adds a RGB camera. But the models which use RGBD camera do not include this additional RGB camera (i.e. X4_SENSOR_CONFIG_2). However, bridges for the RGB cam are created. I only protected the set_rate bridge with the additional unless="$(arg rgbd_cam)", but I think the whole RGB camera block should be excluded when the RGBD is present. But I felt that doing such change could break something I did not foresee, so I did not do it.

---

I also fixed some package.xml's which apparently mistakenly used <buildtool_depend> instead of <exec_depend> (because I needed to add <exec_depend>subt_ros</exec_depend> and it seemed to me this is a good opprotunity to fix the package.xml).

---

Finally, I noticed there's a large room for code reuse in the model launch files.

• roslaunch/XML includes (e.g. the breadcrumb robot config could include the non-breadcrumb robot launch and just add the breadcrumb bridge)
• common roslaunch/XML "library" of bridge pieces (i.e. rgb_cam_bridge.launch which could be included, given a few args, and would bring in all the required nodes like cam info bridge, image bridge, optical frame publisher, set_rate bridge etc.)
• spawner.rb could also utilize some robot building library that would contain plugin definitions which could be composed as Ruby strings (that would be an extension of Refactor Ignition launch files and spawners to share code #786)

All of this would ease the development of new robot models and maintenance of the existing ones and decrease the reviewer's load. But I don't think I have the time for such large refactoring. I'm also not sure if it'd be allowed/ethical to do such large changes to other teams' robot packages. And it would need extensive testing (which could be automated a bit e.g. comparing the "resolved" roslaunch XML files and the resulting Ruby strings in spawners).

[osrf-jenkins]

Build finished. 21 tests run, 0 skipped, 1 failed.

[nkoenig]

This works for me. Tested with the following:

1. ign launch competition.ign worldName:=simple_cave_01 circuit:=cave robotName1:=X1 robotConfig1:=CTU_CRAS_NORLAB_ABSOLEM_SENSOR_CONFIG_1
2. rostopic hz /X1/front/depth
3. rosservice call /X1/front/set_rate 1

[nkoenig]

@peci1 I copied your bug report here: #803.

[peci1]

Great! Now (or with the next feature release) it's time to ask teams to actually use this service to save a lot of computation power.

[angelacmaio]

@peci1 are you still planning to add this feature to the other robots? Once available for each robot, we can add instructions to the API wiki.

[peci1]

I'd rather wait for #759 to be merged.