search

mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
900 stars 994 forks source link

Distance Sensor Plugin Msg not received by FCU #734

Closed JD-ETH closed 7 years ago

JD-ETH commented 7 years ago

I am converting a spinning lidar information to distance sensor information, and sequentially sent it over to my FCU. But after setup the plugin and make sure my node is publishing data, I cant oberserve the Mavlink messages on the MavLink inspector of QGroundControl

Mavros Node info:

Node [/mavros] Publications:

  • /mavros/manual_control/control [mavros_msgs/ManualControl]
  • /mavros/imu/data_raw [sensor_msgs/Imu]
  • /mavros/imu/mag [sensor_msgs/MagneticField]
  • /mavros/radio_status [mavros_msgs/RadioStatus]
  • /mavros/setpoint_raw/target_local [mavros_msgs/PositionTarget]
  • /mavros/imu/atm_pressure [sensor_msgs/FluidPressure]
  • /mavros/altitude [mavros_msgs/Altitude]
  • /mavros/mission/waypoints [mavros_msgs/WaypointList]
  • /mavros/global_position/compass_hdg [std_msgs/Float64]
  • /mavros/distance_sensor/rangefinder_pub [sensor_msgs/Range]
  • /mavlink/from [mavros_msgs/Mavlink]
  • /mavros/local_position/pose [geometry_msgs/PoseStamped]
  • /mavros/imu/data [sensor_msgs/Imu]
  • /mavros/wind_estimation [geometry_msgs/TwistStamped]
  • /mavros/setpoint_raw/target_global [mavros_msgs/GlobalPositionTarget]
  • /mavros/global_position/global [sensor_msgs/NavSatFix]
  • /mavros/imu/temperature [sensor_msgs/Temperature]
  • /mavros/px4flow/raw/optical_flow_rad [mavros_msgs/OpticalFlowRad]
  • /diagnostics [diagnostic_msgs/DiagnosticArray]
  • /mavros/global_position/raw/fix [sensor_msgs/NavSatFix]
  • /mavros/battery [sensor_msgs/BatteryState]
  • /mavros/setpoint_raw/target_attitude [mavros_msgs/AttitudeTarget]
  • /mavros/cam_imu_sync/cam_imu_stamp [mavros_msgs/CamIMUStamp]
  • /mavros/state [mavros_msgs/State]
  • /mavros/rc/in [mavros_msgs/RCIn]
  • /rosout [rosgraph_msgs/Log]
  • /mavros/global_position/local [nav_msgs/Odometry]
  • /mavros/hil_actuator_controls [mavros_msgs/HilActuatorControls]
  • /tf [tf2_msgs/TFMessage]
  • /mavros/vfr_hud [mavros_msgs/VFR_HUD]
  • /mavros/px4flow/ground_distance [sensor_msgs/Range]
  • /mavros/px4flow/temperature [sensor_msgs/Temperature]
  • /mavros/hil_controls/hil_controls [mavros_msgs/HilControls]
  • /mavros/global_position/rel_alt [std_msgs/Float64]
  • /mavros/local_position/odom [nav_msgs/Odometry]
  • /mavros/extended_state [mavros_msgs/ExtendedState]
  • /mavros/local_position/velocity [geometry_msgs/TwistStamped]
  • /mavros/global_position/raw/gps_vel [geometry_msgs/TwistStamped]
  • /mavros/rc/out [mavros_msgs/RCOut]
  • /mavros/time_reference [sensor_msgs/TimeReference]

