mavlink / mavros

MAVLink to ROS gateway with proxy for Ground Control Station
Other
902 stars 993 forks source link

mavros_msgs::Status.armed not properly set as boolean #990

Closed ndepal closed 6 years ago

ndepal commented 6 years ago

The armed flag in mavros_msgs::Status is not properly written as a boolean true when the vehicle is armed. It is written to 128.

The reason for this is that booleans in ROS messages are really uint8s, so there is no implicit cast to bool in the below line, as was perhaps assumed.

https://github.com/mavlink/mavros/blob/1b0600f0a6a0606ca70b72eb240f21baf8b8544c/mavros/src/plugins/sys_status.cpp#L657

The problem with this is that the following check does not work:

if (status.armed == true) {
    // will never get here
}

This is problematic as one would assume that this works since the documentation/the message definition claims that status.armed is a boolean.

The below works of course:

if (status.armed) {
    // will get here
}

Changing the above line to the following fixes the issue:

state_msg->armed = (bool)(hb.base_mode & enum_value(MAV_MODE_FLAG::SAFETY_ARMED));
ndepal commented 6 years ago

Thanks!