Gazebo Ros2 Ardupilot Camera Support

[mhl787156]

This PR introduces camera support for iris ardupilot in particular, but also allows for generic cameras to be added to model URDFs. This changes include the following:

• starling-sim-base-core gazebo install updated to download ros-foxy-gazebo-pkgs as it contains the plugins required for sensor simulation
• A script setup_display.sh which configures a virtual framebuffer so that the cameras can virtually render
• Iris-ap now includes the gimble arm and meshes, where the camera has now been configured with a camera controller in the ros2 style: Ros2 migration guide

Starts to address #88

Future Improvements:

• setup_display.sh should be able to setup from a user defined choice of nvidia or virtual framebuffer

[mhl787156]

Further enhancements

TODO:

• [x] Test that an image is actually being output (test using fenswood example).
• [ ] Switch to using xacro so that initial gimbal pose can be configured.

[mhl787156]

Suggestions for testing the image output @rob-clarke ? I tried foxglove yesterday with the ros bridge but it didn't work. This may be because the bridge says it doesn't support Png.... (https://github.com/RobotWebTools/ros2-web-bridge#not-supported-op).

I might try with the foxglove desktop app as that allows native connections... maybe!

[mhl787156]

So while foxglove fix their issue, I have been looking into gimbal control. Turns out there is already a plugin for gimbal control in the gimbal_small_2d/model.sdf !

However that is based on this controller which is never actually built.... If I try and force it to be built, it is so out of date that it will not build

I was also confused by the gazebo joints. There is already a joint set up for tilting. The gazebo docs suggest that the joint alone should create a joint state publisher which can be controlled? Otherwise I will look for an external controller plugin.

[mhl787156]

Update, attempted to fix the ardupilot_gazebo repo gimbal plugin, but it is rather incredibly out of date. It would be quicker just to write a gimbal controller from scratch! @rob-clarke here are a list of available plugins for foxy (https://github.com/ros-simulation/gazebo_ros_pkgs/tree/foxy/gazebo_plugins/src) - which one do you think should be simple enough to move the single gimbal joint?

[mhl787156]

Verified through foxglove that the gimbal topics of of /vehicle_1/gimbal_tilt_status and /vehicle_1/gimbal_tilt_cmd work and move the camera.

Currently hardcoded to vehicle_1, going to need xacro (or some other method ) to be able to specify namespace.

[mhl787156]

Added xacro parsing of the SDF file. The sdf parameters are now editable through the launch file and environment variables:

    <!-- Xacro launch the gimball small 2d for the iris_with_ardupilot model -->
    <executable unless="$(var sim_only)" 
        cmd="xacro model.sdf.xacro
              ros_namespace:=$(env VEHICLE_NAMESPACE)
              camera_name:=$(env CAMERA_NAME camera)
              camera_height:=$(env CAMERA_HEIGHT 480)
              camera_width:=$(env CAMERA_WIDTH 640)
              gimbal_initial_angle:=$(env GIMBAL_INITIAL_ANGLE 0.785)
              -o model.sdf
              "
        cwd="/ardupilot_gazebo/models/gimbal_small_2d/"
        output="screen"
    /> 

VEHICLENAMESPACE is now set by default in the arduino-gazebo container to `vehicle${AP_SYSID}soAP_SYSID` must be set when launching a vehicle (otherwise an error will be raised)

[rob-clarke]

The xacro stuff here will likely conflict with the changes I've made to support the non-network host mode (#117). I suggest we merge that first as it's simpler then restructure here where needed.

[mhl787156]

Alright, then feel free to merge #117 and I'll come back and fix this one if there's any issues.

[mhl787156]

@rob-clarke

1. Should AP_SITL_ADDDRESS/ AP_SITL_HOST check be moved into the ardupilot-gazebo container? For this branch I needed access to vehicle namespace and AP_SYSID so we have a sim_setup.sh script in base/ardupilot specifically for the ardupilot-gazebo container
2. I will move the gimbal xacro to the iris_setup.sh from the launch file, it's a better solution.

[rob-clarke]

Yes, we could move the underlying environment variable logic. It was mainly there because it was model specific xacro that needed modifying so I felt it fit better at the vehicle layer. We would need to figure out if we need control over ordering of the ros.env.d scripts if we moved it where as now it definitely happens before the xacro call. Could go the udev rules route and prepend 00_ etc for ordering.

[mhl787156]

@rob-clarke I have moved some things around, but currently having issues loading iris_with_ardupilot xacro model into gazebo. It only appears to load the iris_with_standoffs model from /ardutpilot_gazebo/models.

The xacro should overwrite the model.sdf in /ardupilot_gazebo/models/iris_with_ardupilot, so i'm thinking something is not quite write about the generated sdf file. Everything looks about right though...

[mhl787156]

Update, I have isolated it to the <listen_addr>$(arg listen_addr)</listen_addr> line in model.sdf.xacro.

With this line, gzserverfor model path /ardupilot_gazebo/models only has iris_with_standoffs:

Without, it confirms that /ardupilot_gazebo/models is also overriding iris_with_ardupilot:

I have checked the source so listen_addr is read in the libArduPilotPlugin.so. After turning on verbose mode, it also looks whatever happening is making it fail silently. (None of the logs i can find so anything).

When I was debugging the gimbal model, I also faced some silent failures when the xacro file generated a 'invalid' sdf file. I have a feeling something similar might be happening, but I cannot work out why that particular line is making it invalid!

[mhl787156]

Found a solution! For some reason xacro:args does not work. Replaced with xacro:property in a similar manner to how the gimbal model.sdf.xacro was implemented.

Gimbal now uses std_msgs/msg/Float32.msg.I am aware it is being deprecated, but could not find a better topic short of doing proper Pose. Pose2D is not available on foxglove.

Also updated Docs.

I believe this is ready to merge on approved review.

[mhl787156]

Note! Currently looking at the rosbridge streaming, they all assume some level of compressed image transport. Do we need to add compression topic?

[rob-clarke]

I'd worry about the streaming in a separate PR. Just get this in for now