Getting STATUSTEXT messages

okalachev commented 7 years ago

Issue details

How to get STATUSTEXT messages, receiving from the FCU? I want to track errors, warnings, etc in my program. I found in the code, that these messages are forwarded by ROS_ERROR_STREAM_NAMED macro. But I can't understand, how to handle them by another node.

MAVROS version and platform

Mavros: ? 0.18.7 ROS: Kinetic Ubuntu: 16.04

Autopilot type and version

[X] ArduPilot [X] PX4

Version: Any

TSC21 commented 7 years ago

What status msgs are you looking for in specific? Some of the status msgs come encoded on Mavlink msgs and we just parse them into text to be presented on console. If you want specific status msgs, you should check the code where they are being parsed into text and add code so they can be sent out of the node as usable data (like in a msg topic).

okalachev commented 7 years ago

For example, I want to track error messages like "ACCELS INCONSISTENT", or emergency messages like "Engine failure. Loitering down". Can I do it without modifying the mavros code?

vooon commented 7 years ago

See how rqt console uses /rosout topic. All logging go through rosout.

TSC21 commented 7 years ago

Those msgs in general feedback from encoded flags and bitmasks that come on Mavlink msgs, so I suppose you would have to read them yourself from the Mavlink stream unless you adapt the MAVROS code. You can start by looking at here: https://github.com/mavlink/mavros/blob/master/mavros/src/lib/mavros.cpp. Or as @vooon said, you can check /rosout.

okalachev commented 7 years ago

According to this code https://github.com/mavlink/mavros/blob/master/mavros/src/plugins/sys_status.cpp#L540, STATUSTEXT messages are forwarded with ROS_ERROR_STREAM_NAMED macro with adding FCU: prefix. For some reason, I couldn't get these messages in /rosout for SITL drone, but I got them with a real quadcopter.

If I understand correctly, that all of these messages end up it /rosout topic, then it's the answer to my question.

TSC21 commented 7 years ago

For some reason, I couldn't get these messages in /rosout for SITL drone, but I got them with a real quadcopter.

What's your setup?

If I understand correctly, that all of these messages end up it /rosout topic, then it's the answer to my question.

Yeah I suppose that answers your question.

TSC21 commented 7 years ago

@okalachev so we can then close this issue, let us know what solution did you implement so anyone else that in the future wants the same feature knows how to do it. Thanks.

vooon commented 7 years ago

Note you can use Log.function to select only fcu log lines. And Log.name == node-name.

Example:

---
header: 
  seq: 106
  stamp: 
    secs: 1502351949
    nsecs: 102909310
  frame_id: ''
level: 2
name: /mavros
msg: TM: Timesync mode: NONE
file: /ws/src/mavros/mavros/src/plugins/sys_time.cpp
function: std_plugins::SystemTimePlugin::initialize
line: 178
topics: ['/rosout', '/diagnostics', '/tf', '/mavlink/from', '/mavros/radio_status', '/mavros/adsb/vehicle', '/mavros/cam_imu_sync/cam_imu_stamp', '/mavros/distance_sensor/rangefinder_pub', '/mavros/global_position/raw/fix', '/mavros/global_position/raw/gps_vel', '/mavros/global_position/global', '/mavros/global_position/local', '/mavros/global_position/rel_alt', '/mavros/global_position/compass_hdg', '/mavros/global_position/gp_origin', '/mavros/global_position/gp_lp_offset', '/mavros/hil_actuator_controls', '/mavros/hil_controls/hil_controls', '/mavros/home_position/home', '/mavros/imu/data', '/mavros/imu/mag', '/mavros/imu/temperature', '/mavros/imu/atm_pressure', '/mavros/imu/data_raw', '/mavros/local_position/pose', '/mavros/local_position/velocity', '/mavros/local_position/odom', '/mavros/manual_control/control', '/mavros/rangefinder/rangefinder', '/mavros/rc/in', '/mavros/rc/out', '/mavros/setpoint_raw/target_local', '/mavros/setpoint_raw/target_global', '/mavros/setpoint_raw/target_attitude', '/mavros/state', '/mavros/extended_state', '/mavros/battery']

TSC21 commented 7 years ago

@okalachev any updates regarding this?

okalachev commented 7 years ago

@TSC21 I think I got everything I wanted from the /rosout topic. Like this:

from rosgraph_msgs.msg import Log

# ...

def handle_rosout(self, data):
    if data.name == '/mavros':
        # handle some info from MAVROS
        if data.msg.startswith('FCU: '):
            pass  # handle STATUSTEXT message from the copter

rospy.Subscriber('/rosout', Log, handle_rosout)

Not the most straight way, actually, but it works.

TSC21 commented 7 years ago

Good to hear. Closing now

shupx commented 2 months ago

Try to set the mavros conn/heartbeat_mav_type:

    <arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
    <arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
    <node pkg="mavros" type="mavros_node" name="mavros" required="false" clear_params="true" output="screen" respawn="false">
        <param name="fcu_url" value="$(arg fcu_url)" />
        <param name="gcs_url" value="" />
        <param name="target_system_id" value="$(arg ID)" />
        <param name="target_component_id" value="1" />
        <param name="fcu_protocol" value="v2.0" />

        <!-- load blacklist, config -->
        <rosparam command="load" file="$(arg pluginlists_yaml)" />
        <rosparam command="load" file="$(arg config_yaml)" />

        <param name="conn/heartbeat_mav_type" value="GCS"/> <!--added  to receive statustext-->
    </node>

mavlink / mavros