Skip to content
New issue

Have a question about this project? # for a free GitHub account to open an issue and contact its maintainers and the community.

#

By clicking “#”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? # to your account

Jump to bottom

local_position/pose unable to adjust properly when /gazebo/set_model_state is called #754

Closed
schmittlema opened this issue Jul 13, 2017 · 10 comments
Closed

local_position/pose unable to adjust properly when /gazebo/set_model_state is called #754

schmittlema opened this issue Jul 13, 2017 · 10 comments
Labels
question SITL

Comments

@schmittlema
Copy link

schmittlema commented Jul 13, 2017 •
edited
Loading

This is only bug and feature tracker, please use it
to report bugs or request features.

Issue details

When I call /gazebo/set_model_state to reset my model to 0,0 the local position does not hop to 0,0. Instead it slowly moves down.
For Example:
My drone is at 25,25 (x,y) and I reset it to 0,0. I keep printing the local position and instead of it reading 0,0 it reads 25,25 24,24 23,23 etc etc. until it reaches 0,0.

Is this a bug or am I implementing the local_position topic wrong?
Thank you for any help you can provide!

MAVROS version and platform

Mavros: 0.19.0
ROS: Kinetic
Ubuntu: 16.04

Autopilot type and version

[ ] ArduPilot
[x] PX4

Version: 1.0.0

Node logs

