Wrong movement using ArduRover and ROS navigation stack

[sammar80]

Issue details

I am using ArduRover with ROS navigation stack and although I can see a correct trajectory in rviz map once I send the goal to move base but the rover is not moving in the correct direction I checked and the conversion from ENU to NED is done by the setpoint_velocity.cpp plugin in mavros

MAVROS version and platform

Mavros: master
ROS: Melodic
Ubuntu: 18.04

Autopilot type and version

[ X ] ArduPilot
[ ] PX4

Version: ArduRover V4.0.0 (0e52bafa)

Node logs

roslaunch mavros apm.launch fcu_url:=/dev/ttyACM0:921600
... logging to /home/optimus/.ros/log/802f23ec-1d9a-11eb-87fe-d03745187cad/roslaunch-Autobot-32312.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://Autobot:39447/

SUMMARY

CLEAR PARAMETERS

• /mavros/

PARAMETERS

• /mavros/cmd/use_comp_id_system_control: False
• /mavros/conn/heartbeat_mav_type: ONBOARD_CONTROLLER
• /mavros/conn/heartbeat_rate: 1.0
• /mavros/conn/system_time_rate: 1.0
• /mavros/conn/timeout: 10.0
• /mavros/conn/timesync_rate: 10.0
• /mavros/distance_sensor/lidar0/id: 0
• /mavros/distance_sensor/lidar0/orientation: PITCH_270
• /mavros/distance_sensor/lidar0/subscriber: False
• /mavros/fake_gps/eph: 2.0
• /mavros/fake_gps/epv: 2.0
• /mavros/fake_gps/fix_type: 3
• /mavros/fake_gps/geo_origin/alt: 408.0
• /mavros/fake_gps/geo_origin/lat: 47.3667
• /mavros/fake_gps/geo_origin/lon: 8.55
• /mavros/fake_gps/gps_id: 4
• /mavros/fake_gps/gps_rate: 5.0
• /mavros/fake_gps/horiz_accuracy: 0.5
• /mavros/fake_gps/mocap_transform: False
• /mavros/fake_gps/mocap_withcovariance: False
• /mavros/fake_gps/satellites_visible: 6
• /mavros/fake_gps/speed_accuracy: 0.0
• /mavros/fake_gps/tf/child_frame_id: fix
• /mavros/fake_gps/tf/frame_id: map
• /mavros/fake_gps/tf/listen: False
• /mavros/fake_gps/tf/rate_limit: 10.0
• /mavros/fake_gps/tf/send: False
• /mavros/fake_gps/use_hil_gps: True
• /mavros/fake_gps/use_mocap: True
• /mavros/fake_gps/use_vision: False
• /mavros/fake_gps/vert_accuracy: 0.5
• /mavros/fcu_protocol: v2.0
• /mavros/fcu_url: /dev/ttyACM0:921600
• /mavros/gcs_url:
• /mavros/global_position/child_frame_id: base_link
• /mavros/global_position/frame_id: map
• /mavros/global_position/gps_uere: 1.0
• /mavros/global_position/rot_covariance: 99999.0
• /mavros/global_position/tf/child_frame_id: base_link
• /mavros/global_position/tf/frame_id: map
• /mavros/global_position/tf/global_frame_id: earth
• /mavros/global_position/tf/send: False
• /mavros/global_position/use_relative_alt: True
• /mavros/image/frame_id: px4flow
• /mavros/imu/angular_velocity_stdev: 0.0003490659 // 0...
• /mavros/imu/frame_id: base_link
• /mavros/imu/linear_acceleration_stdev: 0.0003
• /mavros/imu/magnetic_stdev: 0.0
• /mavros/imu/orientation_stdev: 1.0
• /mavros/landing_target/camera/fov_x: 2.0071286398
• /mavros/landing_target/camera/fov_y: 2.0071286398
• /mavros/landing_target/image/height: 480
• /mavros/landing_target/image/width: 640
• /mavros/landing_target/land_target_type: VISION_FIDUCIAL
• /mavros/landing_target/listen_lt: False
• /mavros/landing_target/mav_frame: LOCAL_NED
• /mavros/landing_target/target_size/x: 0.3
• /mavros/landing_target/target_size/y: 0.3
• /mavros/landing_target/tf/child_frame_id: camera_center
• /mavros/landing_target/tf/frame_id: landing_target
• /mavros/landing_target/tf/listen: False
• /mavros/landing_target/tf/rate_limit: 10.0
• /mavros/landing_target/tf/send: True
• /mavros/local_position/frame_id: base_link
• /mavros/local_position/tf/child_frame_id: base_link
• /mavros/local_position/tf/frame_id: map
• /mavros/local_position/tf/send: False
• /mavros/local_position/tf/send_fcu: False
• /mavros/mission/pull_after_gcs: True
• /mavros/mission/use_mission_item_int: True
• /mavros/mocap/use_pose: True
• /mavros/mocap/use_tf: False
• /mavros/odometry/estimator_type: 3
• /mavros/odometry/frame_tf/desired_frame: ned
• /mavros/plugin_blacklist: ['actuator_contro...
• /mavros/plugin_whitelist: []
• /mavros/px4flow/frame_id: px4flow
• /mavros/px4flow/ranger_fov: 0.118682
• /mavros/px4flow/ranger_max_range: 5.0
• /mavros/px4flow/ranger_min_range: 0.3
• /mavros/safety_area/p1/x: 1.0
• /mavros/safety_area/p1/y: 1.0
• /mavros/safety_area/p1/z: 1.0
• /mavros/safety_area/p2/x: -1.0
• /mavros/safety_area/p2/y: -1.0
• /mavros/safety_area/p2/z: -1.0
• /mavros/setpoint_accel/send_force: False
• /mavros/setpoint_attitude/reverse_thrust: False
• /mavros/setpoint_attitude/tf/child_frame_id: target_attitude
• /mavros/setpoint_attitude/tf/frame_id: map
• /mavros/setpoint_attitude/tf/listen: False
• /mavros/setpoint_attitude/tf/rate_limit: 50.0
• /mavros/setpoint_attitude/use_quaternion: False
• /mavros/setpoint_position/mav_frame: LOCAL_NED
• /mavros/setpoint_position/tf/child_frame_id: target_position
• /mavros/setpoint_position/tf/frame_id: map
• /mavros/setpoint_position/tf/listen: False
• /mavros/setpoint_position/tf/rate_limit: 50.0
• /mavros/setpoint_raw/thrust_scaling: 1.0
• /mavros/setpoint_velocity/mav_frame: LOCAL_NED
• /mavros/startup_px4_usb_quirk: False
• /mavros/sys/disable_diag: False
• /mavros/sys/min_voltage: 10.0
• /mavros/target_component_id: 1
• /mavros/target_system_id: 1
• /mavros/tdr_radio/low_rssi: 40
• /mavros/time/time_ref_source: fcu
• /mavros/time/timesync_avg_alpha: 0.6
• /mavros/time/timesync_mode: MAVLINK
• /mavros/vibration/frame_id: base_link
• /mavros/vision_pose/tf/child_frame_id: vision_estimate
• /mavros/vision_pose/tf/frame_id: map
• /mavros/vision_pose/tf/listen: False
• /mavros/vision_pose/tf/rate_limit: 10.0
• /mavros/vision_speed/listen_twist: True
• /mavros/vision_speed/twist_cov: True
• /mavros/wheel_odometry/child_frame_id: base_link
• /mavros/wheel_odometry/count: 2
• /mavros/wheel_odometry/frame_id: map
• /mavros/wheel_odometry/send_raw: True
• /mavros/wheel_odometry/send_twist: False
• /mavros/wheel_odometry/tf/child_frame_id: base_link
• /mavros/wheel_odometry/tf/frame_id: odom
• /mavros/wheel_odometry/tf/send: False
• /mavros/wheel_odometry/use_rpm: False
• /mavros/wheel_odometry/vel_error: 0.1
• /mavros/wheel_odometry/wheel0/radius: 0.05
• /mavros/wheel_odometry/wheel0/x: 0.0
• /mavros/wheel_odometry/wheel0/y: -0.15
• /mavros/wheel_odometry/wheel1/radius: 0.05
• /mavros/wheel_odometry/wheel1/x: 0.0
• /mavros/wheel_odometry/wheel1/y: 0.15
• /rosdistro: melodic
• /rosversion: 1.14.9

NODES
/
mavros (mavros/mavros_node)

ROS_MASTER_URI=http://localhost:11311

process[mavros-1]: started with pid [32350]
[ INFO] [1604383666.477056764]: FCU URL: /dev/ttyACM0:921600
[ INFO] [1604383666.485917849]: serial0: device: /dev/ttyACM0 @ 921600 bps
[ INFO] [1604383666.488025813]: GCS bridge disabled
[ INFO] [1604383666.784134771]: Plugin 3dr_radio loaded
[ INFO] [1604383666.793645402]: Plugin 3dr_radio initialized
[ INFO] [1604383666.793879313]: Plugin actuator_control blacklisted
[ INFO] [1604383666.946785203]: Plugin adsb loaded
[ INFO] [1604383666.975207979]: Plugin adsb initialized
[ INFO] [1604383666.975319700]: Plugin altitude blacklisted
[ INFO] [1604383666.975715855]: Plugin cam_imu_sync loaded
[ INFO] [1604383666.994185073]: Plugin cam_imu_sync initialized
[ INFO] [1604383666.994817899]: Plugin command loaded
[ INFO] [1604383667.065677724]: Plugin command initialized
[ INFO] [1604383667.066222736]: Plugin companion_process_status loaded
[ INFO] [1604383667.129832866]: Plugin companion_process_status initialized
[ INFO] [1604383667.129942660]: Plugin debug_value blacklisted
[ INFO] [1604383667.130001984]: Plugin distance_sensor blacklisted
[ INFO] [1604383667.130475432]: Plugin esc_status loaded
[ INFO] [1604383667.135064702]: Plugin esc_status initialized
[ INFO] [1604383667.135717477]: Plugin fake_gps loaded
[ INFO] [1604383667.233667651]: Plugin fake_gps initialized
[ INFO] [1604383667.234173027]: Plugin ftp blacklisted
[ INFO] [1604383667.234707779]: Plugin global_position loaded
[ INFO] [1604383667.292947527]: Plugin global_position initialized
[ INFO] [1604383667.293499936]: Plugin gps_rtk loaded
[ INFO] [1604383667.305195511]: Plugin gps_rtk initialized
[ INFO] [1604383667.305709273]: Plugin gps_status loaded
[ INFO] [1604383667.315751634]: Plugin gps_status initialized
[ INFO] [1604383667.315880335]: Plugin hil blacklisted
[ INFO] [1604383667.316593737]: Plugin home_position loaded
[ INFO] [1604383667.329179593]: Plugin home_position initialized
[ INFO] [1604383667.330241961]: Plugin imu loaded
[ INFO] [1604383667.356051969]: Plugin imu initialized
[ INFO] [1604383667.356626825]: Plugin landing_target loaded
[ INFO] [1604383667.403353450]: Plugin landing_target initialized
[ INFO] [1604383667.404113728]: Plugin local_position loaded
[ INFO] [1604383667.427421336]: Plugin local_position initialized
[ INFO] [1604383667.427949108]: Plugin log_transfer loaded
[ INFO] [1604383667.438477678]: Plugin log_transfer initialized
[ INFO] [1604383667.439040243]: Plugin manual_control loaded
[ INFO] [1604383667.451008429]: Plugin manual_control initialized
[ INFO] [1604383667.451501722]: Plugin mocap_pose_estimate loaded
[ INFO] [1604383667.464687539]: Plugin mocap_pose_estimate initialized
[ INFO] [1604383667.465249791]: Plugin mount_control loaded
[ INFO] [1604383667.478513059]: Plugin mount_control initialized
[ INFO] [1604383667.478680719]: Plugin obstacle_distance blacklisted
[ INFO] [1604383667.479650845]: Plugin odom loaded
[ INFO] [1604383667.503766179]: Plugin odom initialized
[ INFO] [1604383667.504244263]: Plugin onboard_computer_status loaded
[ INFO] [1604383667.513161183]: Plugin onboard_computer_status initialized
[ INFO] [1604383667.513883178]: Plugin param loaded
[ INFO] [1604383667.526616017]: Plugin param initialized
[ INFO] [1604383667.527119830]: Plugin play_tune loaded
[ INFO] [1604383667.536060292]: Plugin play_tune initialized
[ INFO] [1604383667.536348371]: Plugin px4flow blacklisted
[ INFO] [1604383667.536834528]: Plugin rangefinder loaded
[ INFO] [1604383667.539398231]: Plugin rangefinder initialized
[ INFO] [1604383667.540737845]: Plugin rc_io loaded
[ INFO] [1604383667.554026113]: Plugin rc_io initialized
[ INFO] [1604383667.554201429]: Plugin safety_area blacklisted
[ INFO] [1604383667.554722066]: Plugin setpoint_accel loaded
[ INFO] [1604383667.564895420]: Plugin setpoint_accel initialized
[ INFO] [1604383667.565693980]: Plugin setpoint_attitude loaded
[ INFO] [1604383667.626996766]: Plugin setpoint_attitude initialized
[ INFO] [1604383667.627584383]: Plugin setpoint_position loaded
[ INFO] [1604383667.704133659]: Plugin setpoint_position initialized
[ INFO] [1604383667.704713776]: Plugin setpoint_raw loaded
[ INFO] [1604383667.741847581]: Plugin setpoint_raw initialized
[ INFO] [1604383667.742404833]: Plugin setpoint_trajectory loaded
[ INFO] [1604383667.761104785]: Plugin setpoint_trajectory initialized
[ INFO] [1604383667.761715840]: Plugin setpoint_velocity loaded
[ INFO] [1604383667.781171591]: Plugin setpoint_velocity initialized
[ INFO] [1604383667.782252344]: Plugin sys_status loaded
[ INFO] [1604383667.821293432]: Plugin sys_status initialized
[ INFO] [1604383667.821853652]: Plugin sys_time loaded
[ INFO] [1604383667.841213620]: TM: Timesync mode: MAVLINK
[ INFO] [1604383667.846238576]: Plugin sys_time initialized
[ INFO] [1604383667.846730202]: Plugin trajectory loaded
[ INFO] [1604383667.864761285]: Plugin trajectory initialized
[ INFO] [1604383667.865370049]: Plugin vfr_hud loaded
[ INFO] [1604383667.868700124]: Plugin vfr_hud initialized
[ INFO] [1604383667.868835856]: Plugin vibration blacklisted
[ INFO] [1604383667.869261386]: Plugin vision_pose_estimate loaded
[ INFO] [1604383667.899906139]: Plugin vision_pose_estimate initialized
[ INFO] [1604383667.900092914]: Plugin vision_speed_estimate blacklisted
[ INFO] [1604383667.900796315]: Plugin waypoint loaded
[ INFO] [1604383667.922937543]: Plugin waypoint initialized
[ INFO] [1604383667.923114995]: Plugin wheel_odometry blacklisted
[ INFO] [1604383667.924174498]: Plugin wind_estimation loaded
[ INFO] [1604383667.935716007]: Plugin wind_estimation initialized
[ INFO] [1604383667.937436515]: Built-in SIMD instructions: ARM NEON
[ INFO] [1604383667.937694281]: Built-in MAVLink package version: 2020.9.10
[ INFO] [1604383667.937931318]: Known MAVLink dialects: common ardupilotmega ASLUAV all autoquad icarous matrixpilot paparazzi standard uAvionix ualberta
[ INFO] [1604383667.938121843]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1604383667.954146995]: RC_CHANNELS message detected!
[ INFO] [1604383667.955971776]: IMU: Raw IMU message used.
[ WARN] [1604383667.957418736]: IMU: linear acceleration on RAW_IMU known on APM only.
[ WARN] [1604383667.958409695]: IMU: ~imu/data_raw stores unscaled raw acceleration report.
[ WARN] [1604383667.961033140]: GP: No GPS fix
[ WARN] [1604383667.963039227]: TM: Wrong FCU time.
[ INFO] [1604383668.362075345]: FCU: EKF2 IMU0 is using external nav data
[ INFO] [1604383668.363317404]: FCU: EKF2 IMU0 initial pos NED = -0.0,0.0,-0.0 (m)
[ INFO] [1604383668.365834075]: FCU: EKF2 IMU0 ext nav yaw alignment complete
[ INFO] [1604383668.367967196]: FCU: EKF2 IMU1 is using external nav data
[ INFO] [1604383668.369335977]: FCU: EKF2 IMU1 initial pos NED = -0.0,0.0,-0.0 (m)
[ INFO] [1604383668.370588193]: FCU: EKF2 IMU1 ext nav yaw alignment complete
[ INFO] [1604383668.749626257]: CON: Got HEARTBEAT, connected. FCU: ArduPilot
[ INFO] [1604383668.753458895]: RC_CHANNELS message detected!
[ INFO] [1604383669.708112834]: FCU: Set HOME to 0.000003 0.000003 at -0.01m
[ WARN] [1604383669.762217020]: CMD: Unexpected command 520, result 0
[ INFO] [1604383669.764672492]: WP: Using MISSION_ITEM_INT
[ INFO] [1604383669.765349226]: VER: 1.1: Capabilities 0x000000000000f1ef
[ INFO] [1604383669.765920906]: VER: 1.1: Flight software: 040000ff ( 0e52baf)
[ INFO] [1604383669.766339769]: VER: 1.1: Middleware software: 00000000 ( )
[ INFO] [1604383669.766696913]: VER: 1.1: OS software: 00000000 ( 0997003)
[ INFO] [1604383669.767053171]: VER: 1.1: Board hardware: 00000000
[ INFO] [1604383669.767426460]: VER: 1.1: VID/PID: 0000:0000
[ INFO] [1604383669.768633311]: VER: 1.1: UID: 0000000000000000
[ WARN] [1604383673.548312875]: TM : RTT too high for timesync: 401.91 ms.
[ INFO] [1604383678.757269718]: FCU: ArduRover V4.0.0 (0e52bafa)
[ INFO] [1604383678.757956977]: FCU: ChibiOS: 0997003f
[ INFO] [1604383678.758387510]: FCU: Pixracer 00330026 374E5005 2031384B
[ INFO] [1604383683.757857354]: WP: mission received
[ WARN] [1604383690.168471355]: TM : RTT too high for timesync: 522.12 ms.
[ WARN] [1604383690.949640565]: TM : RTT too high for timesync: 203.25 ms.
[ INFO] [1604383690.950290948]: PR: parameters list received
[ WARN] [1604383693.723605856]: TM : RTT too high for timesync: 177.21 ms.
[ WARN] [1604383698.050486202]: GP: No GPS fix
[ WARN] [1604383701.133954197]: TM : RTT too high for timesync: 187.58 ms.
[ WARN] [1604383709.433121584]: TM : RTT too high for timesync: 286.75 ms.
[ WARN] [1604383714.909394585]: TM : RTT too high for timesync: 162.95 ms.
[ WARN] [1604383728.051014800]: GP: No GPS fix
[ WARN] [1604383736.234291346]: TM : RTT too high for timesync: 187.93 ms.
[ WARN] [1604383744.115348399]: TM : RTT too high for timesync: 368.32 ms.
[ WARN] [1604383748.513256131]: TM : RTT too high for timesync: 166.88 ms.
[ WARN] [1604383758.157722746]: GP: No GPS fix
[ WARN] [1604383765.759491412]: TM : RTT too high for timesync: 13.09 ms.

