Clarification regarding frames of reference of position controller

[luccagandra]

Hello! I'm not sure how to frame it, but I just want to know how to direct my solution to the competition. The problem is the following: I'm sending messages directly to position_hold/trajectory topic. The problem is that my drone is heading to a different location. After some search, I came to the conclusion that the frames of reference were different. Please correct me if i'm wrong at any point with these conclusions.

The messages were recorded with the drone hovering in the air, at start position.

There is a discrepancy on those. In global_position, drone is at (0,1,2, Xrot=0,Yrot=0,Zrot=90), in odometry, at (-10,0,2, Xrot=0,Yrot=0,Zrot=0)

I'm trying to send waypoints in the world frame, but I think that the position controller is interpreting it at map coordinate frame. This because the odometry arg at pid_carrot.launch is set as <arg name="odometry" default="mavros/global_position/local"/>. So, overall, my problem is that when I send a goal pose, the drone heads to another direction.

After publishing this message (Drone was at (-10,0,2)): rostopic pub /red/tracker/input_pose geometry_msgs/PoseStamped '{header: {stamp: now, frame_id: "world"}, pose: {position: {x: 8.0, y: 0.0, z: 2.0}, orientation:{w: 1.0}}}'

The response of the drone was: instead of heading north to (x: 8.0, y: 0.0, z: 2.0), it went east, and crashed into the wall.

So, in conclusion, can I change odometry argument at pid_carrot.launch? Also, in which frame do I need to publish the message that reconstruct the position of the alvar_tile?

/odometry:
header: 
  seq: 2
  stamp: 
    secs: 3688
    nsecs: 466000000
  frame_id: "world"
child_frame_id: "red/base_link"
pose: 
  pose: 
    position: 
      x: -9.99525591906669
      y: 0.007577561064916776
      z: 2.389472754893611
    orientation: 
      x: 0.000348626340673066
      y: -0.00011237404801617793
      z: -0.009345777317617009
      w: 0.999956260182453
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: 0.004403927582612187
      y: -0.0008191278020681579
      z: -0.001630612307862894
    angular: 
      x: -0.004004426650456003
      y: -0.00031597869552088103
      z: 0.0006617724010358379
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

mavros/global_position/local:
header: 
  seq: 16515
  stamp: 
    secs: 3682
    nsecs: 989000000
  frame_id: "map"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: -0.009089141470354889
      y: 1.716102815407794e-10
      z: 2.009
    orientation: 
      x: -0.0034713829052463617
      y: 0.004853540375286878
      z: -0.7065561147413258
      w: -0.7076318600573275
  covariance: [0.04000000000000001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04000000000000001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.04000000000000001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 99999.0]
twist: 
  twist: 
    linear: 
      x: -0.01
      y: 0.02
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [-1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]```

[fpetric]

With respect to the alvar tile position, it should be published in the global reference frame. In Gazebo, that is the world reference frame. For that, and I suspect that it may resolve control issues since it will enable the transformation of the waypoint from the world reference frame to the mavros frame, we need to supply the transform between the world frame and the mavros frame but @lmark1 will have more information on the control side of the issue.

[lmark1]

Hello Lucca,

The UAV is currently controlled using mavros/global_position/local feedback, which is different from the Gazebo odometry topic. It would probably make sense to have a single origin frame.

To change the tracking frame to Gazebo odometry topic please check out this pull request https://github.com/larics/icuas22_competition/pull/10. You may have to pull the base Docker image as shown here.

[luccagandra]

Thank you both for the help! It's working now.

[bruno-gpa]

Hi @lmark1. While running the simulation on branch no-gps, a problem occurs: ardupilot is not receiving the home position. So, the quad does not arm and, consequently, does not fly.

Detected vehicle 1:1 on link 0
STABILIZE> Mode STABILIZE
AP_IRLock_SITL::init()
AP: Barometer 1 calibration complete
Init Gyro**ftp open failed
Got COMMAND_ACK: REQUEST_AUTOPILOT_CAPABILITIES: ACCEPTED
AP: ArduCopter V4.0.7 (4de142fc)
AP: 00f16c6bba14f456e26396e461fc826b
AP: Frame: QUAD
*Received 1197 parameters
Saved 1197 parameters to mav.parm
Ready to FLY Got COMMAND_ACK: REQUEST_AUTOPILOT_CAPABILITIES: ACCEPTED
Got COMMAND_ACK: REQUEST_AUTOPILOT_CAPABILITIES: ACCEPTED
GUIDED_NOGPS> Mode GUIDED_NOGPS
AP: EKF2 IMU0 tilt alignment complete
AP: EKF2 IMU1 tilt alignment complete
AP: ArduCopter V4.0.7 (4de142fc)
AP: 00f16c6bba14f456e26396e461fc826b
AP: Frame: QUAD
Got COMMAND_ACK: GET_HOME_POSITION: FAILED
Flight battery 100 percent
Got COMMAND_ACK: GET_HOME_POSITION: FAILED
Got COMMAND_ACK: GET_HOME_POSITION: FAILED
Got COMMAND_ACK: GET_HOME_POSITION: FAILED
Got COMMAND_ACK: GET_HOME_POSITION: FAILED

[lmark1]

Hello @bruno-gpa ,

The home position is not received since the GPS should be disabled on the no-gps branch.

The drone should still be able to takeoff. Please try updateing the base simulation stack by following the instructions here. If the problem persists please open a separate issue describing your problems.