INFO  [dataman] Unkown restart, data manager file 'rootfs/fs/microsd/dataman' size is 103090 bytes
INFO  [platforms__posix__drivers__ledsim] LED::init
INFO  [platforms__posix__drivers__ledsim] LED::init
INFO  [simulator] Not using /dev/ttyACM0 for radio control input. Assuming joystick input via MAVLink.
INFO  [simulator] Waiting for initial data on UDP port 14560. Please start the flight simulator to proceed..
process[gazebo-2]: started with pid [2537]
process[vehicle_spawn_59d5e7024476_2400_2780903416454810023-3]: started with pid [2546]
[ INFO] [1499950034.561136368]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1499950034.561821757]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
process[mavros-4]: started with pid [2582]
SpawnModel script started
[INFO] [1499950034.692981, 0.000000]: Loading model XML from file
[INFO] [1499950034.693421, 0.000000]: Waiting for service /gazebo/spawn_sdf_model
[ INFO] [1499950034.706463448]: FCU URL: udp://:14540@localhost:14557
[ WARN] [1499950034.707644298]: init: message from ASLUAV, MSG-ID 201 ignored! Table has different entry.
[ WARN] [1499950034.708014755]: init: message from autoquad, MSG-ID 150 ignored! Table has different entry.
[ WARN] [1499950034.708077204]: init: message from autoquad, MSG-ID 152 ignored! Table has different entry.
[ WARN] [1499950034.708348147]: init: message from matrixpilot, MSG-ID 150 ignored! Table has different entry.
[ WARN] [1499950034.708384201]: init: message from matrixpilot, MSG-ID 151 ignored! Table has different entry.
[ WARN] [1499950034.708401564]: init: message from matrixpilot, MSG-ID 152 ignored! Table has different entry.
[ WARN] [1499950034.708419553]: init: message from matrixpilot, MSG-ID 153 ignored! Table has different entry.
[ WARN] [1499950034.708448537]: init: message from matrixpilot, MSG-ID 155 ignored! Table has different entry.
[ WARN] [1499950034.708468509]: init: message from matrixpilot, MSG-ID 156 ignored! Table has different entry.
[ WARN] [1499950034.708514241]: init: message from matrixpilot, MSG-ID 157 ignored! Table has different entry.
[ WARN] [1499950034.708534678]: init: message from matrixpilot, MSG-ID 158 ignored! Table has different entry.
[ WARN] [1499950034.708552518]: init: message from matrixpilot, MSG-ID 170 ignored! Table has different entry.
[ WARN] [1499950034.708570465]: init: message from matrixpilot, MSG-ID 171 ignored! Table has different entry.
[ WARN] [1499950034.708594187]: init: message from matrixpilot, MSG-ID 172 ignored! Table has different entry.
[ WARN] [1499950034.708622454]: init: message from matrixpilot, MSG-ID 173 ignored! Table has different entry.
[ WARN] [1499950034.708640543]: init: message from matrixpilot, MSG-ID 174 ignored! Table has different entry.
[ WARN] [1499950034.708659387]: init: message from matrixpilot, MSG-ID 175 ignored! Table has different entry.
[ WARN] [1499950034.708677082]: init: message from matrixpilot, MSG-ID 176 ignored! Table has different entry.
[ WARN] [1499950034.708696116]: init: message from matrixpilot, MSG-ID 177 ignored! Table has different entry.
[ WARN] [1499950034.708714912]: init: message from matrixpilot, MSG-ID 178 ignored! Table has different entry.
[ WARN] [1499950034.708737161]: init: message from matrixpilot, MSG-ID 179 ignored! Table has different entry.
[ WARN] [1499950034.708758287]: init: message from matrixpilot, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1499950034.708777400]: init: message from matrixpilot, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1499950034.708812526]: init: message from matrixpilot, MSG-ID 182 ignored! Table has different entry.
[ WARN] [1499950034.708837819]: init: message from matrixpilot, MSG-ID 183 ignored! Table has different entry.
[ WARN] [1499950034.708857470]: init: message from matrixpilot, MSG-ID 184 ignored! Table has different entry.
[ WARN] [1499950034.708870050]: init: message from matrixpilot, MSG-ID 185 ignored! Table has different entry.
[ WARN] [1499950034.708881738]: init: message from matrixpilot, MSG-ID 186 ignored! Table has different entry.
[ WARN] [1499950034.709082816]: init: message from paparazzi, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1499950034.709103222]: init: message from paparazzi, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1499950034.709120640]: init: message from paparazzi, MSG-ID 182 ignored! Table has different entry.
[ WARN] [1499950034.709137280]: init: message from paparazzi, MSG-ID 183 ignored! Table has different entry.
[ WARN] [1499950034.709154038]: init: message from paparazzi, MSG-ID 184 ignored! Table has different entry.
[ WARN] [1499950034.709343341]: init: message from slugs, MSG-ID 170 ignored! Table has different entry.
[ WARN] [1499950034.709365399]: init: message from slugs, MSG-ID 172 ignored! Table has different entry.
[ WARN] [1499950034.709385143]: init: message from slugs, MSG-ID 173 ignored! Table has different entry.
[ WARN] [1499950034.709420357]: init: message from slugs, MSG-ID 176 ignored! Table has different entry.
[ WARN] [1499950034.709441871]: init: message from slugs, MSG-ID 177 ignored! Table has different entry.
[ WARN] [1499950034.709460805]: init: message from slugs, MSG-ID 179 ignored! Table has different entry.
[ WARN] [1499950034.709475309]: init: message from slugs, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1499950034.709493338]: init: message from slugs, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1499950034.709524676]: init: message from slugs, MSG-ID 184 ignored! Table has different entry.
[ WARN] [1499950034.709546261]: init: message from slugs, MSG-ID 185 ignored! Table has different entry.
[ WARN] [1499950034.709564830]: init: message from slugs, MSG-ID 186 ignored! Table has different entry.
[ WARN] [1499950034.709581704]: init: message from slugs, MSG-ID 188 ignored! Table has different entry.
[ WARN] [1499950034.709602615]: init: message from slugs, MSG-ID 191 ignored! Table has different entry.
[ WARN] [1499950034.709619613]: init: message from slugs, MSG-ID 192 ignored! Table has different entry.
[ WARN] [1499950034.709647452]: init: message from slugs, MSG-ID 193 ignored! Table has different entry.
[ WARN] [1499950034.709702551]: init: message from slugs, MSG-ID 194 ignored! Table has different entry.
[ INFO] [1499950034.710502308]: udp0: Bind address: 0.0.0.0:14540
[ INFO] [1499950034.710645762]: udp0: Remote address: 127.0.0.1:14557
[ INFO] [1499950034.710905134]: GCS bridge disabled
[ INFO] [1499950034.784242721]: Plugin 3dr_radio loaded
[ INFO] [1499950034.787034193]: Plugin 3dr_radio initialized
[ INFO] [1499950034.787162843]: Plugin actuator_control loaded
[ INFO] [1499950034.790872899]: Plugin actuator_control initialized
[ INFO] [1499950034.829170689]: Plugin adsb loaded
[ INFO] [1499950034.834276391]: Plugin adsb initialized
[ INFO] [1499950034.834458474]: Plugin altitude loaded
[ INFO] [1499950034.835865878]: Plugin altitude initialized
[ INFO] [1499950034.836001525]: Plugin cam_imu_sync loaded
[ INFO] [1499950034.836861676]: Plugin cam_imu_sync initialized
[ INFO] [1499950034.837056254]: Plugin command loaded
[ INFO] [1499950034.844007738]: Plugin command initialized
[ INFO] [1499950034.844049461]: Plugin distance_sensor blacklisted
[ INFO] [1499950034.844291672]: Plugin ftp loaded
[ INFO] [1499950034.854520012]: Plugin ftp initialized
[ INFO] [1499950034.854686172]: Plugin global_position loaded
[ INFO] [1499950034.876604187]: Plugin global_position initialized
[ INFO] [1499950034.876756811]: Plugin hil_actuator_controls loaded
[ INFO] [1499950034.877709147]: Plugin hil_actuator_controls initialized
[ INFO] [1499950034.877858361]: Plugin hil_controls loaded
[ INFO] [1499950034.878808444]: Plugin hil_controls initialized
[ INFO] [1499950034.878965668]: Plugin home_position loaded
[ INFO] [1499950034.884622654]: Plugin home_position initialized
[ INFO] [1499950034.884776986]: Plugin imu_pub loaded
[ INFO] [1499950034.895186921]: Plugin imu_pub initialized
[ INFO] [1499950034.895343628]: Plugin local_position loaded
[ INFO] [1499950034.903849928]: Plugin local_position initialized
[ INFO] [1499950034.904014273]: Plugin manual_control loaded
[ INFO] [1499950034.905009472]: Plugin manual_control initialized
[ INFO] [1499950034.905145606]: Plugin mocap_fake_gps loaded
[ INFO] [1499950034.908929136]: Plugin mocap_fake_gps initialized
[ INFO] [1499950034.909061618]: Plugin mocap_pose_estimate loaded
[ INFO] [1499950034.914746745]: Plugin mocap_pose_estimate initialized
[ INFO] [1499950034.914891265]: Plugin odom loaded
[ INFO] [1499950034.919565105]: Plugin odom initialized
[ INFO] [1499950034.919785607]: Plugin param loaded
[ INFO] [1499950034.923495395]: Plugin param initialized
[ INFO] [1499950034.923639971]: Plugin px4flow loaded
[ INFO] [1499950034.931291078]: Plugin px4flow initialized
[ INFO] [1499950034.931480581]: Plugin rc_io loaded
[ INFO] [1499950034.941449615]: Plugin rc_io initialized
[ INFO] [1499950034.941478402]: Plugin safety_area blacklisted
[ INFO] [1499950034.941635704]: Plugin setpoint_accel loaded
[ INFO] [1499950034.951186981]: Plugin setpoint_accel initialized
[ INFO] [1499950034.951350989]: Plugin setpoint_attitude loaded
[ INFO] [1499950034.981452928]: Plugin setpoint_attitude initialized
[ INFO] [1499950034.981693440]: Plugin setpoint_position loaded
[ INFO] [1499950034.993126566]: Plugin setpoint_position initialized
[ INFO] [1499950034.993298149]: Plugin setpoint_raw loaded
[INFO] [1499950034.997182, 0.000000]: Calling service /gazebo/spawn_sdf_model
[ INFO] [1499950035.012536205]: Plugin setpoint_raw initialized
[ INFO] [1499950035.012711308]: Plugin setpoint_velocity loaded
[ INFO] [1499950035.016469264, 0.022000000]: waitForService: Service [/gazebo/set_physics_properties] is now available.
[ INFO] [1499950035.024321015]: Plugin setpoint_velocity initialized
[ INFO] [1499950035.024600527]: Plugin sys_status loaded
[ INFO] [1499950035.040253156]: Plugin sys_status initialized
[ INFO] [1499950035.040424126]: Plugin sys_time loaded
[ INFO] [1499950035.049723610]: TM: Timesync mode: MAVLINK
[ INFO] [1499950035.051436063]: Plugin sys_time initialized
[ INFO] [1499950035.051594378]: Plugin vfr_hud loaded
[ INFO] [1499950035.054004478]: Plugin vfr_hud initialized
[ INFO] [1499950035.054034810]: Plugin vibration blacklisted
[ INFO] [1499950035.054169395]: Plugin vision_pose_estimate loaded
[ INFO] [1499950035.069281771]: Plugin vision_pose_estimate initialized
[ INFO] [1499950035.069445841]: Plugin vision_speed_estimate loaded
[ INFO] [1499950035.074402619]: Plugin vision_speed_estimate initialized
[ INFO] [1499950035.074571854]: Plugin waypoint loaded
[ INFO] [1499950035.080492496]: Plugin waypoint initialized
[ INFO] [1499950035.080561135]: Autostarting mavlink via USB on PX4
[ INFO] [1499950035.080707936]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1499950035.080733733]: Built-in MAVLink package version: 2017.6.6
[ INFO] [1499950035.080762611]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1499950035.080787317]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[INFO] [1499950035.226930, 0.128000]: Spawn status: SpawnModel: Successfully spawned entity
[ INFO] [1499950035.241173733, 0.128000000]: Physics dynamic reconfigure ready.
[vehicle_spawn_59d5e7024476_2400_2780903416454810023-3] process has finished cleanly
log file: /root/.ros/log/63dc76d4-67c9-11e7-af3a-0242ac110003/vehicle_spawn_59d5e7024476_2400_2780903416454810023-3*.log
INFO  [simulator] Got initial simuation data, running sim..
INFO  [pwm_out_sim] MODE_8PWM
INFO  [tone_alarm] startup
INFO  [mavlink] mode: Normal, data rate: 4000000 B/s on udp port 14556 remote port 14550
INFO  [mavlink] mode: Onboard, data rate: 4000000 B/s on udp port 14557 remote port 14540
INFO  [mavlink] MAVLink only on localhost (set param MAV_BROADCAST = 1 to enable network)
INFO  [logger] logger started
pxh> INFO  [logger] start log
INFO  [logger] Opened log file: rootfs/fs/microsd/log/2017-07-13/12_47_18.ulg
[ INFO] [1499950038.644411198, 2.192000000]: udp0: Remote address: 127.0.0.1:14557
[ INFO] [1499950038.646513083, 2.194000000]: FCU: [logger] file: rootfs/fs/microsd/log/2017-07-13/1
[ INFO] [1499950038.656818879, 2.204000000]: IMU: High resolution IMU detected!
INFO  [mavlink] partner IP: 127.0.0.1
WARN  [mavlink] [timesync] Hard setting offset.
[ INFO] [1499950039.474532645, 3.022000000]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1499950039.484755241, 3.032000000]: IMU: High resolution IMU detected!
INFO  [local_position_estimator] [lpe] baro init 0 m std -2147483648 cm
[ INFO] [1499950039.784744686, 3.332000000]: FCU: [lpe] baro init 0 m std -2147483648 cm
INFO  [local_position_estimator] [lpe] z resume 
[ INFO] [1499950039.836448068, 3.384000000]: FCU: [lpe] z resume 
[ INFO] [1499950040.477483930, 4.024000000]: VER: 1.1: Capabilities         0x00000000000024eb
[ INFO] [1499950040.477550236, 4.024000000]: VER: 1.1: Flight software:     00000000 (00000000178ECB52)
[ INFO] [1499950040.477586455, 4.024000000]: VER: 1.1: Middleware software: 00000000 (00000000178ECB52)
[ INFO] [1499950040.477620229, 4.024000000]: VER: 1.1: OS software:         000000ff (0000000000000000)
[ INFO] [1499950040.477652124, 4.024000000]: VER: 1.1: Board hardware:      00000001
[ INFO] [1499950040.477689466, 4.024000000]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1499950040.477721367, 4.024000000]: VER: 1.1: UID:                 0000000100000002
INFO  [local_position_estimator] [lpe] gps init lat  32.27 lon -111.27 alt   0.1 m
INFO  [local_position_estimator] [lpe] xy resume 
INFO  [commander] home: 32.2651565, -111.2736636, 0.08
INFO  [tone_alarm] home_set
[ INFO] [1499950041.025272348, 4.570000000]: FCU: [lpe] xy resume 
INFO  [tone_alarm] neutral
INFO  [tone_alarm] positive
[ WARN] [1499950052.681551872, 16.216000000]: CMD: Unexpected command 176, result 0
INFO  [commander] home: 32.2651582, -111.2736602, 0.03
INFO  [tone_alarm] positive
INFO  [commander] home: 32.2651582, -111.2736602, 0.03
INFO  [tone_alarm] arming
INFO  [commander] Takeoff detected
[ INFO] [1499950052.723118068, 16.258000000]: FCU: [cmd] ARMED by arm/disarm component command
[ INFO] [1499950052.774883839, 16.310000000]: FCU: Takeoff detected
[ INFO] [1499950054.488465700, 18.022000000]: WP: mission received

