The coordinate conversion and the data of different topics.

[weiweikong]

For understanding the code detail, I draw a diagram to analysis the code.

1. About the data of vehicle_command_ENU_

• 1.1 vehicle_command_ENU_ and vehicle_setpoint_ENU_ only updated in the initialization part, initilaize_setpoint == true.
• 1.2 When each direction of joy_sp = 0， the vehicle_setpoint_ENU_ is the same as the initialized one and vehicle_command_ENU_ is simply copy position and attitude from vehicle_setpoint_ENU_. So during the real flight, without RC control, the /mavros/setpoint_position/local are same.

2. About the data of vision/pose

It takes these FLU coordinates and transforms them into ENU coordinates (fixed inertial frame) with the target at the origin.

• 2.1 the vision_pose/pose describes the vehicle position in target-ENU coordinates, whose origin is the current target. This might be means that, when target is moving, the vision_pose/pose should be changed even though the quadrotor is hovering.
• 2.2 In this code snip,

  /**
  * This function takes the FLU target pose which is relative to the vehicle and transforms it into
  * the intertial frame with the vehicle at the origin.
  * Possibly a more efficient way to do this with ROS frames
  */
  void convertTargetFLUtoENU();

the position of target convert from FLU coordinate (whose origin is the camera ) to ENU coodrinate. As the code comments said, the ENU coodrinate should be the quadrotor-fixed ENU coodrinate.
Then the updateFCULocation() should be get the quadrotor position in target-ENU coodrinate. But when the target_origin_ENU_ is (0, 0, 0)， the publish position through vision_pose should be the same as the quadrotor position as above calculated.

• Is the coordinate conversion correct ?

3. About the data of vehicle_current_ENU

• vehicle_current_ENU, which describes the local position (fusioned by the vision and IMU information) in ROS-ENU, was calculated from PX4 modules.

4. The whole process

4.1 At T = 0

• the vehicle_command_ENU_ was set after calculated vision_pose_ when the target was detected.
• the vehicle_current_ENU should not be (0, 0, 0), because it was calculated during the flight after arming the quadrotor.

4.2 At T = 1

• When the target moved to another position, the related fixed-ROS-ENU coordinate was also changed.
• However, the vehicle_command_ENU_ was keep the same vector relate to the new ROS-ENU coodrinate.
• The current /mavros/local_position/local updated as the target_pose_ changed, which is shown in the purple dashed lines.
• As the all joy_sp=0, the quadrotor should move from the last position to the proposed one, which decpited by green arrow.

I am a little bit confusing about the /mavros/local_position/local. Is the data of this topic mainly influence by /mavros/vision_pose/pose even though fusion with IMU data?

[edkoch]

See my comments in line below:

On Oct 2, 2015, at 3:57 PM, Weiwei notifications@github.com wrote:

https://cloud.githubusercontent.com/assets/2463397/10248114/2e60198a-6950-11e5-8cfe-df072195bb70.jpg
For understanding the code detail, I draw a diagram to analysis the code.

1. About the data of vehicle_command_ENU_

1.1 vehicle_command_ENU_ and vehicle_setpoint_ENU_ only updated in the initialization part, initilaize_setpoint == true.

1.2 When each direction of joy_sp = 0， the vehicle_setpoint_ENU_ is the same as the initialized one and vehicle_command_ENU_ is simply copy position and attitude from vehicle_setpoint_ENU_. So during the real flight, without RC control, the /mavros/setpoint_position/local are same.

1. About the data of vision/pose

It takes these FLU coordinates and transforms them into ENU coordinates (fixed inertial frame) with the target at the origin.

