Mavros: No response to ~set_stream_rate service call
Issue details
I am trying to increase the update rate of the IMU using the ~set_stream_rate service call. I tried increasing and decreasing the rate as well, unfortunately without success.
I have tried the following:
In a terminal: rosservice call /mavros/set_stream_rate 1 200 1 (also tried it with 0 200 1, 0 10 1, 0 10 0 etc.)
In C++:
#include <mavros_msgs/StreamRate.h>
ros::ServiceClient stream_rate_client;
ros::NodeHandle nh;
stream_rate_client = nh.serviceClient<mavros_msgs::StreamRate>("/mavros/set_stream_rate");
mavros_msgs::StreamRate streamRate;
streamRate.request.stream_id = 1;
streamRate.request.message_rate = 200;
streamRate.request.on_off = true;
stream_rate_client.call(streamRate);
None of these worked, have no idea why. I also don't know how to proceed with debugging.
MAVROS version and platform
Mavros: 0.25.1
ROS: Kinetic
Ubuntu: 16.04
Autopilot type and version
[ ] ArduPilot
[ x ] PX4
Version: 1.7.3
Node logs
$ rosrun mavros mavros_node _fcu_url:=tcp://127.0.0.1:5760 _system_id:=2
[ INFO] [1527166997.534725386]: FCU URL: tcp://127.0.0.1:5760
[ INFO] [1527166997.538179020]: tcp0: Server address: 127.0.0.1:5760
[ INFO] [1527166997.538583530]: GCS URL: udp://@
[ INFO] [1527166997.538851787]: udp1: Bind address: 0.0.0.0:14555
[ INFO] [1527166997.582075509]: Plugin 3dr_radio loaded
[ INFO] [1527166997.587216072]: Plugin 3dr_radio initialized
[ INFO] [1527166997.587433590]: Plugin actuator_control loaded
[ INFO] [1527166997.602908470]: Plugin actuator_control initialized
[ INFO] [1527166997.608098772]: Plugin adsb loaded
[ INFO] [1527166997.624933409]: Plugin adsb initialized
[ INFO] [1527166997.625174728]: Plugin altitude loaded
[ INFO] [1527166997.629202189]: Plugin altitude initialized
[ INFO] [1527166997.629439220]: Plugin cam_imu_sync loaded
[ INFO] [1527166997.631736889]: Plugin cam_imu_sync initialized
[ INFO] [1527166997.632005870]: Plugin command loaded
[ INFO] [1527166997.656271690]: Plugin command initialized
[ INFO] [1527166997.656518171]: Plugin debug_value loaded
[ INFO] [1527166997.679747716]: Plugin debug_value initialized
[ INFO] [1527166997.680021635]: Plugin distance_sensor loaded
[ WARN] [1527166997.684616922]: DS: plugin not configured!
[ INFO] [1527166997.684730488]: Plugin distance_sensor initialized
[ INFO] [1527166997.684972781]: Plugin fake_gps loaded
[ INFO] [1527166997.728127889]: Plugin fake_gps initialized
[ INFO] [1527166997.728480748]: Plugin ftp loaded
[ INFO] [1527166997.758929019]: Plugin ftp initialized
[ INFO] [1527166997.759219651]: Plugin global_position loaded
[ INFO] [1527166997.818296662]: Plugin global_position initialized
[ INFO] [1527166997.818588156]: Plugin hil loaded
[ INFO] [1527166997.882781780]: Plugin hil initialized
[ INFO] [1527166997.883048549]: Plugin home_position loaded
[ INFO] [1527166997.903052314]: Plugin home_position initialized
[ INFO] [1527166997.903807895]: Plugin imu loaded
[ INFO] [1527166997.933612538]: Plugin imu initialized
[ INFO] [1527166997.933850257]: Plugin local_position loaded
[ INFO] [1527166997.952751657]: Plugin local_position initialized
[ INFO] [1527166997.953008414]: Plugin manual_control loaded
[ INFO] [1527166997.968141910]: Plugin manual_control initialized
[ INFO] [1527166997.968481980]: Plugin mocap_pose_estimate loaded
[ INFO] [1527166997.984059425]: Plugin mocap_pose_estimate initialized
[ INFO] [1527166997.984304531]: Plugin obstacle_distance loaded
[ INFO] [1527166997.997733823]: Plugin obstacle_distance initialized
[ INFO] [1527166997.998059268]: Plugin odom loaded
[ INFO] [1527166998.013623175]: Plugin odom initialized
[ INFO] [1527166998.013984083]: Plugin param loaded
[ INFO] [1527166998.023299249]: Plugin param initialized
[ INFO] [1527166998.023588331]: Plugin px4flow loaded
[ INFO] [1527166998.042473344]: Plugin px4flow initialized
[ INFO] [1527166998.042727126]: Plugin rangefinder loaded
[ INFO] [1527166998.045338152]: Plugin rangefinder initialized
[ INFO] [1527166998.045632059]: Plugin rc_io loaded
[ INFO] [1527166998.062006661]: Plugin rc_io initialized
[ INFO] [1527166998.062537311]: Plugin safety_area loaded
[ INFO] [1527166998.083768132]: Plugin safety_area initialized
[ INFO] [1527166998.084033551]: Plugin setpoint_accel loaded
[ INFO] [1527166998.097458792]: Plugin setpoint_accel initialized
[ INFO] [1527166998.097776863]: Plugin setpoint_attitude loaded
[ INFO] [1527166998.135016088]: Plugin setpoint_attitude initialized
[ INFO] [1527166998.135395910]: Plugin setpoint_position loaded
[ INFO] [1527166998.199004407]: Plugin setpoint_position initialized
[ INFO] [1527166998.199288389]: Plugin setpoint_raw loaded
[ INFO] [1527166998.247947782]: Plugin setpoint_raw initialized
[ INFO] [1527166998.248232176]: Plugin setpoint_velocity loaded
[ INFO] [1527166998.276827990]: Plugin setpoint_velocity initialized
[ INFO] [1527166998.277159823]: Plugin sys_status loaded
[ INFO] [1527166998.313704869]: Plugin sys_status initialized
[ INFO] [1527166998.314003064]: Plugin sys_time loaded
[ INFO] [1527166998.325176275]: TM: Timesync mode: MAVLINK
[ INFO] [1527166998.327852553]: Plugin sys_time initialized
[ INFO] [1527166998.328095534]: Plugin vfr_hud loaded
[ INFO] [1527166998.330919928]: Plugin vfr_hud initialized
[ INFO] [1527166998.331147359]: Plugin vibration loaded
[ INFO] [1527166998.335247172]: Plugin vibration initialized
[ INFO] [1527166998.335499616]: Plugin vision_pose_estimate loaded
[ INFO] [1527166998.369932485]: Plugin vision_pose_estimate initialized
[ INFO] [1527166998.370189466]: Plugin vision_speed_estimate loaded
[ INFO] [1527166998.383793912]: Plugin vision_speed_estimate initialized
[ INFO] [1527166998.384077019]: Plugin waypoint loaded
[ INFO] [1527166998.403765289]: Plugin waypoint initialized
[ INFO] [1527166998.404032333]: Plugin wind_estimation loaded
[ INFO] [1527166998.406681323]: Plugin wind_estimation initialized
[ INFO] [1527166998.406813352]: Built-in SIMD instructions: SSE, SSE2
[ INFO] [1527166998.406874153]: Built-in MAVLink package version: 2018.5.5
[ INFO] [1527166998.406924542]: Known MAVLink dialects: common ardupilotmega ASLUAV autoquad icarous matrixpilot paparazzi slugs standard uAvionix ualberta
[ INFO] [1527166998.406979693]: MAVROS started. MY ID 2.240, TARGET ID 1.1
[ INFO] [1527166998.408225699]: IMU: Attitude quaternion IMU detected!
[ INFO] [1527166998.408474430]: IMU: High resolution IMU detected!
[ INFO] [1527166999.200899783]: CON: Got HEARTBEAT, connected. FCU: PX4 Autopilot
[ INFO] [1527166999.204518434]: IMU: High resolution IMU detected!
[ INFO] [1527166999.204972608]: IMU: Attitude quaternion IMU detected!
[ INFO] [1527167000.246101700]: VER: 1.1: Capabilities 0x00000000000024ef
[ INFO] [1527167000.246399932]: VER: 1.1: Flight software: 01060500 (00000000B9C7CD26)
[ INFO] [1527167000.246488372]: VER: 1.1: Middleware software: 01060500 (00000000B9C7CD26)
[ INFO] [1527167000.246534273]: VER: 1.1: OS software: 000000c0 (0000000000000000)
[ INFO] [1527167000.246748090]: VER: 1.1: Board hardware: 0000a4a7
[ INFO] [1527167000.246867818]: VER: 1.1: VID/PID: 0525:a4a7
[ INFO] [1527167000.246908907]: VER: 1.1: UID: 3335511839343335
[ WARN] [1527167000.247388481]: CMD: Unexpected command 520, result 0
[ WARN] [1527167013.218527443]: PR: request param #452 timeout, retries left 2, and 1 params still missing
[ INFO] [1527167014.241870430]: WP: mission received
[ WARN] [1527167359.915573803]: CMD: Unexpected command 200, result 4
[ WARN] [1527168934.267074100]: TM: Clock skew detected (-763580720.440153122 s). Hard syncing clocks.
Diagnostics
header:
seq: 3416
stamp:
secs: 1527169233
nsecs: 907997653
frame_id: ''
status:
-
level: 0
name: "mavros: FCU connection"
message: "connected"
hardware_id: "tcp://127.0.0.1:5760"
values:
-
key: "Received packets:"
value: "60196"
-
key: "Dropped packets:"
value: "0"
-
key: "Buffer overruns:"
value: "0"
-
key: "Parse errors:"
value: "0"
-
key: "Rx sequence number:"
value: "159"
-
key: "Tx sequence number:"
value: "39"
-
key: "Rx total bytes:"
value: "147758546"
-
key: "Tx total bytes:"
value: "580048"
-
key: "Rx speed:"
value: "100808.000000"
-
key: "Tx speed:"
value: "390.000000"
-
level: 0
name: "mavros: GPS"
message: "3D fix"
hardware_id: "tcp://127.0.0.1:5760"
values:
-
key: "Satellites visible"
value: "8"
-
key: "Fix type"
value: "3"
-
key: "EPH (m)"
value: "1.24"
-
key: "EPV (m)"
value: "2.39"
-
level: 0
name: "mavros: Heartbeat"
message: "Normal"
hardware_id: "tcp://127.0.0.1:5760"
values:
-
key: "Heartbeats since startup"
value: "2229"
-
key: "Frequency (Hz)"
value: "0.962947"
-
key: "Vehicle type"
value: "Quadrotor"
-
key: "Autopilot type"
value: "PX4 Autopilot"
-
key: "Mode"
value: "AUTO.RTL"
-
key: "System status"
value: "Standby"
-
level: 0
name: "mavros: System"
message: "Normal"
hardware_id: "tcp://127.0.0.1:5760"
values:
-
key: "Sensor present"
value: "0x00000000"
-
key: "Sensor enabled"
value: "0x00000000"
-
key: "Sensor helth"
value: "0x00000000"
-
key: "CPU Load (%)"
value: "77.8"
-
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://127.0.0.1:5760"
values:
-
key: "Voltage"
value: "17.91"
-
key: "Current"
value: "0.0"
-
key: "Remaining"
value: "100.0"
---
Check ID
WARNING: mavros/target_system_id not set. Used default value: 1
WARNING: mavros/target_component_id not set. Used default value: 1
NOTE: Target parameters may be unset, but that may be result of incorrect --mavros-ns option.Double check results!
OK. I got messages from 1:1.
---
Received 9307 messages, from 1 addresses
sys:comp list of messages
1:1 0, 1, 2, 266, 140, 141, 147, 24, 30, 31, 32, 33, 36, 74, 76, 83, 85, 87, 230, 231, 105, 111, 241, 242, 245
szebedy
All 18 comments
PX4 do not support that message. Read PX4 firmware docs on how to configure message rates.
vooon
on 24 May 2018
PX4 has support for MAV_CMD_GET_MESSAGE_INTERVAL and MAV_CMD_SET_MESSAGE_INTERVAL.
https://mavlink.io/en/messages/common.html#MAV_CMD_GET_MESSAGE_INTERVAL
dagar
on 25 May 2018
Yes, but that iface is not yet supported by mavros (requires new rosapi call).
vooon
on 25 May 2018
@vooon , @dagar Thanks for pointing me to the right direction. Could you suggest a way to get the MAV_CMD_SET_MESSAGE_INTERVAL mavlink command to PX4?
I tried rosrun mavros mavcmd int 511 105 10000 100 1 1 1 1, but no success. The numbers stand for the following:
511: https://mavlink.io/en/messages/common.html#MAV_CMD_GET_MESSAGE_INTERVAL
105: https://github.com/PX4/Firmware/blob/4453e4201b7a245cff52beeb38a293161aea4c48/Tools/mavlink_px4.py#L506
1 1 1 1: Had to fill up with something, otherwise got mavcmd int: error: too few arguments
szebedy
on 1 Jun 2018
Also increased the IMU rate with the mavlink module of PX4 mavlink stream -u 14556 -s HIGHRES_IMU -r 100. Still the rate stays 50 when I check it with rostopic hz /mavros/imu/data. What needs to be done outside of mavros so that is publishes the /mavros/imu/data topic faster?
szebedy
on 1 Jun 2018
Try also command long, mavcmd long 511 105 1000 0 0 0 0 0 0
vooon
on 1 Jun 2018
Thanks for the quick reply @vooon
Tried rosrun mavros mavcmd long 511 105 1000 0 0 0 0 0, unfortunately the rate is still 50.
What triggers mavros to publish on /mavros/imu/data? I've found that the data is published here in the function handle_attitude_quaternion, but I haven't found what triggers that function.
szebedy
on 1 Jun 2018
@szebedy the Aero is streaming the data through UDP right? What's the port it is using? Why are you plugging through TCP instead? mavlink stream -u 14556 -s HIGHRES_IMU -r 100 will only work if the Aero is actually streaming through that UDP port.
What triggers mavros to publish on
/mavros/imu/data? I've found that the data is published here in the functionhandle_attitude_quaternion, but I haven't found what triggers that function.
It triggers when MAVROS receives a HIGHRES_IMU msg from the stream.
TSC21
on 1 Jun 2018
@TSC21 Very good remark, the Aero Flight Controller is actually streaming the data through a serial port (/dev/ttyS1) to the compute board. However, to make things "easier", the compute board has a proxy which exposes the serial port as a network socket (port 5760).
Therefore I tried mavlink stream -d /dev/ttyS1 -s HIGHRES_IMU -r 100, but unfortunately the streaming rate remains 50 :(
szebedy
on 1 Jun 2018
On PX4 data_raw shoud have same rate as HIGHRES_IMU, but data combine ATTITUDE_QUATERNION and hr-imu, so try change it's rate too.
vooon
on 1 Jun 2018
the compute board has a proxy which exposes the serial port as a network socket (port 5760).
Then the problem may be on the proxy. I would guarantee that the bottleneck is not there.
TSC21
on 1 Jun 2018
Thank you so much @vooon (and @TSC21), that did the trick!!! So the current solutions are:
- Execute the following in QGC Mavlink Console:
mavlink stream -d /dev/ttyS1 -s HIGHRES_IMU -r 100
mavlink stream -d /dev/ttyS1 -s ATTITUDE_QUATERNION -r 100
- In a terminal
rosrun mavros mavcmd long 511 105 10000 0 0 0 0 0
rosrun mavros mavcmd long 511 31 10000 0 0 0 0 0
Is it possible to include these two commands in a ROS launch file (or in a C++ ROS node)?
szebedy
on 1 Jun 2018
Is it possible to include these two commands in a ROS launch file (or in a C++ ROS node)?
Just put:
mavlink stream -d /dev/ttyS1 -s HIGHRES_IMU -r 100
or
mavlink stream -d /dev/ttyS1 -s ATTITUDE_QUATERNION -r 100
in https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/4070_aerofc, reprogram your Aero board and reboot it. And you won't need to even use the ROS commands.
Closing this then.
TSC21
on 1 Jun 2018
Also just a remark:
If you want to get data from mavros/imu/data_raw, you just need to increase the stream of HIGHRES_IMU. If you want to get from mavros/imu/data (filtered attitude), you just increase from ATTITUDE_QUATERNION. Not need to increase for both (as you would increase the bandwidth usage for no reason). You were simply getting the data from the wrong topic in the first place. If you issued rostopic hz /mavros/imu/data_raw, you would have saw the increase on the data rate.
TSC21
on 1 Jun 2018
Just put:
mavlink stream -d /dev/ttyS1 -s HIGHRES_IMU -r 100
or
mavlink stream -d /dev/ttyS1 -s ATTITUDE_QUATERNION -r 100
in https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/4070_aerofc, reprogram your Aero board and reboot it. And you won't need to even use the ROS commands.
Closing this then.
@TSC21 For some reason this didn't have any effect, any idea why? I inserted mavlink stream -d /dev/ttyS1 -s ATTITUDE_QUATERNION -r 100 to the end of https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/4070_aerofc, built with make aerofc-v1_default, then uploaded with make aerofc-v1_default upload. The upload completes, I reboot but the rate of mavros/imu/data stays 50.
szebedy
on 2 Jun 2018
@TSC21 aero does not have extras.txt script feature? Pixhawk can execute extras.txt script from SD card, that allows to setup streams.
vooon
on 2 Jun 2018
TSC21
on 2 Jun 2018
@TSC21 For some reason this didn't have any effect, any idea why? I inserted mavlink stream -d /dev/ttyS1 -s ATTITUDE_QUATERNION -r 100 to the end of https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/4070_aerofc, built with make aerofc-v1_default, then uploaded with make aerofc-v1_default upload. The upload completes, I reboot but the rate of mavros/imu/data stays 50.
Maybe it's overwritten by this line: https://github.com/PX4/Firmware/blob/ab279d5fe9e014cec26add8a56bd5e676152b1c5/src/modules/mavlink/mavlink_main.cpp#L2042
My current working solution is to replace configure_stream("ATTITUDE_QUATERNION", 50.0f); to configure_stream("ATTITUDE_QUATERNION", 100.0f);, then build and upload.
szebedy
on 2 Jun 2018
Related issues
shening
·
10Comments
mohand150
·
5Comments
mtsakaguchi
·
5Comments
y22ma
·
7Comments
Phadadev
·
4Comments