mavlink set_position_target_local_ned does not seem to be transformed
-
Hi ModalAI Team,
I am using the VOXL Cam on an indoor Ardupilot based drone. It has been successful so far with short (distance) auto and manual flights. However, apriltag relocalization would be the next step into making this solution more robust for longer flights.I have been following the documentation on Apriltag Detection & Relocalization, as well as Mavlink for the UDP connection to the VOXL Cam.
This is the issue:
While in Guided mode, sending aset_position_target_local_neddoes not seem to be transformed, despite the VOXL being able to detect the Aruco code on the ground.
Just like your sample video in Relocalization, I took the drone off a few meters away from the Aruco code (EKF home position) without a view of the Aruco code. After flying manually to place the aruco code in view, i change the mode to guided, and sent a mavlink message to go to (0,0,-1). Instead of heading to the aruco code, it flies to the EKF home where i took-off from and hovers there.I have included some version information, configurations, and code snippets below.
I hope that someone could let me know what is wrong with my current setup.
voxl:~$ voxl-version -------------------------------------------------------------------------------- system-image: 4.0.0 kernel: #1 SMP PREEMPT Thu Oct 13 17:57:05 UTC 2022 3.18.71-perf -------------------------------------------------------------------------------- hw platform: VOXL mach.var: 1.0 -------------------------------------------------------------------------------- voxl-suite: 1.2.0 -------------------------------------------------------------------------------- Packages: Repo: http://voxl-packages.modalai.com/dists/apq8096/sdk-1.2/binary-arm64/ Last Updated: Never List: apq8096-dfs-server 0.3.1 apq8096-imu-server 1.1.0 apq8096-libpng 1.6.38-1 apq8096-rangefinder-server 0.1.3 apq8096-system-tweaks 0.2.3 apq8096-tflite 2.8.3-1 libapq8096-io 0.6.1 libmodal-cv 0.4.0 libmodal-exposure 0.1.0 libmodal-journal 0.2.2 libmodal-json 0.4.3 libmodal-pipe 2.10.0 libvoxl-cci-direct 0.2.1 libvoxl-cutils 0.1.1 voxl-camera-calibration 0.5.4 voxl-camera-server 1.9.2 voxl-configurator 0.5.6 voxl-cpu-monitor 0.4.8 voxl-docker-support 1.3.1 voxl-gphoto2-server 0.0.10 voxl-jpeg-turbo 2.1.3-5 voxl-lepton-server 1.2.0 voxl-libgphoto2 0.0.4 voxl-libuvc 1.0.7 voxl-logger 0.4.0 voxl-mavcam-manager 0.5.3 voxl-mavlink 0.1.1 voxl-mavlink-server 1.4.0 voxl-modem 1.0.9 voxl-mongoose 7.7.0-1 voxl-mpa-to-ros 0.3.7 voxl-mpa-tools 1.1.5 voxl-opencv 4.5.5-2 voxl-portal 0.6.5 voxl-qvio-server 1.0.2 voxl-remote-id 0.0.9 voxl-streamer 0.7.4 voxl-suite 1.2.0 voxl-system-image 4.0-r0 voxl-tag-detector 0.0.4 voxl-tflite-server 0.3.2 voxl-utils 1.3.9 voxl-uvc-server 0.1.6 voxl-vision-hub 1.7.4 voxl-vpn 0.0.6 --------------------------------------------------------------------------------voxl:~$ voxl-inspect-services Scanning services... Service Name | Enabled | Running | CPU Usage ------------------------------------------------------------------- docker-autorun | Disabled | Not Running | docker-daemon | Disabled | Not Running | modallink-relink | Disabled | Not Running | voxl-camera-server | Enabled | Running | 6.2% voxl-cpu-monitor | Enabled | Running | 0.0% voxl-dfs-server | Disabled | Not Running | voxl-imu-server | Enabled | Running | 0.0% voxl-lepton-server | Disabled | Not Running | voxl-mavcam-manager | Enabled | Running | 0.0% voxl-mavlink-server | Enabled | Running | 0.0% voxl-modem | Disabled | Not Running | voxl-portal | Enabled | Running | 0.0% voxl-qvio-server | Enabled | Running | 8.3% voxl-rangefinder-server | Disabled | Not Running | voxl-remote-id | Disabled | Not Running | voxl-streamer | Enabled | Running | 0.0% voxl-tag-detector | Enabled | Running | 2.0% voxl-tflite-server | Disabled | Not Running | voxl-time-sync | Disabled | Not Running | voxl-uvc-server | Disabled | Not Running | voxl-vision-hub | Enabled | Running | 0.0% voxl-wait-for-fs | Enabled | Completed |voxl-tag-detector.conf:
/** * This file contains configuration parameters for voxl-tag-detector. * You can specify up to 3 cameras to do detection on simultaneously. * For the stereo camera pair, only the left camera is used. */ { "detector_0": { "enable": true, "input_pipe": "tracking", "en_fast_mode": true, "n_threads": 1, "en_undistortion": true, "undistort_scale": 0.600000023841858, "overlay_name": "tracking_tag_overlay", "lens_cal_file": "/data/modalai/opencv_tracking_intrinsics.yml", "skip_n_frames": 5 }, "detector_1": { "enable": false, "input_pipe": "stereo", "en_fast_mode": true, "n_threads": 1, "en_undistortion": true, "undistort_scale": 0.899999976158142, "overlay_name": "stereo_tag_overlay", "lens_cal_file": "/data/modalai/opencv_stereo_intrinsics.yml", "skip_n_frames": 5 }, "detector_2": { "enable": false, "input_pipe": "extra", "en_fast_mode": true, "n_threads": 1, "en_undistortion": false, "undistort_scale": 1, "overlay_name": "extra_tag_overlay", "lens_cal_file": "/data/modalai/opencv_extra_intrinsics.yml", "skip_n_frames": 5 } }voxl-vision-hub.conf:
{ "config_file_version": 1, "en_localhost_mavlink_udp": false, "localhost_udp_port_number": 14551, "en_vio": true, "vio_pipe": "qvio", "secondary_vio_pipe": "ov", "en_reset_vio_if_initialized_inverted": true, "vio_warmup_s": 3, "send_odom_while_failed": true, "en_set_clock_from_gps": true, "en_force_onboard_mav1_mode": true, "en_reset_px4_on_error": true, "horizon_cal_tolerance": 0.5, "offboard_mode": "off", "follow_tag_id": 1, "figure_eight_move_home": true, "robot_radius": 0.300000011920929, "collision_sampling_dt": 0.1, "max_lookahead_distance": 1, "en_tag_fixed_frame": true, "fixed_frame_filter_len": 5, "en_transform_mavlink_pos_setpoints_from_fixed_frame": true, "en_voa": true, "voa_upper_bound_m": -0.15000000596046448, "voa_lower_bound_m": 0.15000000596046448, "voa_voa_memory_s": 1, "voa_max_pc_per_fusion": 100, "voa_pie_max_dist_m": 20, "voa_pie_min_dist_m": 0.25, "voa_pie_under_trim_m": 1, "voa_pie_threshold": 3, "voa_send_rate_hz": 20, "voa_pie_slices": 36, "voa_pie_bin_depth_m": 0.15000000596046448, "voa_inputs": [{ "enabled": true, "type": "point_cloud", "input_pipe": "dfs_point_cloud", "frame": "stereo_l", "max_depth": 8, "min_depth": 0.300000011920929, "cell_size": 0.079999998211860657, "threshold": 4, "x_fov_deg": 68, "y_fov_deg": 56, "conf_cutoff": 0 }, { "enabled": true, "type": "point_cloud", "input_pipe": "stereo_front_pc", "frame": "stereo_front_l", "max_depth": 8, "min_depth": 0.300000011920929, "cell_size": 0.079999998211860657, "threshold": 4, "x_fov_deg": 68, "y_fov_deg": 56, "conf_cutoff": 0 }, { "enabled": true, "type": "point_cloud", "input_pipe": "stereo_rear_pc", "frame": "stereo_rear_l", "max_depth": 8, "min_depth": 0.300000011920929, "cell_size": 0.079999998211860657, "threshold": 4, "x_fov_deg": 68, "y_fov_deg": 56, "conf_cutoff": 0 }, { "enabled": true, "type": "tof", "input_pipe": "tof", "frame": "tof", "max_depth": 6, "min_depth": 0.15000000596046448, "cell_size": 0.079999998211860657, "threshold": 3, "x_fov_deg": 106.5, "y_fov_deg": 85.0999984741211, "conf_cutoff": 125 }, { "enabled": true, "type": "rangefinder", "input_pipe": "rangefinders", "frame": "body", "max_depth": 8, "min_depth": 0.300000011920929, "cell_size": 0.079999998211860657, "threshold": 4, "x_fov_deg": 68, "y_fov_deg": 56, "conf_cutoff": 0 }] }tag_locations.conf:
{ "locations": [{ "id": 0, "name": "default_name", "loc_type": "fixed", "size_m": 0.16, "T_tag_wrt_fixed": [0, 0, 0], "R_tag_to_fixed": [[0, -1, 0], [1, 0, 0], [0, 0, 1]] }], "default_size_m": 0.16 }Python code used to send the
set_position_target_local_nedmessage:def set_position_target_local_ned(self, n, e, d): self.vehicle.mav.set_position_target_local_ned_send( 0, self.vehicle.target_system, self.vehicle.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED, 0b0000111111111000, n, e, d, 0, 0, 0, 0, 0, 0, 0, 0 ) print(f"Position set to: N={n}, E={e}, D={d}") -
" I took the drone off a few meters away from the Aruco code (EKF home position) without a view of the Aruco code. After flying manually to place the aruco code in view, "
What helped me is that at this stage I will often do voxl-inspect-pose -f. This will tell you the drone's current pose in fixed frame. Usually when this is correct and I have these enabled (as you already did in the tag_locations.conf ) :
"en_tag_fixed_frame": true,
"en_transform_mavlink_pos_setpoints_from_fixed_frame": true,then the drone will fly to that coordinate in fixed_frame instead of in ekf_frame.
If the result of voxl-inspect-pose -f is not as expected (either the same as ekf_frame or comepletely off from where it should be), you can launch voxl-tag-detector or voxl-vision-hub with the debug tag to see what is wrong (type voxl-tag-detector -h in the terminal will reveal some info for launch option).
-
@coreyazion Thank you for this, I have used the web GUI to inspect the fixed frame pose; it does "jump" in position as soon as it sees the Aruco code, which is why i thought the only thing that may not be working was the transformation.
Nonetheless, I will test this out right now and get back to you!
-
@coreyazion I have just tested the
voxl-inspect-pose -f, the results are as they should be. Is it possible that my mavlink command may be incorrect? From my understanding,set_position_target_local_nedtakes in a few differentcoordinate framesas per link. The one which i am using isMAV_FRAME_LOCAL_NED. There are a few other options and parameters, i am unsure if any of these affect or prevents the transformation? -
You seem to be doing everything right, but I am using MAVROS with PX4 maybe there's some subtle difference in the implementation?
One weird error I recall from my memory is that there was a time that I was also doing everything right but the relocalization just not work. (But the symptom is voxl-inspect-pose -f give me the ekf_frame coordinates). What magically worked is that I delete the tag_locations.conf and voxl-tag-detector.conf, reset voxl-tag-detector using voxl-configure-tag-detector, and then it worked. But this may very well not be your case maybe it was just some ill-formed json for me.
-
@coreyazion I would be using MAVROS eventually as well, pymavlink is just a quick and easy way for me to test this out, I would assume that they would be sending the same mavlink message to the VOXL. As for PX4 vs Ardupilot, i would also assume that they would be the same, as the transformation would be within VOXL instead of the AP; if my drone is still reacting to the commands, but going to the EKF origin instead of the Aruco code, the problem may be within the voxl. This is what i think at least.
I will give a go at resetting and reinitializing everything in the configs! Hope it works!
-
@coreyazion Just did another flight test after performing voxl-configure-vision-hub wizard and voxl-configure-tag-detector. Unfortunately the results were still the same and seems like the transformation is not happening despite fixed frame being offset in all debug modes I've tried.
I might try to use MAVROS next time since it worked for you. There is a slight chance that it may be the issue. -
Hi ModalAI team,
I have a suspicion to this issue. Firstly, i am connected to the VOXL via UDP, but i am not sending any heartbeats from my GCS (laptop); i am only sending theset_position_target_local_nedmessage. Could this be causing the VOXL to determine that this GCS is "disconnected" and thus not transforming theset_position_target_local_nedmessage? -
@Matt-Ooi said in mavlink set_position_target_local_ned does not seem to be transformed:
set_position_target_local_ned
@Matt-Ooi so this is actually intrinsicly a huge difference between px4 and ardupilot - PX4 functions in NED as well as ardupilot - BUT if you are using MAVROS to communicate to ardupilot then it leverages the ENU/FLU coordinate system which is a massive pain in transofrmations which is why you are seeing this error - you will need to manually do the transofmraiton your self before streaming it into mavros to match its right coordiante system.
Zach
-
@Zachary-Lowell-0 Hi! Thanks for the response. Im not sure if i am understanding correctly. Are you saying that the VOXL SDK would not compute the transformations for Ardupilot? And that i would have to compute those transformations myself within my GCS before sending the guided commands?
-
Spot on @Matt-Ooi - the voxl2 is natively meant to handle transformations for ardupilot and px4 - BUT you are using MAVROS which is a different tranformation all together - hence why you would have to do the transofmratio yourself before sending into the flight stack running ardupilot (aka when calling mavros offboard) or something
