ArduPilot / ardupilot

ArduPlane, ArduCopter, ArduRover, ArduSub source
http://ardupilot.org/
GNU General Public License v3.0
11.02k stars 17.57k forks source link

MAVLink/Mavros Twist/TwistStamped Msgs always ignored #7217

Closed esaattia closed 6 years ago

esaattia commented 7 years ago

Issue details

Background: Trying to control a rover using explicit setpoint_velocity messages sent over mavros. Specifically, I'm running APMrover2 under SITL on Ubuntu using ROS kinetic and the latest mavros 0.21. Looking into handling of MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED in GCS_Mavlink.cpp I observe that the messages always arrive with ignore flags (masks):

We could not control a rover (running under SITL) in guided mode by publishing to cmd_velocity topic. There were no issues with controlling a Copter the same way. We upgraded Mavros to 0.21.2 without any improvement.

After some debugging, apparently, the failure to control the robot over ROS messages is a feature, a limitation of the current ardurover (or mavros) implementations: In branch Rover-3.2 of ardupilot project (at release rc1), file GCS_Mavlink.cpp, near line 1054 (handling MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED) the ignore flags are always appear as: position - ignore, velocity - do not ignore, acceleration - ignore, yaw - ignore, yaw rate - do not ignore this combination of flags is not handled (explicitly ignored) at the end of the MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED case (if-then-else at lines 1117-1129) the combination of flags is hard-coded by mavros itself; see mavros/src/plugins/setpoint_velocity.cpp, line 75: type mask cannot be changed by the user. the two call-backs in mavros/src/plugins/setpoint_velocity.cpp (lines 91 and below) that respond to Twist and TwistStamped messages use the provided hard-coded mask. therefore, any Twist or TwistStamped message published over ROS topics would have the combination of flags that is ignored by autopilot. We implemented a workaround. If the incoming mavlink message has zero yaw rate and non-zero velocity, interpret it as setting velocity. Otherwise, if it has zero velocity and non-zero yaw rate, interpret as yaw rate. Set the ignore flags accordingly.

diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 90df2f7..1c23eb0 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -1,5 +1,6 @@
 #include "Rover.h"
 #include "version.h"
+#include <iostream>

 #include "GCS_Mavlink.h"

@@ -1040,6 +1041,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)

             // exit if vehicle is not in Guided mode
             if (rover.control_mode != &rover.mode_guided) {
+                std::cerr << "rover is not guided, break out" << std::endl;
                 break;
             }

@@ -1048,6 +1050,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
                 packet.coordinate_frame != MAV_FRAME_LOCAL_OFFSET_NED &&
                 packet.coordinate_frame != MAV_FRAME_BODY_NED &&
                 packet.coordinate_frame != MAV_FRAME_BODY_OFFSET_NED) {
+                std::cerr << "bad coordinate frame, break out" << std::endl;
                 break;
             }

@@ -1056,6 +1059,11 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             bool acc_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_ACC_IGNORE;
             bool yaw_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_IGNORE;
             bool yaw_rate_ignore = packet.type_mask & MAVLINK_SET_POS_TYPE_MASK_YAW_RATE_IGNORE;
+            // 1,0,1,1,0
+            if ( !yaw_rate_ignore && !vel_ignore ) {
+                if ( packet.yaw_rate == 0.0 && ( packet.vx != 0.0 || packet.vy != 0.0 ) ) { yaw_rate_ignore = true; }
+                if ( packet.vx == 0.0 && packet.vy == 0.0 && packet.yaw_rate != 0.0 ) { vel_ignore = true; }
+            }

             // prepare target position
             Location target_loc = rover.current_loc;
@@ -1126,6 +1134,8 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg)
             } else if (pos_ignore && vel_ignore && acc_ignore && yaw_ignore && !yaw_rate_ignore) {
                 // consume just turn rate(probably only skid steering vehicles can do this)
                 rover.mode_guided.set_desired_turn_rate_and_speed(target_turn_rate_cds, 0.0f);
+            } else {
+                std::cerr << "unsupported combination of ignore flags" << std::endl;
             }
             break;
         }

