mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
879 stars 990 forks source link

Getting local pose in multi-UAV simulation #1723

Open Hs293Go opened 2 years ago

Hs293Go commented 2 years ago

I'm running multi-UAV simulation via roslaunch px4 multi_uav_mavros_sitl.launch. I'm used to using /local_position/pose to provide feedback for a tracking controller. However, at simulation start, the local position readouts from each of /uav[012]/mavros/local_position are close to [0; 0; 0] as shown below. This runs contrary to the expected positions of each uav specified in multi_uav_mavros_sitl.launch, i.e. [0,0,0] for uav0, [1,0,0] for uav1, [0,1,0] for uav2

Data from /uav0/mavros/local_position

## don't care about header
  position: 
    x: -0.014205143787
    y: 0.00331291183829
    z: 0.00626456504688
## don't care about orientation

Data from /uav1/mavros/local_position

## don't care about header
  position: 
    x: 0.0107134236023
    y: -0.00930605269969
    z: 0.0248154960573
## don't care about orientation

Data from /uav2/mavros/local_position

## don't care about header
  position: 
    x: 0.0190009418875
    y: -0.00686375750229
    z: -0.132964879274
## don't care about orientation

I am aware that local_position is computed with respected to some global frame (named map ??). What I assume now is that each UAV's local_position/pose is calculated w.r.t. its own global frame, whose origin is set to its own position at simulation start. The issue now is whether it is possible to calculate every UAV's local position in the same, global, frame. I have attempted to use /mavros/cmd/set_home, to no effect.

Writing a frame transformation function to resolve every UAV's pose in the same frame appears to be feasible, but I still hope that mavros provides this functionality so i don't need to code myself :)

WinstonJQ commented 1 month ago

Hello, I also met the same problem, may I ask how you finally solved it?

Hs293Go commented 1 month ago

I found out that Mavros exposes /mavros/global_position/set_gp_origin, and you can publish your required home coordinates and mavros/local_position/Pose will be expressed w.r.t. that home