Diagnostics

header: 
  seq: 54
  stamp: 
    secs: 49
    nsecs:         0
  frame_id: ''
status: 
  - 
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: udp://:14540@localhost:14557
    values: 
      - 
        key: Received packets:
        value: 21665
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 174
      - 
        key: Tx sequence number:
        value: 192
      - 
        key: Rx total bytes:
        value: 864024
      - 
        key: Tx total bytes:
        value: 50267
      - 
        key: Rx speed:
        value: 18772.000000
      - 
        key: Tx speed:
        value: 1127.000000
  - 
    level: 0
    name: mavros: GPS
    message: 3D fix
    hardware_id: udp://:14540@localhost:14557
    values: 
      - 
        key: Satellites visible
        value: 10
      - 
        key: Fix type
        value: 3
      - 
        key: EPH (m)
        value: 0.00
      - 
        key: EPV (m)
        value: 0.00
  - 
    level: 0
    name: mavros: Heartbeat
    message: Normal
    hardware_id: udp://:14540@localhost:14557
    values: 
      - 
        key: Heartbeats since startup
        value: 46
      - 
        key: Frequency (Hz)
        value: 1.000000
      - 
        key: Vehicle type
        value: Quadrotor
      - 
        key: Autopilot type
        value: PX4 Autopilot
      - 
        key: Mode
        value: OFFBOARD
      - 
        key: System status
        value: Active
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: udp://:14540@localhost:14557
    values: 
      - 
        key: Sensor present
        value: 0x00000000
      - 
        key: Sensor enabled
        value: 0x00000000
      - 
        key: Sensor helth
        value: 0x00000000
      - 
        key: CPU Load (%)
        value: 0.0
      - 
        key: Drop rate (%)
        value: 0.0
      - 
        key: Errors comm
        value: 0
      - 
        key: Errors count #1
        value: 0
      - 
        key: Errors count #2
        value: 0
      - 
        key: Errors count #3
        value: 0
      - 
        key: Errors count #4
        value: 0
  - 
    level: 0
    name: mavros: Battery
    message: Normal
    hardware_id: udp://:14540@localhost:14557
    values: 
      - 
        key: Voltage
        value: 11.11
      - 
        key: Current
        value: -1.0
      - 
        key: Remaining
        value: 71.0
  - 
    level: 0
    name: mavros: Time Sync
    message: Normal
    hardware_id: udp://:14540@localhost:14557
    values: 
      - 
        key: Timesyncs since startup
        value: 465
      - 
        key: Frequency (Hz)
        value: 10.000000
      - 
        key: Last dt (ms)
        value: 0.079462
      - 
        key: Mean dt (ms)
        value: 0.162211
      - 
        key: Last system time (s)
        value: 47.732918000
      - 
        key: Time offset (s)
        value: 1.167113784

