mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
875 stars 987 forks source link

Topic global_position/gp_vel not found #1255

Open mohand150 opened 5 years ago

mohand150 commented 5 years ago

I work on a boat drone, I am looking to get the speed of the meter in meters/s, preferably the speed coming out of the kelman filter I am looking to get the Ground X Speed (m/s), Ground Y Speed (m/s) and Ground Z Speed (m/s) so I search the information fused by FCU and raw GPS data.


Issue details

this data is on message 33 and 63 that I receive :

  <message id="33" name="GLOBAL_POSITION_INT">
      <description>The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up). It
               is designed as scaled integer message since the resolution of float is not sufficient.</description>
      <field type="uint32_t" name="time_boot_ms" units="ms">Timestamp (time since system boot).</field>
      <field type="int32_t" name="lat" units="degE7">Latitude, expressed</field>
      <field type="int32_t" name="lon" units="degE7">Longitude, expressed</field>
      <field type="int32_t" name="alt" units="mm">Altitude (MSL). Note that virtually all GPS modules provide both WGS84 and MSL.</field>
      <field type="int32_t" name="relative_alt" units="mm">Altitude above ground</field>
      <field type="int16_t" name="vx" units="cm/s">Ground X Speed (Latitude, positive north)</field>
      <field type="int16_t" name="vy" units="cm/s">Ground Y Speed (Longitude, positive east)</field>
      <field type="int16_t" name="vz" units="cm/s">Ground Z Speed (Altitude, positive down)</field>
      <field type="uint16_t" name="hdg" units="cdeg">Vehicle heading (yaw angle), 0.0..359.99 degrees. If unknown, set to: UINT16_MAX</field>
    </message>
    <message id="63" name="GLOBAL_POSITION_INT_COV">
      <description>The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up). It  is designed as scaled integer message since the resolution of float is not sufficient. NOTE: This message is intended for onboard networks / companion computers and higher-bandwidth links and optimized for accuracy and completeness. Please use the GLOBAL_POSITION_INT message for a minimal subset.</description>
      <field type="uint64_t" name="time_usec" units="us">Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number.</field>
      <field type="uint8_t" name="estimator_type" enum="MAV_ESTIMATOR_TYPE">Class id of the estimator this estimate originated from.</field>
      <field type="int32_t" name="lat" units="degE7">Latitude</field>
      <field type="int32_t" name="lon" units="degE7">Longitude</field>
      <field type="int32_t" name="alt" units="mm">Altitude in meters above MSL</field>
      <field type="int32_t" name="relative_alt" units="mm">Altitude above ground</field>
      <field type="float" name="vx" units="m/s">Ground X Speed (Latitude)</field>
      <field type="float" name="vy" units="m/s">Ground Y Speed (Longitude)</field>
      <field type="float" name="vz" units="m/s">Ground Z Speed (Altitude)</field>
      <field type="float[36]" name="covariance">Row-major representation of a 6x6 position and velocity 6x6 cross-covariance matrix (states: lat, lon, alt, vx, vy, vz; first six entries are the first ROW, next six entries are the second row, etc.). If unknown, assign NaN value to first element in the array.</field>
    </message>

in the wiki mavros I saw that it exites already a topic ~ global_position/gp_vel (geometry_msgs / TwistStamped) when I run the roslaunch of my mavros node, I don't find this topic

MAVROS version and platform

Mavros: 0.30.0 ROS: Kinetic Ubuntu: 16.04

Autopilot type and version

[ x ] ArduPilot [ ] PX4

Version: 3.4.2

Node logs

nvidia@tegra-ubuntu:~/catkin_ws$ roslaunch mavros apm.launch
WARNING: Package name "stereo_vision_SLAM" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
... logging to /home/nvidia/.ros/log/5f72d66e-9351-11e9-9b47-00044bc4a5de/roslaunch-tegra-ubuntu-16350.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