Diagnostics

rostopic echo -n1 /diagnostics

header:
seq: 20
stamp:
secs: 1604391654
nsecs: 20940989
frame_id: ''
status:

level: 0
name: "mavros: FCU connection"
message: "connected"
hardware_id: "/dev/ttyACM0:921600"
values: 
  - 
    key: "Received packets:"
    value: "13177"
  - 
    key: "Dropped packets:"
    value: "0"
  - 
    key: "Buffer overruns:"
    value: "0"
  - 
    key: "Parse errors:"
    value: "0"
  - 
    key: "Rx sequence number:"
    value: "78"
  - 
    key: "Tx sequence number:"
    value: "0"
  - 
    key: "Rx total bytes:"
    value: "387516"
  - 
    key: "Tx total bytes:"
    value: "31462"
  - 
    key: "Rx speed:"
    value: "10047.000000"
  - 
    key: "Tx speed:"
    value: "2386.000000"

• level: 2
  name: "mavros: GPS"
  message: "No satellites"
  hardware_id: "/dev/ttyACM0:921600"
  values:

  key: "Satellites visible"
  value: "0"
  

  • key: "Fix type"
    value: "0"
  • key: "EPH (m)"
    value: "Unknown"
  • key: "EPV (m)"
    value: "Unknown"

