Mavros: No hay respuesta a la llamada de servicio ~ set_stream_rate
Detalles del problema
Estoy tratando de aumentar la tasa de actualización de la IMU usando la llamada de servicio ~ set_stream_rate. Intenté aumentar y disminuir la tasa también, desafortunadamente sin éxito.
He probado lo siguiente:
En una terminal: rosservice call /mavros/set_stream_rate 1 200 1 (también lo probé con 0200 1, 0 10 1, 0 10 0 etc.)
En 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);
Ninguno de estos funcionó, no tengo idea de por qué. Tampoco sé cómo proceder con la depuración.
Versión y plataforma MAVROS
Mavros: 0.25.1
ROS: cinético
Ubuntu: 16.04
Tipo y versión de piloto automático
[] ArduPilot
[x] PX4
Versión: 1.7.3
Registros de nodo
$ 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.
Diagnóstico
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"
---
Verificar 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
Todos 18 comentarios
PX4 no admite ese mensaje. Lea los documentos del firmware PX4 sobre cómo configurar las tasas de mensajes.
vooon
en 24 may. 2018
PX4 tiene soporte para MAV_CMD_GET_MESSAGE_INTERVAL y MAV_CMD_SET_MESSAGE_INTERVAL.
https://mavlink.io/en/messages/common.html#MAV_CMD_GET_MESSAGE_INTERVAL
dagar
en 25 may. 2018
Sí, pero eso iface aún no es compatible con mavros (requiere nueva llamada de rosapi).
vooon
en 25 may. 2018
@vooon , @dagar Gracias por indicarme la dirección correcta. ¿Podría sugerir una forma de obtener el comando MAV_CMD_SET_MESSAGE_INTERVAL mavlink a PX4?
Intenté rosrun mavros mavcmd int 511 105 10000 100 1 1 1 1 , pero no tuve éxito. Los números representan lo siguiente:
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: Tuve que llenarlo con algo, de lo contrario obtuve mavcmd int: error: too few arguments
szebedy
en 1 jun. 2018
También aumentó la tasa de IMU con el módulo mavlink de PX4 mavlink stream -u 14556 -s HIGHRES_IMU -r 100 . Aún así, la tasa se mantiene en 50 cuando la verifico con rostopic hz /mavros/imu/data . ¿Qué se debe hacer fuera de mavros para que se publique el tema /mavros/imu/data más rápido?
szebedy
en 1 jun. 2018
Pruebe también el comando largo, mavcmd long 511 105 1000 0 0 0 0 0 0
vooon
en 1 jun. 2018
Gracias por la rápida respuesta @vooon
Intenté rosrun mavros mavcmd long 511 105 1000 0 0 0 0 0 , desafortunadamente la tasa sigue siendo 50.
¿Qué hace que mavros publique en /mavros/imu/data ? Descubrí que los datos se publican aquí en la función handle_attitude_quaternion , pero no he encontrado qué activa esa función.
szebedy
en 1 jun. 2018
@szebedy, el Aero está transmitiendo los datos a través de UDP, ¿verdad? ¿Cuál es el puerto que está usando? ¿Por qué se conecta a través de TCP en su lugar? mavlink stream -u 14556 -s HIGHRES_IMU -r 100 solo funcionará si el Aero está realmente transmitiendo a través de ese puerto UDP.
¿Qué hace que mavros publique en
/mavros/imu/data? Descubrí que los datos se publican aquí en la funciónhandle_attitude_quaternion, pero no he encontrado qué activa esa función.
Se activa cuando MAVROS recibe un HIGHRES_IMU msg de la transmisión.
TSC21
en 1 jun. 2018
@ TSC21 Muy buen comentario, el Aero Flight Controller en realidad está transmitiendo los datos a través de un puerto serie (/ dev / ttyS1) a la placa de cómputo. Sin embargo, para hacer las cosas "más fáciles", la placa de cómputo tiene un proxy que expone el puerto serie como un conector de red (puerto 5760).
Por lo tanto, probé mavlink stream -d /dev/ttyS1 -s HIGHRES_IMU -r 100 , pero desafortunadamente la tasa de transmisión sigue siendo 50 :(
szebedy
en 1 jun. 2018
En PX4 data_raw tener la misma tasa que HIGHRES_IMU, pero data combina ATTITUDE_QUATERNION y hr-imu, así que intenta cambiar su tasa también.
vooon
en 1 jun. 2018
la placa de cálculo tiene un proxy que expone el puerto serie como un enchufe de red (puerto 5760).
Entonces, el problema puede estar en el proxy. Garantizaría que el cuello de botella no está ahí.
TSC21
en 1 jun. 2018
Muchas gracias @vooon (y @ TSC21), ¡¡¡eso funcionó !!! Entonces las soluciones actuales son:
- Ejecute lo siguiente en QGC Mavlink Console:
mavlink stream -d /dev/ttyS1 -s HIGHRES_IMU -r 100
mavlink stream -d /dev/ttyS1 -s ATTITUDE_QUATERNION -r 100
- En una 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
¿Es posible incluir estos dos comandos en un archivo de ejecución ROS (o en un nodo ROS de C ++)?
szebedy
en 1 jun. 2018
¿Es posible incluir estos dos comandos en un archivo de ejecución ROS (o en un nodo ROS de C ++)?
Sólo hay que poner:
mavlink stream -d /dev/ttyS1 -s HIGHRES_IMU -r 100
o
mavlink stream -d /dev/ttyS1 -s ATTITUDE_QUATERNION -r 100
en https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/4070_aerofc , reprograme su placa Aero y reiníciela. Y ni siquiera necesitará usar los comandos ROS.
Cerrando esto entonces.
TSC21
en 1 jun. 2018
También solo un comentario:
Si desea obtener datos de mavros/imu/data_raw , solo necesita aumentar el flujo de HIGHRES_IMU . Si desea obtener de mavros/imu/data (actitud filtrada), simplemente aumente de ATTITUDE_QUATERNION . No es necesario aumentar para ambos (ya que aumentaría el uso de ancho de banda sin ningún motivo). En primer lugar, simplemente estaba obteniendo los datos del tema equivocado. Si emitió rostopic hz /mavros/imu/data_raw , habría visto el aumento en la tasa de datos.
TSC21
en 1 jun. 2018
Sólo hay que poner:
flujo de mavlink -d / dev / ttyS1 -s HIGHRES_IMU -r 100
o
flujo de mavlink -d / dev / ttyS1 -s ATTITUDE_QUATERNION -r 100
en https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/4070_aerofc , reprograme su placa Aero y reiníciela. Y ni siquiera necesitará usar los comandos ROS.
Cerrando esto entonces.
@ TSC21 Por alguna razón, esto no tuvo ningún efecto, ¿alguna idea de por qué? Inserté mavlink stream -d /dev/ttyS1 -s ATTITUDE_QUATERNION -r 100 al final de https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/4070_aerofc , construido con make aerofc-v1_default , luego subí con make aerofc-v1_default upload . La carga se completa, reinicio pero la tarifa de mavros/imu/data permanece 50.
szebedy
en 2 jun. 2018
@ TSC21 aero no tiene la función de script extras.txt ? Pixhawk puede ejecutar un script extras.txt desde la tarjeta SD, que permite configurar transmisiones.
vooon
en 2 jun. 2018
TSC21
en 2 jun. 2018
@ TSC21 Por alguna razón, esto no tuvo ningún efecto, ¿alguna idea de por qué? Inserté mavlink stream -d / dev / ttyS1 -s ATTITUDE_QUATERNION -r 100 al final de https://github.com/PX4/Firmware/blob/master/ROMFS/px4fmu_common/init.d/4070_aerofc , construido con make aerofc -v1_default, luego subido con make aerofc-v1_default upload. La carga se completa, reinicio pero la tasa de mavros / imu / data permanece 50.
Tal vez esté sobrescrito por esta línea: https://github.com/PX4/Firmware/blob/ab279d5fe9e014cec26add8a56bd5e676152b1c5/src/modules/mavlink/mavlink_main.cpp#L2042
Mi solución de trabajo actual es reemplazar configure_stream("ATTITUDE_QUATERNION", 50.0f); por configure_stream("ATTITUDE_QUATERNION", 100.0f); , luego compilar y cargar.
szebedy
en 2 jun. 2018
Temas relacionados
tfoote
·
5Comentarios
zhahaoyu
·
12Comentarios
watakandai
·
8Comentarios
TeixeiraRafael
·
4Comentarios
mtsakaguchi
·
5Comentarios