Check ID

OK. I got messages from 1:1.

---
Received 7026 messages, from 1 addresses
sys:comp   list of messages
  1:1     0, 1, 2, 140, 141, 147, 24, 30, 32, 33, 36, 74, 76, 83, 85, 87, 230, 105, 111, 241, 242, 245
@jgoppert
Copy link
Member

Use spawn from gazebo_ros to set the initial position. You are effectively moving it after you initialize the filter, so it is probably using gps to update to the new location.

@TSC21
Copy link
Member

TSC21 commented Jul 13, 2017

I assume that will be the position estimator recomputing the position through the filter.

@TSC21 TSC21 assigned TSC21 and unassigned TSC21 Jul 13, 2017
@schmittlema
Copy link
Author

schmittlema commented Jul 13, 2017 •
edited
Loading

Thank you for the quick response! The looked up the filter and that sounds right!
I am currently using spawn_model in my roslaunch:

<node name="$(anon vehicle_spawn)" output="screen" pkg="gazebo_ros" type="spawn_model"
    args="-sdf -file $(arg sdf) -model $(arg vehicle) -x $(arg x) -y $(arg y    ) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>

Is this what you mean? I have to reset the model position multiple times during the simulation so that is why I am using gazebo/set_model_state.

@TSC21
Copy link
Member