• level: 0
  name: "mavros: Heartbeat"
  message: "Normal"
  hardware_id: "/dev/ttyACM0:921600"
  values:

  key: "Heartbeats since startup"
  value: "20"
  

  • key: "Frequency (Hz)"
    value: "0.965685"
  • key: "Vehicle type"
    value: "Ground rover"
  • key: "Autopilot type"
    value: "ArduPilot"
  • key: "Mode"
    value: "MANUAL"
  • key: "System status"
    value: "Critical"

• level: 2
  name: "mavros: System"
  message: "Sensor health"
  hardware_id: "/dev/ttyACM0:921600"
  values:

  key: "Sensor present"
  value: "0x0320DC0B"
  

  • key: "Sensor enabled"
    value: "0x0020800B"
  • key: "Sensor health"
    value: "0x0110810B"
  • key: "3D gyro"
    value: "Ok"
  • key: "3D accelerometer"
    value: "Ok"
  • key: "absolute pressure"
    value: "Ok"
  • key: "motor outputs / control"
    value: "Ok"
  • key: "AHRS subsystem health"
    value: "Fail"
  • key: "CPU Load (%)"
    value: "19.0"
  • key: "Drop rate (%)"
    value: "0.0"
  • key: "Errors comm"
    value: "0"
  • key: "Errors count mavconn library. #1"
    value: "0"
  • key: "Errors count Pub/sub for Mavlink.msg #2"
    value: "0"
  • key: "Errors count Plugin loading functionality. #3"
    value: "0"
  • key: "Errors count Diagnostic mesages. #4"
    value: "0"

