Possibility of creating additional Mavros topics for different Mavlink messages?

Mark-Beaty commented 3 years ago

Issue details

I'm not sure if this is something that already exists, or if it is possible/feasible to implement within mavros, but I am working with gimbal controls in a ROS2 environment. I've been able to publish commands to simulation successfully to control the gimbal, but a lot of the gimbal specific information I need access to is not currently (readably) exposed through an existing mavros topic. I'm not sure if there is some functionality already within mavros that I can take advantage of to get/translate the data from the relevant Mavlink messages.

I've been able to determine that the messages are being received and republished in the ROS topic /uas1/mavlink_source (I collected the messages simply through the terminal using ros2 topic echo /uas1/mavlink_source | egrep -B 14 -A 14 "msgid: XXX"). I assume that mavros is already already doing some very similar message translation in creating the existing topics for things like GPS data, altitude, etc. I'm wondering if there is some way to add additional messages (specifically by id, and likely defining the payload of data contained within somehow) and create topics for the contents of those messages from Mavlink, and how I would go about setting this up.

I think it is likely that mavros will likely include these messages in the future, as the previous method of gimbal control which was included in mavros 1 (mavros/mavros_extras/src/plugins/mount_control.cpp) is now deprecated (see here for the documentation I used in constructing my commands for gimbal control/understanding the messages/commands being used now which replaced those used by mount_control). At this point I don't necessarily need all of the functionality which was provided by the mount_control plugin, but I do need access to the gimbal state being published to Mavlink.

Here are the messages which are relevant to gimbal control (there may be more, but these are the ones that I came across):

GIMBAL_DEVICE_ATTITUDE_STATUS - msgid: 283
- This message (should) include the gimbal orientation within the q field and is published frequently and seen in /uas1/mavlink_source in my simulation environment
- This is also the only message I definitely need access to for the project I'm working on, as the default gimbal_device_id worked for requesting control, but I still need the real-time information about the gimbal's orientation.
GIMBAL_MANAGER_INFORMATION - msgid: 280
- This message contains the information used to identify the gimbal you're requesting control of (gimbal_device_id) as well as its capabilities and maximum angles.
- This message is documented to be requested by the ground station using MAV_CMD_REQUEST_MESSAGE but in my sim it was being published regularly as well.
GIMBAL_MANAGER_STATUS - msgid: 281
- This message indicates the gimbal manager flags as well as primary and secondary control sysid and compid for the respective gimbal_device_id, which you would need to alter to take control of the gimbal from within mavros.

If anyone could point me in the right direction for either decoding the message and its payload manually or using some functionality within mavros to assist so I can get it published to a topic alongside the rest of the mavros data that would be greatly appreciated!

MAVROS version and platform

Mavros: 2.0.3 ROS2: Foxy Ubuntu: 20.04

Autopilot type and version

[ ] ArduPilot [ X ] PX4

Version: Through Auterion suite simulator, software release v2.3.1

Useful notes/commands used to successfully achieve gimbal control in my simulation:

When requesting control of the gimbal from the ros terminal, you need to use the compid of the onboard computer as it is used by the mavros UAS node. I found this (mostly by accident) while looking through the mavros source code in /mavros/mavros/include/mavros/mavros_uas.hpp and it is 191, this is also documented in mavlink as the primary onboard computer's compid. The command I used is as follows:
- ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong "{command: 1001, param1: -2, param2: 191, param3: -1, param4: -1, param7: 154}"
- The command code is for MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE, default gimbal_device_id is 154 and passed to param7 while the compid is passed to param2.
Then to control the gimbal you can use this:
- ros2 service call /mavros/cmd/command mavros_msgs/srv/CommandLong "{command: 1000, param1: -40, param2: -90, param3: -0.3, param4: -0.3, param7: 154}"
- MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW

As a final note, it might be useful (though I'm not sure how feasible) to add functionality for MAV_CMD_REQUEST_MESSAGE to return messages in a useful, translated/readable way, as currently calling that message through the generic CommandLong doesn't return the message but just says whether or not requesting it was successful. It does however publish that message to /uas1/mavlink_source in the untranslated format identifiable by the msgid.

vooon commented 3 years ago

Mavros has ability to load a plugin modules, which does translation logic, like publish/subscribe ros topics, send/receive mavlink messages etc. mount_control.cpp - one of plugins shipped with mavros, but you able to make custom plugin in your custom package.

Most of extras plugins not ported yet, so possibly you need to do that, then to add support for new gimbal protocol.

Mark-Beaty commented 2 years ago

@vooon Resurrecting this issue as I'm now working on developing the support of Mavlink gimbal protocol v2 as a new mavros plugin (finally to a point where its the next blocker for development on my project) and have a few questions for the development process. Hoping to get something workable developed ASAP for my project's use/to be pulled into a mavros release.

First, I'm wondering if there is a way to load a single custom plugin for mavros while maintaining the apt install method, and if so how would I go about accomplishing this? I'm trying to do the development within my existing project, and switching to building mavros/mavlink from source has caused some development pains that will require some work to sort out, easiest way out of this would be to just be building my plugin rather than the whole of mavros, but I'm not sure if this is possible. Potentially could just build mavros-extras locally as well, while maintaining mavros as a apt install, though I'm not sure of the process that would be required for making this work.

I might also end up needing to develop a custom plugin down the line which wouldn't have such a universal use-case for mavros, so wouldn't end up being included in a pull request to the main mavros repo. Being able to load custom plugins without building all of mavros from source would help keep my project repo cleaner and improve expandability of both my project and mavros.

In the same vain as the above question, I'm wondering if mavlink needs to be built locally to enable the building of the mavros-extras plugins? Basically any advice you have for efficiently developing mavros plugins would be appreciated, or any links to existing documentation/relevant answered questions.

Current setup I'm using is still a ROS2 foxy implementation from within a docker container, so dockerfile configs related to mavros looked like this prior to restructuring to clone/build mavros from source/my fork to start development on the gimbal v2 plugin, which isn't yet built into the dockerfile (all done manually after the container is built currently) and hopefully can be avoided:

RUN apt-get update \
   && apt-get -y install --no-install-recommends \
   ros-foxy-mavros \
   ros-foxy-mavros-extras \
....

RUN wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh && \
   bash ./install_geographiclib_datasets.sh

Any advice you can provide on getting me on the right path to developing a working plugin and supporting the gimbal v2 protocol would be greatly appreciated, currently I've basically copied the existing mount_control plugin as a reference and started adapting it to the new protocol but I'm not quite ready to start testing anything just yet.

vooon commented 2 years ago

It should be possible to load a plugin from 3-rd party plugin library. Extras may be used as an example.

mavlink / mavros