WARNING: Package name "stereo_vision_SLAM" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
started roslaunch server http://tegra-ubuntu:43688/

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: 0.0
 * /mavros/distance_sensor/rangefinder_pub/field_of_view: 0.0
 * /mavros/distance_sensor/rangefinder_pub/frame_id: lidar
 * /mavros/distance_sensor/rangefinder_pub/id: 0
 * /mavros/distance_sensor/rangefinder_pub/send_tf: False
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/x: 0.0
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/y: 0.0
 * /mavros/distance_sensor/rangefinder_pub/sensor_position/z: -0.1
 * /mavros/distance_sensor/rangefinder_sub/id: 1
 * /mavros/distance_sensor/rangefinder_sub/orientation: PITCH_270
 * /mavros/distance_sensor/rangefinder_sub/subscriber: True
 * /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_rate: 5.0
 * /mavros/fake_gps/mocap_transform: True
 * /mavros/fake_gps/satellites_visible: 5
 * /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_mocap: True
 * /mavros/fake_gps/use_vision: False
 * /mavros/fcu_protocol: v2.0
 * /mavros/fcu_url: tcp://192.168.1.1...
 * /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.000349065850399
 * /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/local_position/frame_id: map
 * /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/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.118682389136
 * /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: map
 * /mavros/wheel_odometry/tf/send: True
 * /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: kinetic
 * /rosversion: 1.12.14

NODES
  /
    mavros (mavros/mavros_node)