Subscriptions:

  • /mavros/setpoint_attitude/attitude [unknown type]
  • /mavros/actuator_control [unknown type]
  • /mavros/setpoint_raw/global [unknown type]
  • /mavros/setpoint_attitude/att_throttle [unknown type]
  • /mavros/setpoint_attitude/cmd_vel [unknown type]
  • /tf [tf2_msgs/TFMessage]
  • /mavros/setpoint_raw/attitude [unknown type]
  • /mavros/setpoint_velocity/cmd_vel [unknown type]
  • /mavros/setpoint_position/local [unknown type]
  • /mavlink/to [mavros_msgs/Mavlink]
  • /mavros/mocap/pose [unknown type]
  • /tf_static [unknown type]
  • /mavros/vision_pose/pose_cov [unknown type]
  • /mavros/fake_gps/fix [unknown type]
  • /mavros/setpoint_accel/accel [unknown type]
  • /mavros/rc/override [unknown type]
  • /mavros/setpoint_raw/local [unknown type]
  • /mavros/vision_pose/pose [unknown type]
  • /mavros/vision_speed/speed_vector [unknown type]
  • /mavros/distance_sensor/lidar_split6 [sensor_msgs/Range]
  • /mavros/distance_sensor/lidar_split7 [sensor_msgs/Range]
  • /mavros/distance_sensor/lidar_split4 [sensor_msgs/Range]
  • /mavros/distance_sensor/lidar_split5 [sensor_msgs/Range]
  • /mavros/distance_sensor/lidar_split2 [sensor_msgs/Range]
  • /mavros/distance_sensor/lidar_split3 [sensor_msgs/Range]
  • /mavros/distance_sensor/lidar_split0 [sensor_msgs/Range]
  • /mavros/distance_sensor/lidar_split1 [sensor_msgs/Range]

Services:

  • /mavros/cmd/set_home
  • /mavros/param/get
  • /mavros/ftp/checksum
  • /mavros/cmd/land
  • /mavros/ftp/mkdir
  • /mavros/param/pull
  • /mavros/mission/push
  • /mavros/ftp/close
  • /mavros/ftp/rmdir
  • /mavros/ftp/list
  • /mavros/cmd/command_int
  • /mavros/param/push
  • /mavros/param/set
  • /mavros/cmd/takeoff
  • /mavros/ftp/truncate
  • /mavros/get_loggers
  • /mavros/mission/pull
  • /mavros/cmd/command
  • /mavros/ftp/rename
  • /mavros/ftp/open
  • /mavros/ftp/read
  • /mavros/ftp/remove
  • /mavros/ftp/reset
  • /mavros/mission/clear
  • /mavros/set_stream_rate
  • /mavros/set_mode
  • /mavros/cmd/arming
  • /mavros/cmd/trigger_control
  • /mavros/set_logger_level
  • /mavros/mission/set_current
  • /mavros/ftp/write

contacting node http://erle-brain-3:40925/ ... Pid: 1114 Connections:

  • topic: /rosout
    • to: /rosout
    • direction: outbound
    • transport: TCPROS
  • topic: /tf
    • to: /mavros
    • direction: outbound
    • transport: INTRAPROCESS
  • topic: /tf
  • topic: /mavros/distance_sensor/lidar_split0
  • topic: /mavros/distance_sensor/lidar_split1
  • topic: /mavros/distance_sensor/lidar_split2
  • topic: /mavros/distance_sensor/lidar_split3
  • topic: /mavros/distance_sensor/lidar_split4
  • topic: /mavros/distance_sensor/lidar_split5
  • topic: /mavros/distance_sensor/lidar_split6
  • topic: /mavros/distance_sensor/lidar_split7

Issue details

Please describe the problem, or desired feature

MAVROS version and platform

Mavros: 0.18.4 ROS: Kinetic on Erle Brain 3 board (rapsberry pi 3) Rapsberry pi linux

Autopilot type and version

[X] ArduPilot [ ] PX4

Version: 3.4 rc2

Node logs

* /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/vibration/frame_id: vibration
 * /mavros/vision_pose/tf/child_frame_id: vision
 * /mavros/vision_pose/tf/frame_id: local_origin
 * /mavros/vision_pose/tf/listen: False
 * /mavros/vision_pose/tf/rate_limit: 10.0
 * /mavros/vision_speed/listen_twist: False
 * /rosdistro: kinetic
 * /rosversion: 1.12.4

NODES
  /
    mavros (mavros/mavros_node)

ROS_MASTER_URI=http://192.168.1.21:11311

