By using vision_pose, how can I stop local_position/local from updating using other sensors?

harewei commented 8 years ago

Hello

Currently I'm using OFFBOARD mode in gazebo simulator for position control. With mavros/setpoint_position/local, I can control the position of the vehicle. Now, I'm publishing to the topic mavros/vision_pose/pose in http://wiki.ros.org/mavros_extras#vision_pose_estimate as follows

ros::Publisher vision_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/vision_pose/pose", 10);
geometry_msgs::PoseStamped fake_pose;
fake_pose.pose.position.x = 0;
fake_pose.pose.position.y = 0;
fake_pose.pose.position.z = 2;
vision_pos_pub.publish(fake_pose);

The mavros node does not give any error or confirmation, so I tried to echo the topic mavros/local_position/local, but the positions do not match the values I publish through vision_pose. Is this because the vehicle's local position is bound to the gazebo simulated world, or does vision_pose not suppose to change its local position?

I'm doing this simulation because later on, I wish to implement a vision system on the copter for indoor movements.

I've read on http://dev.px4.io/external-position.html that I need to set two parameters for enabling external pose input, which I have not done so because I don't know how to do it through mavros code. If this is the problem, can someone tell me how to?

Thank you.

EDIT: I forgot to give the header

fake_pose.header.stamp = ros::Time::now();

mavros/local_position/local does indeed to be changing to my values in vision_pose, but it keeps decreasing. To illustrate, my vehicle is at (0,0,0) and vision_pose is set as (5,0,2) when I display its local pose, I get

header: 
  seq: 194815
  stamp: 
    secs: 1453174737
    nsecs: 101720448
  frame_id: fcu
pose: 
  position: 
    x: 4.94869422913
    y: 0.0107232546434
    z: 1.99170863628
  orientation: 
    x: 0.249517752963
    y: -0.0471021631409
    z: 0.769283859966
    w: 0.586280325424

---
header: 
  seq: 194816
  stamp: 
    secs: 1453174737
    nsecs: 139778560
  frame_id: fcu
pose: 
  position: 
    x: 4.85067892075
    y: 0.028346111998
    z: 1.9479521513
  orientation: 
    x: 0.248702027537
    y: -0.0481760988165
    z: 0.768570199812
    w: 0.587474436008

---
header: 
  seq: 194817
  stamp: 
    secs: 1453174737
    nsecs: 163778560
  frame_id: fcu
pose: 
  position: 
    x: 4.78933334351
    y: 0.0373524688184
    z: 1.91836643219
  orientation: 
    x: 0.247990310631
    y: -0.0491599409153
    z: 0.767841426681
    w: 0.588645606044

---
header: 
  seq: 194818
  stamp: 
    secs: 1453174737
    nsecs: 200778560
  frame_id: fcu
pose: 
  position: 
    x: 4.6962518692
    y: 0.0478611886501
    z: 1.87021708488
  orientation: 
    x: 0.247225511053
    y: -0.0502318565973
    z: 0.767075588609
    w: 0.589874010809

---
header: 
  seq: 194819
  stamp: 
    secs: 1453174737
    nsecs: 236819264
  frame_id: fcu
pose: 
  position: 
    x: 4.60733270645
    y: 0.0542828068137
    z: 1.8201829195
  orientation: 
    x: 0.246439027416
    y: -0.0514939958984
    z: 0.766174850136
    w: 0.591263285832

---
header: 
  seq: 194820
  stamp: 
    secs: 1453174737
    nsecs: 261819264
  frame_id: fcu
pose: 
  position: 
    x: 4.55269241333
    y: 0.0620739646256
    z: 1.78350627422
  orientation: 
    x: 0.245725932793
    y: -0.0525117074863
    z: 0.765410010886
    w: 0.592459957942

---
header: 
  seq: 194821
  stamp: 
    secs: 1453174737
    nsecs: 301819264
  frame_id: fcu
pose: 
  position: 
    x: 4.47959661484
    y: 0.0804646536708
    z: 1.722458601
  orientation: 
    x: 0.244938254219
    y: -0.0535362235845
    z: 0.764486714988
    w: 0.59388482637

---
header: 
  seq: 194822
  stamp: 
    secs: 1453174737
    nsecs: 338705856
  frame_id: fcu
pose: 
  position: 
    x: 4.41278362274
    y: 0.0943420231342
    z: 1.66188287735
  orientation: 
    x: 0.244185470509
    y: -0.0544645704199
    z: 0.763681147387
    w: 0.595145504635

---
header: 
  seq: 194823
  stamp: 
    secs: 1453174737
    nsecs: 374705856
  frame_id: fcu
pose: 
  position: 
    x: 4.35053110123
    y: 0.104449227452
    z: 1.6009837389
  orientation: 
    x: 0.243456628131
    y: -0.0553405131374
    z: 0.762860215571
    w: 0.596414779598

So basically it keeps decreasing then returns to (5,0,2) again. I'm guessing this is because the vehicle is at (0,0,0) in the simulated world. How can I stop this from happening by suppressing it?

UPDATE: I tried it on a real copter, the x and y value work as expected, but even though the z value changes for a split second, it returns to the real world value very quickly. This is possibly due to z being updated by barometer or something. So the question remains the same, how can I disable all xyz value updates by the copter and only accept those from vision_pose_estimate?

UPDATE 2: I've found that I have to edit the INAV parameters. However, whether I follow the parameters in https://github.com/AutoModality/vis_pose_test/issues/2 , or by setting all INAV parameters in https://pixhawk.org/firmware/parameters#position_estimator_inav to 0 except those involving vision, the local position published are still doing weird things in gazebo simulator. It would be great if someone can give me their parameter settings in order to tell the vehicle's position purely based on vision estimate.

Jaeyoung-Lim commented 8 years ago

I guess in the gazebo simulation, you are not giving any visual feedback(e.g. cameras) for the position estimator to have reference

You should publish the proper vision data so the vision estimate can have a proper estimate in position

LorenzMeier commented 8 years ago

The gazebo simulation for PX4 is publishing optical flow.

Jaeyoung-Lim commented 8 years ago

@LorenzMeier Thanks for correcting comment :)

Thomas00010111 commented 8 years ago

Hi @harewei , I am facing the same issue at the moment. I am running gazebo with SITL and would like to update the current position by publishing data to mavros/vision_pose/pose. The idea is to use some sort of slam algorithm later to estimate the position in the future. When echoing mavros/local_position/local the position does not change. The message I am sending should be correct I am sending the same message like you. How did you solve this issue?

Thomas

Edit: Works now.

had to start inav manually by position_estimator_inav start
added some warnx() commands for debugging
orb_check returned false
compiled code several times and suddenly local position was updated
guess some files weren't compiled/linked correctly

weiheng209 commented 8 years ago

Hi @Thomas00010111
i have the same problem. Have you already solved it? can you tell me how did you solve this issue?

NCharabaruk commented 7 years ago

I am also having this issue using LPE

trkygt commented 7 years ago

It seems we have some people who have the same problem including me.

khoindq commented 7 years ago

@NCharabaruk How can use solve by using LPE this is my c code but local pose does not change anything

mavlink / mavros

By using vision_pose, how can I stop local_position/local from updating using other sensors? #475