Hi there, I’m experimenting with the local_planner (PX4/avoidance) and mission mode. I want to control yaw/heading of the UAV after reaching individual waypoints (similar to the ROI feature), but it seems that the local planner overrides any yaw control. I understand that local_planner needs to control yaw to oversee the space towards the next set point.
However, is it feasible to set the heading of the UAV at least for the time it hovers in place at a waypoint?
This is indeed a great question, as it may be a valid use case for many users. (I’m thinking of inspection with enabled obstacle avoidance).
I guess you have to dig into the code quite a bit to add this functionality though.
If I’m not mistaken the way it currently works is that we plan in just 3D and create a tree. Then we ‘invent’ the required heading to follow that path safely. This is in the waypoint generator (by calling getDirectionFromTree)
Changing this won’t be so trivial, but certainly a welcomed contribution!
Thanks for the swift reply and pointing me to the relevant place in the codebase! Indeed, I’m working towards inspection with avoidance. I generate missions based on the geometry of a model environment and use the captured point clouds for reconstruction purposes.
getDirectionFromTree is only called in the tryPath case of switch (planner_info_.waypoint_type) in calculateWaypoint. Since we’re not actually trying to fly a path for looking at an ROI, do you think it would make sense to add a case wpRoi?
I guess as long as your goal is to hover at that position and face a certain direction then sure, I think that could work.
Next you would need to think about how to get the planner to switch in/ and out of that state depending on how your mission waypoint is defined. I’m not sure on the PX4 side, if those mission item ‘heading’ fields are being used and/or sent to the avoidance.
The input to the planner is the mavros/trajectory/desired topic, which is a message defined here, it consists of PositionTarget messages (defined here), which do intrinsically allow for a ‘yaw’ setpoint. But I’m not sure if it’s actively being used. If it is sent from PX4 then I can guarantee the current logic in the planner just ignores it. You’d have to change that
Thanks again for pointing out the relevant pieces, really helpful! However, this seems to be even more complicated than I expected. Mavros for example doesn’t even recognise ROI mission items, so I’d have to patch that too. This seems a bit steep for my first prototype application.
Do you think the desired behaviour could be easier to achieve if I stick w/ offboard mode and mavros/trajectory/generated messages?
As far as I know, the offboard control does not currently use those messages. Offboard listens to /mavros/setpoint_position/local or /mavros/setpoint_velocity/cmd_vel_unstamped rostopics which are converted to a PX4 topic named position_setpoint_triplet. The trajectory messages are only used in mission mode. If you would want to change that I expect it to be quite some work on the PX4 firmware side, as you would have to change the interface.
@henkf you can get the type of waypoint by listening to /mavros/mission/waypoints . However it’s not syncronized with /mavros/trajectory/desired that gives you the position of the waypoint in local coordinates. /mavros/mission/waypoints gives the waypoint position in global coordinates.
I am actually thinking about integrating the waypoint type in the mavros/trajectory/desired message since I run into similar issues with the safe landing planner. So maybe keep an eye on future PRs (I cannot commit on a timeline tough).
From what you have described the interface in mission mode is more suitable than offboard. It seems like you would like to have features like ROI that are not integrated into offboard. I think it’s easier to set up the mission in QGC (that already supports these features) and only make changes to the planner.