core service [/rosout] found
process[mavros-1]: started with pid [2448]
[ INFO] [1497959270.030383777]: FCU URL: udp://:6001@
[ WARN] [1497959270.037466893]: init: message from ASLUAV, MSG-ID 201 ignored! Table has different entry.
[ WARN] [1497959270.039582448]: init: message from autoquad, MSG-ID 150 ignored! Table has different entry.
[ WARN] [1497959270.040285619]: init: message from autoquad, MSG-ID 152 ignored! Table has different entry.
[ WARN] [1497959270.041842480]: init: message from matrixpilot, MSG-ID 150 ignored! Table has different entry.
[ WARN] [1497959270.042189665]: init: message from matrixpilot, MSG-ID 151 ignored! Table has different entry.
[ WARN] [1497959270.042470912]: init: message from matrixpilot, MSG-ID 152 ignored! Table has different entry.
[ WARN] [1497959270.042724764]: init: message from matrixpilot, MSG-ID 153 ignored! Table has different entry.
[ WARN] [1497959270.043053199]: init: message from matrixpilot, MSG-ID 155 ignored! Table has different entry.
[ WARN] [1497959270.043303405]: init: message from matrixpilot, MSG-ID 156 ignored! Table has different entry.
[ WARN] [1497959270.043565694]: init: message from matrixpilot, MSG-ID 157 ignored! Table has different entry.
[ WARN] [1497959270.044004128]: init: message from matrixpilot, MSG-ID 158 ignored! Table has different entry.
[ WARN] [1497959270.044386989]: init: message from matrixpilot, MSG-ID 170 ignored! Table has different entry.
[ WARN] [1497959270.044636310]: init: message from matrixpilot, MSG-ID 171 ignored! Table has different entry.
[ WARN] [1497959270.044888652]: init: message from matrixpilot, MSG-ID 172 ignored! Table has different entry.
[ WARN] [1497959270.045153597]: init: message from matrixpilot, MSG-ID 173 ignored! Table has different entry.
[ WARN] [1497959270.045328752]: init: message from matrixpilot, MSG-ID 174 ignored! Table has different entry.
[ WARN] [1497959270.045770727]: init: message from matrixpilot, MSG-ID 175 ignored! Table has different entry.
[ WARN] [1497959270.046116766]: init: message from matrixpilot, MSG-ID 176 ignored! Table has different entry.
[ WARN] [1497959270.046408430]: init: message from matrixpilot, MSG-ID 177 ignored! Table has different entry.
[ WARN] [1497959270.046670668]: init: message from matrixpilot, MSG-ID 178 ignored! Table has different entry.
[ WARN] [1497959270.046924259]: init: message from matrixpilot, MSG-ID 179 ignored! Table has different entry.
[ WARN] [1497959270.047087018]: init: message from matrixpilot, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1497959270.047239413]: init: message from matrixpilot, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1497959270.047390088]: init: message from matrixpilot, MSG-ID 182 ignored! Table has different entry.
[ WARN] [1497959270.048885440]: init: message from paparazzi, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1497959270.049087886]: init: message from paparazzi, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1497959270.049244864]: init: message from paparazzi, MSG-ID 182 ignored! Table has different entry.
[ WARN] [1497959270.049394967]: init: message from paparazzi, MSG-ID 183 ignored! Table has different entry.
[ WARN] [1497959270.049545122]: init: message from paparazzi, MSG-ID 184 ignored! Table has different entry.
[ WARN] [1497959270.051171618]: init: message from slugs, MSG-ID 170 ignored! Table has different entry.
[ WARN] [1497959270.052009736]: init: message from slugs, MSG-ID 172 ignored! Table has different entry.
[ WARN] [1497959270.052920770]: init: message from slugs, MSG-ID 173 ignored! Table has different entry.
[ WARN] [1497959270.053576702]: init: message from slugs, MSG-ID 176 ignored! Table has different entry.
[ WARN] [1497959270.054142686]: init: message from slugs, MSG-ID 177 ignored! Table has different entry.
[ WARN] [1497959270.055958244]: init: message from slugs, MSG-ID 179 ignored! Table has different entry.
[ WARN] [1497959270.056486156]: init: message from slugs, MSG-ID 180 ignored! Table has different entry.
[ WARN] [1497959270.056772195]: init: message from slugs, MSG-ID 181 ignored! Table has different entry.
[ WARN] [1497959270.057099328]: init: message from slugs, MSG-ID 184 ignored! Table has different entry.
[ WARN] [1497959270.057357711]: init: message from slugs, MSG-ID 185 ignored! Table has different entry.
[ WARN] [1497959270.057759582]: init: message from slugs, MSG-ID 186 ignored! Table has different entry.
[ WARN] [1497959270.058162964]: init: message from slugs, MSG-ID 191 ignored! Table has different entry.
[ WARN] [1497959270.058452962]: init: message from slugs, MSG-ID 192 ignored! Table has different entry.
[ WARN] [1497959270.058767803]: init: message from slugs, MSG-ID 193 ignored! Table has different entry.
[ WARN] [1497959270.059073686]: init: message from slugs, MSG-ID 194 ignored! Table has different entry.
[ INFO] [1497959270.064208850]: udp0: Bind address: 0.0.0.0:6001
[ INFO] [1497959270.066583881]: GCS URL: udp://192.168.1.21:9000@192.168.1.15:6000?ids=1,255,252
[ INFO] [1497959270.067911005]: udp1: Bind address: 192.168.1.21:9000
[ INFO] [1497959270.069196098]: udp1: Remote address: 192.168.1.15:6000
[ INFO] [1497959270.202211930]: udp0: Remote address: 127.0.0.1:48257
[ INFO] [1497959270.296453305]: Plugin 3dr_radio loaded
[ INFO] [1497959270.311758485]: Plugin 3dr_radio initialized
[ INFO] [1497959270.313355190]: Plugin actuator_control loaded
[ INFO] [1497959270.338230444]: Plugin actuator_control initialized
[ INFO] [1497959270.339788139]: Plugin altitude loaded
[ INFO] [1497959270.349352744]: Plugin altitude initialized
[ INFO] [1497959270.527090013]: Plugin cam_imu_sync loaded
[ INFO] [1497959270.534796040]: Plugin cam_imu_sync initialized
[ INFO] [1497959270.536735034]: Plugin command loaded
[ INFO] [1497959270.584068584]: Plugin command initialized
[ INFO] [1497959270.584661027]: Plugin distance_sensor blacklisted
[ INFO] [1497959270.586532208]: Plugin ftp loaded
[ INFO] [1497959270.656520251]: Plugin ftp initialized
[ INFO] [1497959270.658434922]: Plugin global_position loaded
[ INFO] [1497959270.666159178]: udp1: Remote address: 192.168.1.15:6000
[ INFO] [1497959270.723951440]: Plugin global_position initialized
[ INFO] [1497959270.725816528]: Plugin hil_actuator_controls loaded
[ INFO] [1497959270.733624950]: Plugin hil_actuator_controls initialized
[ INFO] [1497959270.735393216]: Plugin hil_controls loaded
[ INFO] [1497959270.741883785]: Plugin hil_controls initialized
[ INFO] [1497959270.743679499]: Plugin imu_pub loaded
[ INFO] [1497959270.805239124]: Plugin imu_pub initialized
[ INFO] [1497959270.807190774]: Plugin local_position loaded
[ INFO] [1497959270.855175568]: Plugin local_position initialized
[ INFO] [1497959270.857009198]: Plugin manual_control loaded
[ INFO] [1497959270.863422268]: Plugin manual_control initialized
[ INFO] [1497959270.865104128]: Plugin mocap_fake_gps loaded
[ INFO] [1497959270.887442842]: Plugin mocap_fake_gps initialized
[ INFO] [1497959270.889502512]: Plugin mocap_pose_estimate loaded
[ INFO] [1497959270.926181206]: Plugin mocap_pose_estimate initialized
[ INFO] [1497959270.928877432]: Plugin param loaded
[ INFO] [1497959270.955572723]: Plugin param initialized
[ INFO] [1497959270.957196042]: Plugin px4flow loaded
[ INFO] [1497959271.001147434]: Plugin px4flow initialized
[ INFO] [1497959271.003322780]: Plugin rc_io loaded
[ INFO] [1497959271.039460437]: Plugin rc_io initialized
[ INFO] [1497959271.039948557]: Plugin safety_area blacklisted
[ INFO] [1497959271.042386870]: Plugin setpoint_accel loaded
[ INFO] [1497959271.073370769]: Plugin setpoint_accel initialized
[ INFO] [1497959271.075479813]: Plugin setpoint_attitude loaded
[ INFO] [1497959271.179819434]: Plugin setpoint_attitude initialized
[ INFO] [1497959271.181972905]: Plugin setpoint_position loaded
[ INFO] [1497959271.234478598]: Plugin setpoint_position initialized
[ INFO] [1497959271.236864307]: Plugin setpoint_raw loaded
[ INFO] [1497959271.330973912]: Plugin setpoint_raw initialized
[ INFO] [1497959271.333159882]: Plugin setpoint_velocity loaded
[ INFO] [1497959271.355446982]: Plugin setpoint_velocity initialized
[ INFO] [1497959271.358490080]: Plugin sys_status loaded
[ INFO] [1497959271.417892016]: Plugin sys_status initialized
[ INFO] [1497959271.420356109]: Plugin sys_time loaded
[ INFO] [1497959271.447331918]: Plugin sys_time initialized
[ INFO] [1497959271.448738781]: Plugin vfr_hud loaded
[ INFO] [1497959271.461419557]: Plugin vfr_hud initialized
[ INFO] [1497959271.461639451]: Plugin vibration blacklisted
[ INFO] [1497959271.463271885]: Plugin vision_pose_estimate loaded
[ INFO] [1497959271.535506210]: Plugin vision_pose_estimate initialized
[ INFO] [1497959271.537433693]: Plugin vision_speed_estimate loaded
[ INFO] [1497959271.566288809]: Plugin vision_speed_estimate initialized
[ INFO] [1497959271.568921234]: Plugin waypoint loaded
[ INFO] [1497959271.608461049]: Plugin waypoint initialized
[ INFO] [1497959271.609459009]: Built-in SIMD instructions: None
[ INFO] [1497959271.611587116]: Built-in MAVLink package version: 2016.9.9
[ INFO] [1497959271.612198986]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad matrixpilot paparazzi slugs uAvionix ualberta
[ INFO] [1497959271.612813876]: MAVROS started. MY ID 1.240, TARGET ID 1.1
[ WARN] [1497959272.403148100]: IMU: linear acceleration on RAW_IMU known on APM only.
[ WARN] [1497959272.403314765]: IMU: ~imu/data_raw stores unscaled raw acceleration report.
[ INFO] [1497959272.416997095]: RC_CHANNELS message detected!
[ WARN] [1497959272.421678930]: TM: Wrong FCU time.
[ INFO] [1497959273.223722115]: CON: Got HEARTBEAT, connected. FCU: ArduPilotMega / ArduCopter
[ WARN] [1497959273.411899294]: GP: No GPS fix
[ INFO] [1497959273.419448916]: RC_CHANNELS message detected!
[ WARN] [1497959273.454114919]: FCU: EKF2 IMU0 has stopped aiding
[ INFO] [1497959273.455681989]: FCU: EKF2 IMU0 is using optical flow
[ INFO] [1497959274.303437436]: VER: 1.1: Capabilities         0x0000000000001bcf
[ INFO] [1497959274.304078004]: VER: 1.1: Flight software:     030500c0 (87abccef)
[ INFO] [1497959274.304831539]: VER: 1.1: Middleware software: 00000000 (        )
[ INFO] [1497959274.305064402]: VER: 1.1: OS software:         00000000 (        )
[ INFO] [1497959274.305239400]: VER: 1.1: Board hardware:      00000000
[ INFO] [1497959274.306027831]: VER: 1.1: VID/PID:             0000:0000
[ INFO] [1497959274.306261266]: VER: 1.1: UID:                 0000000000000000
[ WARN] [1497959274.309943474]: CMD: Unexpected command 520, result 0
[ WARN] [1497959278.462578277]: FCU: EKF2 IMU0 has stopped aiding
[ INFO] [1497959278.472419129]: FCU: EKF2 IMU0 is using optical flow
[ INFO] [1497959283.242121994]: FCU: APM:Copter V3.5.0-rc8 (87abccef)
[ INFO] [1497959283.244447755]: FCU: Frame: QUAD
[ WARN] [1497959283.588171509]: FCU: EKF2 IMU0 has stopped aiding
[ INFO] [1497959283.589156709]: FCU: EKF2 IMU0 is using optical flow
[ INFO] [1497959285.035445014]: PR: parameters list received
[ INFO] [1497959288.243640724]: WP: mission received

