ModalAI Forum
    • Categories
    • Recent
    • Tags
    • Popular
    • Users
    • Groups
    • Register
    • Login

    mavlink set_position_target_local_ned does not seem to be transformed

    AprilTag Relocalization
    3
    11
    515
    Loading More Posts
    • Oldest to Newest
    • Newest to Oldest
    • Most Votes
    Reply
    • Reply as topic
    Log in to reply
    This topic has been deleted. Only users with topic management privileges can see it.
    • Matt OoiM
      Matt Ooi
      last edited by

      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 a set_position_target_local_ned does 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_ned message:

      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}")
      
      C 1 Reply Last reply Reply Quote 0
      • C
        coreyazion @Matt Ooi
        last edited by coreyazion

        @Matt-Ooi

        " 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).

        Matt OoiM 2 Replies Last reply Reply Quote 0
        • Matt OoiM
          Matt Ooi @coreyazion
          last edited by

          @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!

          1 Reply Last reply Reply Quote 0
          • Matt OoiM
            Matt Ooi @coreyazion
            last edited by

            @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_ned takes in a few different coordinate frames as per link. The one which i am using is MAV_FRAME_LOCAL_NED. There are a few other options and parameters, i am unsure if any of these affect or prevents the transformation?

            C 1 Reply Last reply Reply Quote 0
            • C
              coreyazion @Matt Ooi
              last edited by

              @Matt-Ooi

              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.

              Matt OoiM 2 Replies Last reply Reply Quote 0
              • Matt OoiM
                Matt Ooi @coreyazion
                last edited by

                @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!

                1 Reply Last reply Reply Quote 0
                • Matt OoiM
                  Matt Ooi @coreyazion
                  last edited by

                  @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.

                  1 Reply Last reply Reply Quote 0
                  • Matt OoiM
                    Matt Ooi
                    last edited by

                    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 the set_position_target_local_ned message. Could this be causing the VOXL to determine that this GCS is "disconnected" and thus not transforming the set_position_target_local_ned message?

                    1 Reply Last reply Reply Quote 0
                    • Zachary Lowell 0Z
                      Zachary Lowell 0 ModalAI Team
                      last edited by

                      @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

                      Matt OoiM 1 Reply Last reply Reply Quote 0
                      • Matt OoiM
                        Matt Ooi @Zachary Lowell 0
                        last edited by

                        @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?

                        1 Reply Last reply Reply Quote 0
                        • Zachary Lowell 0Z
                          Zachary Lowell 0 ModalAI Team
                          last edited by

                          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

                          1 Reply Last reply Reply Quote 1
                          • First post
                            Last post
                          Powered by NodeBB | Contributors