2.1 the vision_pose/pose describes the vehicle position in target-ENU coordinates, whose origin is the current target. This might be means that, when target is moving, the vision_pose/pose should be changed even though the quadrotor is hovering.
2.2 In this code snip,
/**

• This function takes the FLU target pose which is relative to the vehicle and transforms it into
• the intertial frame with the vehicle at the origin.
• Possibly a more efficient way to do this with ROS frames
  */
  void convertTargetFLUtoENU();
  the position of target convert from FLU coordinate (whose origin is the camera ) to ENU coodrinate. As the code comments said, the ENU coodrinate should be the quadrotor-fixed ENU coodrinate.
  Then the updateFCULocation() should be get the quadrotor position in target-ENU coodrinate. But when the target_origin_ENU_ is (0, 0, 0)， the publish position through vision_pose should be the same as the quadrotor position as above calculated.

Is the coordinate conversion correct ?
Yes the conversion is correct. The following happens:
(1) The target pose arrives in FLU with respect to the camera (i.e. the quadrotor).
(2) The current heading of the quadrotor which was received from the FCU and is stored in vehicle_current_ENU is used to convert the relative coordinates of the target into the fixed coordinates of the ENU frame, but the origin is still at the quadrotor. Essentially it simply rotates the coordinates so they line up with the fixed compass heading.
(3) To convert from ENU coordinate with the quadrotor at the origin to coordinates with the target at the orgin you simply have to negate the coordinates before sending them to the FCU. There is an additional origin offset that may be used in this conversion process (target_origin_ENU_), but in the version of vis_pose_test you are using it never has a value other than 0,0,0 so for the most part you can ignore it. It is there only to illustrate how you might define a different origin than one that is at the target. If you ever start tracking multiple targets you might find something like that interesting.
3. About the data of vehicle_current_ENU

vehicle_current_ENU, which describes the local position (fusioned by the vision and IMU information) in ROS-ENU, was calculated from PX4 modules.
4. The whole process

https://cloud.githubusercontent.com/assets/2463397/10248044/ababb33c-694f-11e5-9b83-1fbf72949788.png
4.1 At T = 0

the vehicle_command_ENU_ was set after calculated vision_pose_ when the target was detected.
the vehicle_current_ENU should not be (0, 0, 0), because it was calculated during the flight after arming the quadrotor.
This is true the first time you get a vehicle location from the FCU, but as soon as you start sending vision_pose/pose locations to the FCU then the locations you get back from the FCU will for the most part be the same locations you just sent the FCU. It may not be EXACTLY those locations, because there is mixing with the other sensors such as the IMU and the barometer, etc. How closely the FCU tracks the locations sent via vision_pose/pose depends upon how you set the parameters associated with the inav estimator in the PX4.
4.2 At T = 1

When the target moved to another position, the related fixed-ROS-ENU coordinate was also changed.
However, the vehicle_command_ENU_ was keep the same vector relate to the new ROS-ENU coodrinate.
The current /mavros/local_position/local updated as the target_pose_ changed, which is shown in the purple dashed lines.
As the all joy_sp=0, the quadrotor should move from the last position to the proposed one, which decpited by green arrow.
Your description sounds correct and I think your diagram depicts that, but you may be missing the crucial initialization step as described above where the vision_pose/pose topic starts to get updated which in turn changes the vehicle_current_ENU when it is fed back from the FCU. This all happens before you even start moving the target.

Essentially the position setpoint in vehicle_command_ENU_ dictates the relationship that the quadrotor should be with respect to the target as the origin. Of course moving the target means the quadrotor should also move the same amount to maintain the relationship in vehicle_command_ENU_.

I am a little bit confusing about the /mavros/local_position/local. Is the data of this topic mainly influence by /mavros/vision_pose/pose even though fusion with IMU data?

Yes. See the above comment. What you will notice is that if the values in /mavros/vision_pose/pose change suddenly and the quadrotor does not actually move then it may take a little time for the /mavros/local_position/local values converge to the new position because it is being mixed with the IMU. How quickly it converges is dependent upon the coefficients in inav.

—
Reply to this email directly or view it on GitHub #4.

[weiweikong]

Hi @edkoch , your quick replies and codes were really helpful for me to understand the OFFBOARD local position control.

I have already tested my PX4 with TK1 running under OFFBOARD control. But my control is based on GPS and set the local position based on it. I would like to use the camera instead the GPS as the local position descriptor and doing further research. Thank you!