auto-starting new master
process[master]: started with pid [16364]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 5f72d66e-9351-11e9-9b47-00044bc4a5de
WARNING: Package name "stereo_vision_SLAM" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
process[rosout-1]: started with pid [16379]
started core service [/rosout]
process[mavros-2]: started with pid [16397]
[ INFO] [1561031334.405473885]: FCU URL: tcp://192.168.1.130:4003
[ INFO] [1561031334.419219296]: tcp0: Server address: 192.168.1.130:4003
[ INFO] [1561031334.420883061]: GCS bridge disabled
[ INFO] [1561031334.513731266]: Plugin 3dr_radio loaded
[ INFO] [1561031334.523883261]: Plugin 3dr_radio initialized
[ INFO] [1561031334.524051164]: Plugin actuator_control blacklisted
[ INFO] [1561031334.544295891]: Plugin adsb loaded
[ INFO] [1561031334.571814330]: Plugin adsb initialized
[ INFO] [1561031334.571988664]: Plugin altitude blacklisted
[ INFO] [1561031334.572830579]: Plugin cam_imu_sync loaded
[ INFO] [1561031334.580423071]: Plugin cam_imu_sync initialized
[ INFO] [1561031334.581601943]: Plugin command loaded
[ INFO] [1561031334.662144823]: Plugin command initialized
[ INFO] [1561031334.663784812]: Plugin companion_process_status loaded
[ INFO] [1561031334.695870324]: Plugin companion_process_status initialized
[ INFO] [1561031334.696061042]: Plugin debug_value blacklisted
[ INFO] [1561031334.697232874]: Plugin distance_sensor loaded
[ INFO] [1561031334.835599332]: Plugin distance_sensor initialized
[ INFO] [1561031334.838401713]: Plugin fake_gps loaded
[ INFO] [1561031335.123249133]: Plugin fake_gps initialized
[ INFO] [1561031335.123535531]: Plugin ftp blacklisted
[ INFO] [1561031335.125442782]: Plugin global_position loaded
[ INFO] [1561031335.390621343]: Plugin global_position initialized
[ INFO] [1561031335.392832432]: Plugin gps_rtk loaded
[ INFO] [1561031335.456279428]: Plugin gps_rtk initialized
[ INFO] [1561031335.458778355]: Plugin hil loaded
[ INFO] [1561031335.747538325]: Plugin hil initialized
[ INFO] [1561031335.749148650]: Plugin home_position loaded
[ INFO] [1561031335.816032902]: Plugin home_position initialized
[ INFO] [1561031335.817839770]: Plugin imu loaded
[ INFO] [1561031335.983918840]: Plugin imu initialized
[ INFO] [1561031335.986539558]: Plugin local_position loaded
[ INFO] [1561031336.133792932]: Plugin local_position initialized
[ INFO] [1561031336.136572657]: Plugin log_transfer loaded
[ INFO] [1561031336.180080235]: Plugin log_transfer initialized
[ INFO] [1561031336.180775591]: Plugin manual_control loaded
[ INFO] [1561031336.213247595]: Plugin manual_control initialized
[ INFO] [1561031336.213708328]: Plugin mocap_pose_estimate loaded
[ INFO] [1561031336.262862492]: Plugin mocap_pose_estimate initialized
[ INFO] [1561031336.263671543]: Plugin obstacle_distance loaded
[ INFO] [1561031336.287752052]: Plugin obstacle_distance initialized
[ INFO] [1561031336.288737517]: Plugin odom loaded
[ INFO] [1561031336.347711103]: Plugin odom initialized
[ INFO] [1561031336.349995696]: Plugin param loaded
[ INFO] [1561031336.395873658]: Plugin param initialized
[ INFO] [1561031336.396082744]: Plugin px4flow blacklisted
[ INFO] [1561031336.396930419]: Plugin rangefinder loaded
[ INFO] [1561031336.406943439]: Plugin rangefinder initialized
[ INFO] [1561031336.408547492]: Plugin rc_io loaded
[ INFO] [1561031336.474146441]: Plugin rc_io initialized
[ INFO] [1561031336.474464999]: Plugin safety_area blacklisted
[ INFO] [1561031336.475948925]: Plugin setpoint_accel loaded
[ INFO] [1561031336.535231309]: Plugin setpoint_accel initialized
[ INFO] [1561031336.538764501]: Plugin setpoint_attitude loaded
[ INFO] [1561031336.712745726]: Plugin setpoint_attitude initialized
[ INFO] [1561031336.714405874]: Plugin setpoint_position loaded
[ INFO] [1561031336.972657954]: Plugin setpoint_position initialized
[ INFO] [1561031336.974883571]: Plugin setpoint_raw loaded
[ INFO] [1561031337.159694003]: Plugin setpoint_raw initialized
[ INFO] [1561031337.161534406]: Plugin setpoint_velocity loaded
[ INFO] [1561031337.296304536]: Plugin setpoint_velocity initialized
[ INFO] [1561031337.301544309]: Plugin sys_status loaded
[ INFO] [1561031337.512410597]: Plugin sys_status initialized
[ INFO] [1561031337.514853172]: Plugin sys_time loaded
[ INFO] [1561031337.633559218]: TM: Timesync mode: MAVLINK
[ INFO] [1561031337.658963463]: Plugin sys_time initialized
[ INFO] [1561031337.660576124]: Plugin trajectory loaded
[ INFO] [1561031337.738267951]: Plugin trajectory initialized
[ INFO] [1561031337.738792652]: Plugin vfr_hud loaded
[ INFO] [1561031337.746596215]: Plugin vfr_hud initialized
[ INFO] [1561031337.746794550]: Plugin vibration blacklisted
[ INFO] [1561031337.747639632]: Plugin vision_pose_estimate loaded
[ INFO] [1561031337.903525715]: Plugin vision_pose_estimate initialized
[ INFO] [1561031337.904035376]: Plugin vision_speed_estimate blacklisted
[ INFO] [1561031337.906208513]: Plugin waypoint loaded
[ INFO] [1561031337.989330799]: Plugin waypoint initialized
[ INFO] [1561031337.989659373]: Plugin wheel_odometry blacklisted
[ INFO] [1561031337.991411617]: Plugin wind_estimation loaded
[ INFO] [1561031338.006660794]: Plugin wind_estimation initialized
[ INFO] [1561031338.007771091]: Built-in SIMD instructions: ARM NEON
[ INFO] [1561031338.008092817]: Built-in MAVLink package version: 2019.4.4
[ INFO] [1561031338.008448718]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad icarous matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1561031338.008742988]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ INFO] [1561031338.128093222]: CON: Got HEARTBEAT, connected. FCU: ArduPilot
[ INFO] [1561031338.167215710]: IMU: Raw IMU message used.
[ WARN] [1561031338.186075679]: GP: No GPS fix
[ INFO] [1561031338.198216237]: RC_CHANNELS message detected!
[ WARN] [1561031338.218472612]: TM: Wrong FCU time.
[ INFO] [1561031339.211225491]: VER: 1.1: Capabilities         0x00000000000011cf
[ INFO] [1561031339.211539761]: VER: 1.1: Flight software:     030402ff ( 318a941)
[ INFO] [1561031339.211766479]: VER: 1.1: Middleware software: 00000000 ( b535f97)
[ INFO] [1561031339.211990254]: VER: 1.1: OS software:         00000000 ( 1472b16)
[ INFO] [1561031339.212249612]: VER: 1.1: Board hardware:      00000000
[ INFO] [1561031339.212463306]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1561031339.212632617]: VER: 1.1: UID:                 0000000000000000
[ WARN] [1561031339.214095871]: CMD: Unexpected command 520, result 0
[ INFO] [1561031348.131953944]: HP: requesting home position
[ INFO] [1561031348.171211503]: FCU: ArduRover V3.4.2 (318a941d)
[ INFO] [1561031348.174423417]: FCU: PX4: b535f974 NuttX: 1472b16c
[ INFO] [1561031348.179524694]: FCU: PX4v2 001E002F 3235510B 37393635
[ INFO] [1561031353.185323493]: WP: item #0  F:0 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 0.000000 y: 0.000000 z: 0.000000
[ INFO] [1561031353.205330366]: WP: item #1  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.585346 y: -3.026326 z: 100.000000
[ INFO] [1561031353.226900876]: WP: item #2  F:0 C:206 p: 0.000000 0.000000 0.000000 0.000000 x: 0.000000 y: 0.000000 z: 0.000000
[ INFO] [1561031353.246223466]: WP: item #3  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584755 y: -3.026141 z: 100.000000
[ INFO] [1561031353.265592999]: WP: item #4  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584713 y: -3.026127 z: 100.000000
[ INFO] [1561031353.285625824]: WP: item #5  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584763 y: -3.026101 z: 100.000000
[ INFO] [1561031353.306753041]: WP: item #6  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.585346 y: -3.026286 z: 100.000000
[ INFO] [1561031353.326470252]: WP: item #7  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.585350 y: -3.026246 z: 100.000000
[ INFO] [1561031353.345223309]: WP: item #8  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584766 y: -3.026062 z: 100.000000
[ INFO] [1561031353.366498333]: WP: item #9  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584721 y: -3.026048 z: 100.000000
[ INFO] [1561031353.405378487]: WP: item #10  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584770 y: -3.026022 z: 100.000000
[ INFO] [1561031353.428124477]: WP: item #11  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.585354 y: -3.026206 z: 100.000000
[ INFO] [1561031353.446326978]: WP: item #12  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.585358 y: -3.026166 z: 100.000000
[ INFO] [1561031353.466694809]: WP: item #13  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584770 y: -3.025982 z: 100.000000
[ INFO] [1561031353.485404218]: WP: item #14  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584732 y: -3.025969 z: 100.000000
[ INFO] [1561031353.506481324]: WP: item #15  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584778 y: -3.025943 z: 100.000000
[ INFO] [1561031353.526638820]: WP: item #16  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.585361 y: -3.026127 z: 100.000000
[ INFO] [1561031353.544636362]: WP: item #17  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.585278 y: -3.026345 z: 100.000000
[ INFO] [1561031353.564167014]: WP: item #18  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584751 y: -3.026180 z: 100.000000
[ INFO] [1561031353.584116608]: WP: item #19  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584709 y: -3.026166 z: 100.000000
[ INFO] [1561031353.607034085]: WP: item #20  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584751 y: -3.026221 z: 100.000000
[ INFO] [1561031353.646922487]: WP: item #21  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.585125 y: -3.026338 z: 100.000000
[ INFO] [1561031353.664846238]: WP: item #22  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584980 y: -3.026333 z: 100.000000
[ INFO] [1561031353.684188124]: WP: item #23  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584755 y: -3.026263 z: 100.000000
[ INFO] [1561031353.707618013]: WP: item #24  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584713 y: -3.026249 z: 100.000000
[ INFO] [1561031353.726395871]: WP: item #25  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584763 y: -3.026305 z: 100.000000
[ INFO] [1561031353.747571696]: WP: item #26  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584896 y: -3.026348 z: 100.000000
[ INFO] [1561031353.765792725]: WP: item #27  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584896 y: -3.026389 z: 100.000000
[ INFO] [1561031353.784234936]: WP: item #28  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584766 y: -3.026347 z: 100.000000
[ INFO] [1561031353.804775213]: WP: item #29  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584721 y: -3.026333 z: 100.000000
[ INFO] [1561031353.827619955]: WP: item #30  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584770 y: -3.026389 z: 100.000000
[ INFO] [1561031353.844983422]: WP: item #31  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584900 y: -3.026430 z: 100.000000
[ INFO] [1561031353.863679231]: WP: item #32  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584892 y: -3.026469 z: 100.000000
[ INFO] [1561031353.884115157]: WP: item #33  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584770 y: -3.026431 z: 100.000000
[ INFO] [1561031353.906229728]: WP: item #34  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584732 y: -3.026417 z: 100.000000
[ INFO] [1561031353.926418936]: WP: item #35  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584778 y: -3.026473 z: 100.000000
[ INFO] [1561031353.944531773]: WP: item #36  F:3 C: 16 p: 0.000000 0.000000 0.000000 0.000000 x: 47.584831 y: -3.026490 z: 100.000000
[ INFO] [1561031353.964175577]: WP: item #37  F:0 C:206 p: 0.000000 0.000000 0.000000 0.000000 x: 0.000000 y: 0.000000 z: 0.000000
[ INFO] [1561031353.965057107]: WP: mission received
[ INFO] [1561031358.130825899]: HP: requesting home position
[ INFO] [1561031363.947951794]: PR: parameters list received
[ INFO] [1561031368.130155483]: HP: requesting home position
[ WARN] [1561031368.678166470]: GP: No GPS fix

