Flying in position mode with Optitrack motion capture is unstable

lshums16 commented 1 year ago

Describe the bug

We are attempting to fly in position mode with an Optitrack motion capture system as a prerequisite to flying offboard. The drone flies just fine with manual control, but when we switch to position control we are seeing jittery motion that makes the drone motion unstable. We have been using the motion capture page on PX4 (https://docs.px4.io/main/en/ros/external_position_estimation.html) and have also used this issue as reference.

Here are our current parameters:

EKF2_AID_MASK = 2 EKF2_EV_CTRL = 11 EKF2_EV_DELAY = 0.0ms (haven't been able to calibrate this through log files yet) EKF2_EV_NOISE_MD = EV noise parameters EKF2_HGT_REF = Vision EKF2_IMU_CTRL = 1 EKF2_MAG_DECL = 0.0 deg EKF2_MULTI_IMU = 3 EKF2_MULTI_MAG = 3

Like the previously mentioned issue, we wrote a ROS2 node that takes the Optitrack data and publishes it to the PX4 micro_dds bridge topic /fmu/in/vehicle_visual_odometry. It is publishing at 100 Hz, and changing this does not seem to change the strange behavior that we are seeing.

We are using the following: PX4-Autopilot (main): commit 93ba7de0c3b82507720c6fbbe9d1953217c4722d px4_msgs (main): commit b6eebbfa5350f7bd0969944255db9384a0182d39

Log Files and Screenshots

Here is the Flight Review link from our latest log: https://review.px4.io/plot_app?log=6c279b1a-6d66-44c8-8c09-3e53e4514f0d

Here is a screenshot of /fmu/in/vehicle_visual_odometry: Untitled drawing(1)

Drone (please complete the following information):

Quadcopter
CubePilot Cube Orange Flight Controller

Jaeyoung-Lim commented 1 year ago

@lshums16 You can see in the log that you are having estimator problems that might be related how you are providing optitrack information to the vehicle.

Few things that comes to mind:

Have you checked the frame convensions? The default convention of ROS is ENU, while PX4 uses NED I am not sure EKF would automatically translate this if the frame is not defined as POSE_FRAME_NED. @bresch @dagar As far as I can see this is not the case?

https://github.com/PX4/PX4-Autopilot/blob/d532578ecc470e186e64a48e6bf98666a2d2fb03/src/modules/ekf2/EKF2.cpp#L2040-L2069

Also, your visual odometry seems to have some issues with the timestamps

ajordan5 commented 1 year ago

Working with @lshums16 on this. Thank you for helping us with this!

Our mocap system gives position with an FRD position so we are using POSE_FRAME_FRD which is indicated by pose_frame: 2 in our mocap messages above. If we use NED, QGC says there is a yaw estimate error which I'm assuming is because it expects heading to be aligned with a true earth-based NED frame.

I agree that the latency is suspect. Do you know how this latency is calculated? I realized that we weren't publishing to timestep_sample so I went in and started publishing to it which changes the latency plot but I'm not sure if this is better. The flight was still very jittery and didn't hold position.

Here is the log that produced that plot: https://review.px4.io/plot_app?log=e0504a94-572b-46e9-ad16-058fd341a681

It's not clear from documentation what timestep_sample is supposed to be so right now I'm just publishing the same thing to both timestep and timestep_sample since some other topics seemed to be doing something like that. I should note that mocap is stamped according to the ros clock running on our companion computer (converted to microseconds) and that those timesteps when viewed in post processing seem to be chronological and correct.

Is stamping with the ros clock okay?
What exactly should be published to timestamp_sample?
What is the meaning of the latency plot?

ajordan5 commented 1 year ago

Interestingly I looked at a log from those who successfully flew in offboard here and their latency plot looked similar to ours with latency jumping from 0 to a very large number at a high frequency.

Jaeyoung-Lim commented 1 year ago

If we use NED, QGC says there is a yaw estimate error which I'm assuming is because it expects heading to be aligned with a true earth-based NED frame.

@ajordan5 I am not saying you should just swap the flag. you need to transform the data to be actually NED

ajordan5 commented 1 year ago

@ajordan5 I am not saying you should just swap the flag. you need to transform the data to be actually NED

@Jaeyoung-Lim gotcha, yes we are indeed transforming the raw mocap data to be NED, but we need to set the flag to 2 which is FRD otherwise it says the yaw is inconsistent. Looks like others successfully used the same flag.

mengchaoheng commented 1 year ago

Hello. I am very interested in your project, do you have a detailed introduction to this project? I also want to build a similar platform, using optitrack.

ajordan5 commented 1 year ago

Okay update here. We managed to get it working with both position control and offboard mode. The problem was that we were publishing to /fmu/in/vehicle_trajectory_setpoint when trying to fly in positon mode and the streaming setpoints were fighting with the RC inputs.

This is still a little problematic as in the past when we were using MAVSDK we were able to stream setpoints without affecting position mode. This was nice because offboard mode could fall back to position mode if setpoints failed to send or if the user gave any manual inputs. Now I need to flip back into manual mode to take back control which isn't always the most smooth transition. @Jaeyoung-Lim do you know if its possible to somehow fly in position mode with offboard setpoints streaming in the background? I'd be happy to help contribute to this if needed

ajordan5 commented 1 year ago

@mengchaoheng we are at an early stage of our project so we don't have a detailed write-up. The basic overview is that we connect our companion computer to our autopilot via telem2. The microdds agent is running on the companion computer and the client is running the autopilot on boot by setting the param XRCE_DDS_CFG: telem2.

We publish the incoming pose info from optitrack to /fmu/in/vehicle_visual_odometry once the micro-dds connection is established. I'm happy to help with specific questions! You might want to checkout this as well where someone did something very similar.

mengchaoheng commented 1 year ago

This is mu issue https://github.com/PX4/PX4-Autopilot/issues/21504 and this is my test log. http://localhost:5018/plot_app?log=/Users/mch/Proj/akstuki-PX4-ECL/PX4-ECL/log_data/log_6_2023-4-26-17-34-32.ulg

mengchaoheng commented 1 year ago

@ajordan5

mengchaoheng commented 1 year ago

about the latency, in flight_review:

# Vision latency
        data_plot = DataPlot(data, plot_config, 'vehicle_visual_odometry',
                             y_axis_label='[ms]', title='Visual Odometry Latency',
                             plot_height='small', changed_params=changed_params,
                             x_range=x_range)
        data_plot.add_graph(
            [lambda data: ('latency', 1e-3*(data['timestamp'] - data['timestamp_sample']))],
            colors3, ['VIO Latency'], mark_nan=True)
        plot_flight_modes_background(data_plot, flight_mode_changes, vtol_states)

        if data_plot.finalize() is not None: plots.append(data_plot)

So, thisVisual Odometry Latency=vehicle_visual_odometry.data.timestamp-vehicle_visual_odometry.data.timestamp_sample

ripdk12 commented 1 year ago

We publish the incoming pose info from optitrack to /fmu/in/vehicle_visual_odometry once the micro-dds connection is established.

@ajordan5 Any specific reason you publish to vehicle_visual_odometry, as opposed to vehicle_mocap_odometry ?

EDIT nvm I just realized that the EKF2 (which you probably use) only subscribes to the vehicle_visual_odometry topic, which answers my question.

ajordan5 commented 1 year ago

This is mu issue #21504 and this is my test log. http://localhost:5018/plot_app?log=/Users/mch/Proj/akstuki-PX4-ECL/PX4-ECL/log_data/log_6_2023-4-26-17-34-32.ulg

@mengchaoheng the link to your log is broken. It looks like you're using mavros on ROS1 is that right? If so, I'm sorry but I'm not really familiar with doing this with mavros. We now have a working system and the main blockers we ran into during the process were

Wrong parameters set leading to EKF2 not listening to mocap data
Incorrect or absent timestamps leading to rejection of mocap data
Sending conflicting setpoints (e.g. Trying to send trajectory setpoints for offboard in the background while flying in position mode leads to conflicting setpoints and unstable flight.)

mengchaoheng commented 1 year ago

log: https://logs.px4.io/plot_app?log=945d4e49-ef60-48b7-9618-ede2105e0ea0 @ajordan5 Yes, I am use mavros. and thanks

mengchaoheng commented 1 year ago

Hi, You mentioned earlier, "we wrote a ROS2 node that takes the Optitrack data and publishes it to the PX4 micro_dds bridge topic /fmu/in/vehicle_visual_odometry.", Could you please provide some references for this, I would like to know how these are set up? @lshums16

junwoo091400 commented 1 year ago

Okay update here. We managed to get it working with both position control and offboard mode. The problem was that we were publishing to /fmu/in/vehicle_trajectory_setpoint when trying to fly in positon mode and the streaming setpoints were fighting with the RC inputs.

This is still a little problematic as in the past when we were using MAVSDK we were able to stream setpoints without affecting position mode. This was nice because offboard mode could fall back to position mode if setpoints failed to send or if the user gave any manual inputs. Now I need to flip back into manual mode to take back control which isn't always the most smooth transition.

Thanks for debugging! Could you create a new issue to resolve this, and provide more details on how you managed to have it work with MAVSDK in the past (which commands? was it different PX4 firmware perhaps?), and what you would like to achieve? @ajordan5

PX4 / PX4-Autopilot