voxl-microdds-agent and px4_msgs offboard compatibility issue.

[jonathankampia]

Hi, I am facing an issue running custom offboard code on a drone running a VOXL 2 thru ROS2 foxy. The node subscribes to the local position topic /fmu/out/vehicle_odometry, and publishes position to commands to /fmu/in/trajectory_setpoint. When the node runs, the drone seemingly cannot take off in position mode; the motors spin up but it struggles to get off the ground. Without the node running, it flies perfectly. I was able to narrow it down to the publisher causing the problem, as running the code with only the subscriber enabled still allows the drone to fly normally. So, basically, publishing TrajectorySetpoint messsages to the fmu/in/trajectory_setpoint topic causes some sort of communication issue(s) that prevent the drone from flying normally, even when the drone is not in offboard mode.

I had the same issue simulating the node with px4_sitl on ros2 foxy, but figured out it was because in some cases I was sending uninitialized values in the position array of the TrajectorySetpoint message. The code that I got to work normally in the sim then replicated the same issue on the VOXL 2 in real life. This leads me to believe the problem is with a mismatching version of the px4_msgs package, which might clash with whatever voxl-microdds-agent accepts. I am using this version of px4_msgs on the drone. It could also potentially be a QoS problem, but I matched the QoS of the publisher to whatever ros2 topic info --verbose spat out, so I don't think that's the issue.

Just wondering if you guys could share any insight into how the voxl-microdds-agent works, what px4_msgs package version I could use, etc. Or if you have experienced the same issue. I am going to try to replicate this on a starling that I have flown in offboard before (not with ROS 2 though). I might honestly just switch back to ROS 1 if this keeps happening, cause I have had nothing but trouble working with ROS 2 thusfar.

Thanks!

[brahim]

@jonathankampia, the way that voxl-microdds-agent works is that it wraps around the XRCE microdds agent binary from e-prosima, and bridges the px4 microdds topics to the DDS global data space (e.g. ROS2 topics).

The voxl-microdds-agent runs irrespective of any serialized message definitions, so it is just the transport. Probably not the issue, but if you still feel strongly that the agent is the issue, there are ways to test that.

Most likely, the mismatch is between px4_msgs and the voxl-px4 version. In that case serialized message definitions DO matter, and the mismatch could cause issues, but usually this is caught when the dds agent service runs (it will report errors you can view via systemctl status).

What sdk version are you using on the voxl2? You could checkout the submodule px4_msgs version that voxl-mpa-to-ros2 uses.

It would be difficult to troubleshoot without the actual offboard code to test, maybe post a snippet of your code?

For an offboard example using trajectory setpoints, you can also try this one which has been referenced a lot in the PX4 community

[jonathankampia]

@brahim
I am using SDK version 1.3.3

After some further testing it seems like the issue is that I was treating the px4 uOrb messaging platform (which the microdds agent just bridges to ROS) as if it were a MAVROS node. It seems like constantly pushing messages to the trajectory_setpoint topic makes the drone fight itself in position hold mode, whereas using MAVROS as the bridge allowed that. This is just a theory though, I have yet to test it in real life.

Here's a snippet of the code in question:

(QoS and publisher creation)

        // QoS profile for publishers
        rclcpp::QoS qos_profile_pub(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_sensor_data));
	qos_profile_pub.keep_last(1);
	qos_profile_pub.best_effort(); 
	qos_profile_pub.durability_volatile(); 
	qos_profile_pub.liveliness(RMW_QOS_POLICY_LIVELINESS_AUTOMATIC);

	// initialize publishers
	printf("Publishing to /fmu/in/trajectory_setpoint \n");
	trajectory_setpoint_publisher = node->create_publisher<TrajectorySetpoint>("/fmu/in/trajectory_setpoint", 10);

(Actual publishing code)

void idle() {
	
	//printf("IDP_x: %f \n", idlepoint.position[0]);
	idlepoint.yaw = -3.14;
	idlepoint.timestamp = node->get_clock()->now().nanoseconds() / 1000;
	trajectory_setpoint_publisher->publish(idlepoint);
}

^ The 'position' array of the idlepoint object is set in another function. Position and yaw are the only two offboard states the code is set to control through the offboard_control_mode topic.

It seems like the example you linked also doesn't actually publish the messages to /fmu/in/trajectory_setpoint unless the vehicle is in offboard mode, which may fix my problem. I will also try just building voxl-mpa-to-ros2 and using that version of px4_msgs instead. Thanks!

[Zachary Lowell 0]

@jonathankampia it is a feature of PX4 that if the drone is not in OFFBOARD mode while feeding in offboard data points to move to, it will ignore and not move based off the incoming feed because it is paying attention to the sticks from manual or position based flight. Switch the drone into offboard and then stream in your commands and it will react accordingly - this is a feature of PX4 and is how it is done for mavsdk as well.

Zach

[jonathankampia]

@Zachary-Lowell-0 What you are saying definitely is expected behavior; my problem was the opposite. When the drone was in position mode, it seemed to be reacting to / fighting the commands my offboard node was passively sending. (i.e. when I disabled the offboard node, the drone flew fine in position mode).

[jonathankampia]

@brahim
After doing some testing on a Starling 2 Max, I've observed that in offboard mode the drone has an incredibly hard time holding its position. I am using the same code I posted on this thread earlier, that sets a waypoint via the trajectory_setpoint topic virtually identically to how this example does it. It flies extremely well in position hold mode (when trajectorysetpoint commands are not being sent, and has an extremely accurate local position, but starts jerking around and generally having a very hard time when I switch it to offboard mode and allow my node to start publishing TrajectorySetpoint position commands.

I am using the latest version of the SDK, (1.3.3), and have tried multiple version of px4_msgs, including the one gets built when you install voxl-mpa-to-ros2 and the 1.14 foxy version that can be found in the px4_msgs repository. There just has to be some communication issue going on, as the drone should fly the same in both position and offboard mode.

There appear to be no errors with the microdds agent as far as I can tell.

The only thing that makes sense is that the offboard commands are being sent in some wrong format that is messing up the drone. The fact that just them being sent messes up position mode flight (which they should have no impact on), combined with the fact that the drone flies awfully in offboard mode but seems tuned very well in position mode, suggests to me some sort of communication formatting error.

[Zachary Lowell 0]

@jonathankampia can you go into voxl-vision-hub.conf in /etc/modalai and ensure you have offboard_figure_eight set to off - I have a feeling these offboard setpoints are interfering with your code.

LMK if this fixes it.

Zach

[jonathankampia]

@Zachary-Lowell-0 Hi, sorry for the late reply. This was the issue (lol)