With these changes in place, we can control the rover in SITL by publishing ros messages to cmd_vel and cmd_vel_unstamped topics.

Version

ardurover-rc1 , ardurover-rc2

Platform

[ ] All [ ] AntennaTracker [ ] Copter [ ] Plane [ x] Rover [ ] Submarine

Airframe type

Skid steer frame

Hardware type

Navio2

Logs

A diff of the changes we made in order for ardurover to accept twist msgs from avros publised above .

OXINARF commented 7 years ago

You can use mavros setpoint_raw to send whatever you want, doing a workaround in the ArduPilot side isn't needed (nor makes much sense).

Supporting other flag combinations is certainly worth it, but I don't think we need an open issue for that.

khancyr commented 7 years ago

This is a known issue. https://github.com/ArduPilot/ardupilot/pull/7155 is a try to fix it. But it will also need a modification on mavros to respect correct frame

esaattia commented 6 years ago

Hi,

I will look into setpoint_raw but I still think the suggestion above will be a really useful fix.

Alot of ROS robotics modules use twistmsgs to output commands to rover base modules. Currently these modules will not work if this fix is not implemented. By introducing this fix, it will make it easier for those interfacing ROS with ardurover.

OXINARF commented 6 years ago

@esaattia As I said, supporting other flag combinations is worth it, putting hacks in our code isn't.

khancyr commented 6 years ago

I am working on the fix. It should be good next week

rmackay9 commented 6 years ago

@esaattia, this is great input, thanks.

esaattia commented 6 years ago

Thanks guys,

Thank you so much for looking into this. I'm sure people who use ROS with APM will greatly appreciate this. When this feature comes online I will be testing it and report back.

Cheers

rmackay9 commented 6 years ago

As part of reviewing @khancyr's PR, I've had a look at the changes above. So it seems like the issue with the ROS integration has something to do with both yaw-rate and velocity fields being filled in.

The change above means that if both yaw-rate and velocity are provided, we only use whichever of the two is non-zero.

Maybe the above change is what we need to do to make it all work with ROS but it's not really a great way to do it because zero is a perfectly good target for either yaw-rate or velocity and we already have flags which specify which fields should be ignored. Using the value within the field as another way to decide whether to ignore the field or not is a bit redundant.

pavloblindnology commented 6 years ago

@rmackay9 I think you're not right. Both yaw-rate and velocity are used. And velocity x-component sign is used for forward\backward throttle direction.

khancyr commented 6 years ago

@rmackay9 @pavloblindnology this has been solve with my PR. We can close this issue

pavloblindnology commented 6 years ago

Thank you @khancyr. Have checked my usage - works as expected.

esaattia commented 6 years ago

Guys does this mean ROS Twist/TwistStamped Msgs now work when both yaw rate and velocity are present?

Sorry I haven't been able to test this yet.

I noticed the RC3 release is out ! :)

khancyr commented 6 years ago

@esaattia yes it must be working ! Bonus, next mavros release will allow to work on body_frame too ^^

pavloblindnology commented 6 years ago

@esaattia Yes, it does work (master branch). Standard output from, for example, ROS navigation stack into cmd_vel (Twist) works as expected. @khancyr Isn't body_frame startup option/service already in the master?

khancyr commented 6 years ago

@pavloblindnology yes it is in mavros master so you need to compile it for now

esaattia commented 6 years ago

Hi,

We are testing the code in the next few days. Can someone confirm what version of mavros is needed to make this work ? the latest ?

esaattia commented 6 years ago

Hi, We did some tests today and were able to send commands via /mavros/setpoint_velocity/cmd_vel with 3.2-RC3 and mavros 0.21. so far it seems to be working but we have more test to do. We also tried mavros 0.22 but it was throwing a segmentation fault when we tried to run it in the field. We are currently cross compiling mavros 0.22 on a desktop before transferring it to the Raspberry pi/Navio2 board.

Someone on my team is also looking at how 3.2-RC-3 is behaving in Software in the loop. Will update you all when we get some results.

Currently, don't know why mavros 0.22 is seg faulting.

khancyr commented 6 years ago

Should be working now. Closing