Diagnostics


nvidia@tegra-ubuntu:~/catkin_ws$ rostopic echo -n1 /diagnostics | tee /tmp/diag.wml
header:
  seq: 291
  stamp:
    secs: 1561030092
    nsecs: 178168742
  frame_id: ''
status:
  -
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "tcp://192.168.1.130:4003"
    values:
      -
        key: "Received packets:"
        value: "34356"
      -
        key: "Dropped packets:"
        value: "0"
      -
        key: "Buffer overruns:"
        value: "0"
      -
        key: "Parse errors:"
        value: "0"
      -
        key: "Rx sequence number:"
        value: "95"
      -
        key: "Tx sequence number:"
        value: "115"
      -
        key: "Rx total bytes:"
        value: "1121640"
      -
        key: "Tx total bytes:"
        value: "18653"
      -
        key: "Rx speed:"
        value: "2902.000000"
      -
        key: "Tx speed:"
        value: "40.000000"
  -
    level: 2
    name: "mavros: GPS"
    message: "No satellites"
    hardware_id: "tcp://192.168.1.130:4003"
    values:
      -
        key: "Satellites visible"
        value: "0"
      -
        key: "Fix type"
        value: "1"
      -
        key: "EPH (m)"
        value: "99.99"
      -
        key: "EPV (m)"
        value: "99.99"
  -
    level: 0
    name: "mavros: Heartbeat"
    message: "Normal"
    hardware_id: "tcp://192.168.1.130:4003"
    values:
      -
        key: "Heartbeats since startup"
        value: "382"
      -
        key: "Frequency (Hz)"
        value: "0.965579"
      -
        key: "Vehicle type"
        value: "Surface vessel"
      -
        key: "Autopilot type"
        value: "ArduPilot"
      -
        key: "Mode"
        value: "HOLD"
      -
        key: "System status"
        value: "Critical"
  -
    level: 0
    name: "mavros: System"
    message: "Normal"
    hardware_id: "tcp://192.168.1.130:4003"
    values:
      -
        key: "Sensor present"
        value: "0x0320DC27"
      -
        key: "Sensor enabled"
        value: "0x03208027"
      -
        key: "Sensor helth"
        value: "0x0320DC27"
      -
        key: "3D gyro"
        value: "Ok"
      -
        key: "3D accelerometer"
        value: "Ok"
      -
        key: "3D magnetometer"
        value: "Ok"
      -
        key: "GPS"
        value: "Ok"
      -
        key: "motor outputs / control"
        value: "Ok"
      -
        key: "AHRS subsystem health"
        value: "Ok"
      -
        key: "Logging"
        value: "Ok"
      -
        key: "Battery"
        value: "Ok"
      -
        key: "CPU Load (%)"
        value: "12.5"
      -
        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: "tcp://192.168.1.130:4003"
    values:
      -
        key: "Voltage"
        value: "14.02"
      -
        key: "Current"
        value: "1.5"
      -
        key: "Remaining"
        value: "98.0"

Check ID

nvidia@tegra-ubuntu:~/catkin_ws$ rosrun mavros checkid
OK. I got messages from 1:1.

---
Received 1344 messages, from 1 addresses
sys:comp   list of messages
  1:1     0, 1, 2, 150, 65, 152, 147, 22, 24, 27, 29, 30, 33, 34, 35, 36, 165, 42, 178, 182, 193, 194, 74, 77, 163, 111, 241, 116, 125

the topic ~ global_position/gp_vel Is it published by the FCU? or the problem comes from GPS which displays warnings " No gps fix "? or I have to write the handler for GLOBAL_POSITION_INT_COV?

how can I get the velocity in m / s

elgarbe commented 4 years ago

so, what is the topic that publish current boat velocity? I need it too...

vooon commented 4 years ago

In current plugin code i do not see that topic: https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/global_position.cpp#L77

Probably you may use global_position/raw/gps_vel or extract velocity from Odom.