TSC21 commented Jul 13, 2017

If you restart the position you will have to restart the position estimator also.

@schmittlema
Copy link
Author

Ok this is good to know for future work. For the purpose of my simulation, I wil just used the /gazebo/get_model_state service to get this info. Thank you for the quick help!

@schmittlema
Copy link
Author

Going off of this, is it possible to reset the lpe programmatically? I can open another ticket if that is prefered

@jgoppert
Copy link
Member

Currently mavlink doesn't have any kind of state reset message to my knowledge, this would also be useful to me.

@TSC21
Copy link
Member

TSC21 commented Jul 17, 2017

Agree. Please open an issue on PX4 Firmware. You can also create a state reset command and create a PR to Mavlink repo so to update common msg definitions with that command.

@schmittlema
Copy link
Author

schmittlema commented Jul 17, 2017 •
edited
Loading

PX4/PX4-Autopilot#7632

The only way I was thinking this would be possible would be to use the mavlink_shell and call local_position_estimator stop followed by local_position_estimator start. But my mavlink_shell wasn't working :/

@TSC21
Copy link
Member

TSC21 commented Jul 17, 2017

This will only be possible with a Mavlink command. And from there a state reset on the position estimator.

# for free to join this conversation on GitHub. Already have an account? # to comment
Assignees
No one assigned
Labels
question SITL
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@jgoppert @TSC21 @schmittlema