Diagnostics

place here result of:
rostopic echo -n1 /diagnostics

Check ID

rosrun mavros checkid
TSC21 commented 7 years ago

Please follow the issue topics as requested above and edit the comment.

vooon commented 7 years ago

How you check messages sent from mavros to FCU? You need some sort of sniffer tool to see what packets are sent upstream. How you connect to FCU? Because regular QGC only shows what FCU send (downstream).

JD-ETH commented 7 years ago

@vooon My fcu is a erle ros hat running ardupilot. The connection is fine now, I am able to send rc/override message from ground control station, and receiving messages on both ends for example. (imu data both on GCS and erle linux board). Messages are being published in the corresponding topics as sensor/Range.

Can you recommand any sniffer tool?

TSC21 commented 7 years ago

@JD-ETH doesn't Ardupilot publish DISTANCE_SENSOR msgs to the ground station? If yes, then you can basically write a wrapper on Ardupilot side that publishes back the msgs that it is receiving.

vooon commented 7 years ago

You may use WireShark, with some setup it should be able to deduce mavlink packets (need wlua).

TSC21 commented 7 years ago

My fcu is a erle ros

This is a Linux-based autopilot. I suppose you can sniff the msgs easily on it.

TSC21 commented 7 years ago

@JD-ETH are you already able to check if the msgs are being sent?

