Servers by jDrones

Has anyone gotten Rover to work with ROS?


(Dmitry) #21

I'm trying to follow your hints but no success so far. I have good overall programming skills but very new to ROS/robotics. Specifically what I did:
1. Use kinetic ros as provided by Ubuntu 16.04
2. Checked out mavros 0.21 tag and mavlink 2017.8.26-0; built from source; sourced devel/setup.bash
3. Checkout out branch Rover-3.2 of ardupilot, built using sim_vehicle.py (following http://ardupilot.org/dev/docs/ros-sitl.html tutorial)
4. run roscore, sim_vehicle.py -v APMrover2 -f rover-skid --console --map, and roslaunch apm.launch; enter "mode guided; arm throttle" in SITL prompt
5. I can see the /mavros/setpoint_velocity/cmd_vel_unstamped topic and publish to it (either from rqt or manually using rostopic pub) but the rover in SITL does not care and does not move.
I tried to understand more from your youtube videos, but again, not success.

Can you help me to figure out what am I doing wrong in trying to reproduce your setup?


(Dan Pollock) #22

Lots of things have changed in the rover code recently. So I would use the latest mavlink and firmware. I will have an ardurover in a few days. First thing is to get this working. I have it working on the sim so shouldn't be too much of a problem to get that done. Once I have it working I'll post up the instructions here.


(Esa Attia) #23

Hi All,

Just wondering if there is any update on this?

Just posting what my Colleague Dmitry has found so far about this problem. If would be great if we can get some input from the ardurover team

Background:
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

#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.

However, this is a hack and presumably a proper solution would be implemented in ArduRover itself. Are we able to inclue these changes in the next RC release ?


(Dan Pollock) #24

rosparam set /mavros/system_id 1 fixed it for me when the r/c out seemed dead. Use the checkid script that comes with mavros to make sure that the id's are right.