• level: 1
  name: "mavros: Battery"
  message: "Low voltage"
  hardware_id: "/dev/ttyACM0:921600"
  values:

  key: "Voltage"
  value: "0.00"
  

  • key: "Current"
    value: "-0.0"
  • key: "Remaining"
    value: "-1.0"

• level: 1
  name: "mavros: Time Sync"
  message: "Frequency too high."
  hardware_id: "/dev/ttyACM0:921600"
  values:

  key: "Timesyncs since startup"
  value: "359"
  

  • key: "Frequency (Hz)"
    value: "18.141095"
  • key: "Last RTT (ms)"
    value: "607.003685"
  • key: "Mean RTT (ms)"
    value: "4469004945.422476"
  • key: "Last remote time (s)"
    value: "1604391653.295881033"
  • key: "Estimated time offset (s)"
    value: "1604372388.825948715"

rosrun mavros checkid
OK. I got messages from 1:1.

---

Received 3485 messages, from 2 addresses
sys:comp list of messages
255:190 0, 66, 111
1:1 0, 1, 2, 193, 152, 150, 24, 27, 29, 30, 32, 33, 34, 163, 36, 165, 42, 178, 182, 62, 65, 74, 111, 241, 116, 125

[Alex-Beh]

Hello, may I know the board that you using?

[sammar80]

@Alex-Beh for the companion computer we are using NVIDIA Jetson nano integeated with pixracer for ardupilot

[chiragmakwana0296]

Thats Probably due to setpoint_velocity mav_frame is set to LOCAL_NED
Set it to BODY_NED in apm_config.yaml or change it using:

rosservice call /mavros/setpoint_velocity/mav_frame "mav_frame: 8"

[sammar80]

@chiragmakwana0296 thanks a lot for your reply and sorry for the delay . I changed the mav_frame to BODY_NED and the problem was solved