Offboard trajectory_setpoint conflict when in auto modes

[benjinne]

Describe the bug
Offboard trajectory_setpoint sent while in an auto mode conflict with flight_tasks sending trajectory_setpoint since they both publish to the same topic. It appears everything is working correctly when sending trajectory_setpoints at the 10hz recommended by the offboard tutorial and in an auto mode, however this is because flight_tasks publishes at 50hz and is fast enough to essentially "override" the setpoints sent by the 10hz offboard publisher. When publishing at 100hz to trajectory_setpoint from offboard, the auto mode is essentially ignored and the offboard setpoint overrides the internal flight_tasks module.

To Reproduce
Steps to reproduce the behavior:

1. Launch simulator and set PX4 to HOLD mode
2. Send only trajectory_setpoints using offboard at 10hz and nothing happens
3. Send only trajectory_setpoints using offboard at 100hz and observe HOLD mode is overriden

Expected behavior
There needs a way for trajectory_setpoint to be distinguished if it's coming from offboard or flight_tasks. When in offboard mode, flight_tasks no longer send trajectory_setpoints, but if in any auto mode, a conflict is possible if offboard setpoints are sent faster than the flight_task

[dagar]

This type of potential conflict needs to be prevented more fundamentally.

Are you using Mavlink or ROS2/FastRTPS?

In the mavlink case this should already be prevented until the system is actually in OFFBOARD mode.

PX4-Autopilot/src/modules/mavlink/mavlink_receiver.cpp

Lines 1004 to 1008 in 03bd5e0

	if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
	// only publish setpoint once in OFFBOARD
	setpoint.timestamp = hrt_absolute_time();
	_trajectory_setpoint_pub.publish(setpoint);
	}

[benjinne]

I'm using ROS2/FastRTPS

[dagar]

The quickest fix is to respect the nav_state before publishing (like mavlink).

The better fix would be to publish the offboard setpoint on a different topic (same msg) and the consumer (the position controller) can decide which to respect based on the system state.

[benjinne]

I'm working on a fix, and got multicopter position controller working, but for the rover, why is there 2 updates for trajectory_setpoint_sub?

PX4-Autopilot/src/modules/rover_pos_control/RoverPositionControl.cpp

Line 431 in 360c378

_trajectory_setpoint_sub.update(&_trajectory_setpoint);

and

PX4-Autopilot/src/modules/rover_pos_control/RoverPositionControl.cpp

Line 478 in 360c378

_trajectory_setpoint_sub.update(&_trajectory_setpoint);

[dagar]

I'm working on a fix, and got multicopter position controller working, but for the rover, why is there 2 updates for trajectory_setpoint_sub?

PX4-Autopilot/src/modules/rover_pos_control/RoverPositionControl.cpp

Line 431 in 360c378

_trajectory_setpoint_sub.update(&_trajectory_setpoint);

and

PX4-Autopilot/src/modules/rover_pos_control/RoverPositionControl.cpp

Line 478 in 360c378

_trajectory_setpoint_sub.update(&_trajectory_setpoint);

I don't think it's intentional, it's probably a result of how things evolved to this point with OFFBOARD setpoints previously getting to the controllers through a different topic.

[benjinne]

Closing since mavlink handles this issue and using ROS2 is experimental and assumes the offboard computer must stop sending setpoints if a pilot takes over.