TSC21 commented 7 years ago

@JD-ETH updates?

JD-ETH commented 7 years ago

@TSC21 Sorry for the late reply.

I was not confident with WireShark nor any sniffing tools and I was in a hurry to find a solution(I was in a hackathon). So I worked around and used RC/Override instead on the linux board. the result using just RC/Override https://www.youtube.com/watch?v=aWOI7xQgk4Q&t=4s. The code was quite easy too running on ros.

I couldn't see the distance sensor message being published and after a lot of trouble with Erle brain, I am currently switching to Pixhawk2 (cube). I plan to test on that system instead and see whether the plugin works better there.

TSC21 commented 7 years ago

Oh cool stuff @JD-ETH! Well what were you using for position estimation? Why didn't you use velocity setpoints instead of overriding the RC controller?

TSC21 commented 7 years ago

I couldn't see the distance sensor message being published

Why don't you write a mavlink sender on the Firmware side that publishes back what it receives? Like a ping?

TSC21 commented 7 years ago

Oh now I see. You were in ALTHOLD and just letting it drift and avoid the walls in the process.

TSC21 commented 7 years ago

In the mean time, if you are willing to find a solution for DISTANCE_SENSOR publishing for Laser Scans, I'll leave this PR open. Otherwise, we can close it. But it would be cool if you could get to a solution useful on this case (not the practical case of the Hackaton).

JD-ETH commented 7 years ago

@TSC21 I would love to help, but now that I am switching to Pixhawk 2 that doesn't natively run ROS, I wonder if this could still help? My setup is going to be Odroid XU4 + pixhawk 2.1 (cube). I am quite unsure still whether I need ROS running on the odroid yet.

I didn't have position estimation because I had big trouble using px4flow on the erle board. So as cool as the object avoidance is, this won't work in real life :P

TSC21 commented 7 years ago

@TSC21 I would love to help, but now that I am switching to Pixhawk 2 that doesn't natively run ROS, I wonder if this could still help? My setup is going to be Odroid XU4 + pixhawk 2.1 (cube). I am quite unsure still whether I need ROS running on the odroid yet.

Pixhawk 2 is a Mavlink-based autopilot, so it can run both PX4 and APM. And since it is Mavlink-based, it can pair with MAVROS.

I'm closing this then. If you have intentions on doing a PR to add support for LaserScan in the distance_sensor plugin, you are welcomed.