Cannot Arm nor change mode (GUIDED) with Mavros and ArduRover 4.0

Dear All,

Here is the set up: there is a companion computer (NUC) which is connected via TELEM2 to the ArduRover

On the NUC, we have a 2D SLAM which is computing x,y and yaw. These values are sent to the ArduRover via Mavros.

When I launch all the nodes, I can see that after few seconds, the EKF is correctly initialized as it is shown on the MissionPlanner’ screenshot below:

My problem is the following: I cannot arm neither change the mode from MANUAL to GUIDED using a “wellknown” piece of code which is detailed at the end f the message.

but if I do: rosrun mavros mavsafety arm, I am able to arm the rover.

However if I do : rosrun mavros mavsys -c GUIDED, I got an error from ArduRover “Flight Mode change failed” but I do not know the reason…

So can we use the code below to run MAVROS with ArduRover? If so please can you tell me where am I wrong? Besides I noticed some Clock/Time Syncronisation between ArduRover and MAVROS (even though I set BRD_RTC_TYPES = 2). Mavros topics are updated anyway.

Actually I have an additional question because we are using PX4 for quadrotors and in order to set the OFFBOARD mode (GUIDED for ardurover), you need to send at a given frequency setpoints for instance. Is it the same with GUIDED and ardurover?

I attached the list of parameters as well because there are a lot of parameters I have changed and may be there is one I missed…1 29-05-2020 11-26-05.bin.log.param (15.7 KB)

I have also logs but I do not know which one to provide (.bin, bin.kmz, log or log.gpx), please tell me which is the one that you may want to have a look to.

Any help warmly appreciated.

@rmackay9 sorry to bother you but I saw that you are very active in ROS + Ardurover so may I ask you if you have any hint about these issues ?

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
      ros::NodeHandle nh;

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");

//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);

// wait for FCU connection
while(ros::ok() && !current_state.connected){
    ros::spinOnce();
    rate.sleep();
}

geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;

//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
    local_pos_pub.publish(pose);
    ros::spinOnce();
    rate.sleep();
}

mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "GUIDED";

mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;

ros::Time last_request = ros::Time::now();

while(ros::ok()){
    if( current_state.mode != "GUIDED" &&
        (ros::Time::now() - last_request > ros::Duration(5.0))){
        if( set_mode_client.call(offb_set_mode) &&
            offb_set_mode.response.mode_sent){
            ROS_INFO("Guided enabled");
        }
        last_request = ros::Time::now();
    } else {
        if( !current_state.armed &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( arming_client.call(arm_cmd) &&
                arm_cmd.response.success){
                ROS_INFO("Vehicle armed");
            }
            last_request = ros::Time::now();
        }
    }

    local_pos_pub.publish(pose);

    ros::spinOnce();
    rate.sleep();
}

return 0;

}

Hi!

I have no experience with ardurover, but I faced a similar issue with arducopter. I don’t know if yours is the same issue, but I had to set the global position explicitly. Ardupilot requires a global position, even if it will only use relative movment commands.

You can explicitly set the global position in different ways. To test if this is the issue, you can right click in the mavproxy map and click “Set origin”. If this is the issue and you want to automate it, you can use the mavros topic /mavros/global_position/set_gp_origin.

It is not sufficient to set it to (0, 0, 0), it has to be a plausible point. I read somewhere that the controller uses the position to correct the magnetometer for known disturbances.

The post I asked when I had the same issue: Guided mode with optical flow, without GPS, in simulation

@Henrik

Hi, thank you very much for your message and indeed you are right by using the script set_origin.py I was able to arm and switch to guided mode. OUF and thanks again
May I ask you a couple of questions:

  • Once you are in Guided mode, do you need to send periodically the position you want to reach to the ArduPilot (otherwise ArduPilot switches back to Manual) or not?
  • I saw in another arduPilot page that we need to set SYSID_MYGCS 1 (to accept control from mavros) - did you do it as well?
    Thank you again for your support
    Fabrice

Hi

  • I don’t know. We used a COTS optical flow sensor connected directly to the flight controller, so it was never an issue for us.
  • I was not aware that I had to set SYSID_MYGCS, so I did not. It worked in SITL, but I have not tried it on a real drone yet. Either it is set automatically somehow(in either mavros or SITL) or it is not required for simple operation.

Sorry that I can’t help more, but hopes it helps a little.

@Henrik thank you very much - you helped me a lot already :slight_smile:

Hi @Henrik and @fabono,
Sorry for reviving this subject but it’s really in the right direction of the doubts I’m having.

I do have a GPS connected, although currently I’m mainly doing tests inside (due to weather conditions), so I won’t be using it for the time being

I’ve tried do change EK2_GPS_TYPE set to ‘3’ and GPS_TYPE to ‘0’ and on mission planner I set EKF Origin, although every time I try to change mode using:

rosrun mavros mavsys mode -c GUIDED

I continue getting “Flight mode change failed”.

@Henrik, Following your suggestion I’ve also tried to set through mavros:

rostopic pub -1 /mavros/global_position/gp_origin geographic_msgs/GeoPointStamped ‘[0, now, base_link]’ ‘[49.8571735, 8.5784662, 100]’

I don’t get any feedback from the FC, so I’m so sure if was actually set (I might have a look into the logs to see if I find anything…)

@fabono Did you get any problems related to the EKF2?

Any help would be greatly appreciated.

Difficult to say exactly what the problem is here.

If you got no response from the FC, do you know that it reset the origin? You can see in the mavproxy map if the origin is set where you expect it to be(and you can set it there by right clicking).

In my mavros version(which is a little old by now) it seems that the topic is called /mavros/global_position/set_gp_origin. Are you sure that you are publishing to the correct topic?

Hi @Henrik,

Thank you for your answer!

Ah yes… You’re absolutely right… typo in the command led me to publish it to the wrong topic.

I did try to publish it to the right topic this time:

rostopic pub -1 /mavros/global_position/set_gp_origin geographic_msgs/GeoPointStamped ‘[0, now, base_link]’ ‘[49.8571735, 8.5784662, 100]’

But unfortunately the result is basically the same. i.e. mavros doesn’t say anything and I can’t figure out if origin is is actually set.
One thing I’ve noticed while echoing:

rostopic echo /mavros/global_position/gp_origin

Is that I’m basically getting the Home position. I’ve tried to set up a new one and that is was actually being output in this topic (I thought it was the origin).

I don’t have MavProxy installed, but I’ll install it and check if I get the set origin there.

I guess that then origin is correctly set since you get the new one on /mavros/global_position/gp_origin. It is a good idea to check with mavproxy, but now I guess the issue is somewhere else.

I have unfortunately not used ardurover. How do you get the relative position? Is it possible to use wheel odometry? fabono used a 2D SLAM system to get the position and I think(at least for copter) you need some relative position measurement.

And… here is where I humbly realize that I still have a lot to learn. I guess I could use something like what is described here.
So, here is how I pictured it. The starting position is wherever the rover is (I would rather prefer if it doesn’t have to rely on GPS at all, although ultimately it will be outside, so Global positioning will come in handy). But the whole idea is to have to rover chase my cat around :slight_smile:, making use of a Object detection NN (running on a Jetson Nano - the companion computer where I’m running mavros).

So, in my innocence I thought I could just publish to /mavros/setpoint_position/local using the message geometry_msgs/PoseStamped:

std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w

and readjust as the cat is moving (always considering the new starting point). And I thought that using something like this:

rostopic pub -1 /mavros/setpoint_position/local geometry_msgs/PoseStamped ‘[0, now, base_link]’ ‘[{x: 1.0, y: 1.0, z: 10.0}, {x: 0.0, y: 0.0, z: 0.0, w: 1.0}]’

as many times as necessary, would just do the trick.

To be honest I don’t know how much does a unit represent in terms of distance in the coordinate axis (it is on my to-do list), but independently, I need to set it GUIDED mode first.

Am I completely off?!?

Do you think I still need to a relative position in this scenario?

If that’s the case, I’ll still to figure out how can I achieve that! :wink:

@Carlos_Vicente @Henrik Atually with Ardurover it is manadatoy to initialize the EKF with a geographic_msgs::GeoPointStamped message. Otherwise it is not possible to arm the rover. And I am almost sure it is the same for any kind of drone (quadcopter, boat or rover). here is the piece of code I implemented:

rover_set_gp = nh_.advertise<geographic_msgs::GeoPointStamped>("/mavros/global_position/set_gp_origin", 10);
std::cout << "Set INIT KF GPS\n";
		// need to send a specifi message to initiate the EKF on ARDUROVER - otherwise we cannot arm nor switch to GUIDED mode
		geographic_msgs::GeoPointStamped geo;
		geo.header.stamp = ros::Time::now();
		geo.position.latitude = 0;
		geo.position.longitude = 0;
		geo.position.altitude = 0;
		rover_set_gp.publish(geo);

Hope it may help

Fabrice

I worked on a quite similar project with a copter and you are not far off. The only thing left is a sensor to measure the relative movement. For ardurover to move to the setpoints you publish on /mavros/setpoint_position/local it needs to be able to track the position along the path to the setpoint. E.g. it needs to know if it has reached the setpoint.

The builtin IMU could in theory provide this, but in is nowhere near precise enough.

I guess that the wheel encoders are quite suitable for you. You can also consider the other options on its parrent page. Since you know you are on the ground, maybe you can make do with less radio beacons(eg. ex1, ex2) for 2D localization. I don’t see that it is mentioned in the rover documentation, but for copters optical flow sensors present a quite nice and cheap alternative for this scenario. If you mount any of these below the rover, it should be possible to estimate your position the same way a computer mouse does it. (I don’t know if this is implemented in ardurover)

While still mentioning that I have not used ardurover, I think you can use /mavros/setpoint_position/local. You probably need to take some care with the orientation so ardurover don’t come up with vastly different paths each timestep when you are close to the target. If this gives issues, you can consider to send velocity setpoints in the direction of your cat instead.

Just in case it was not obvious from fabonos answer: You should set the initial position to approxamentley your actual location, not just [0, 0, 0]. Ardupilot uses this location to correct for the distortions in earths magnetic field when using the magnetometer.

PS: Your project sounds cool, just please be very careful not to hurt the cat when testing :slight_smile:.

Hi @fabono,
Thank you very much for your answer.
I haven’t created my own nodes up until now, but it will definitely come to that. I was just trying to test it out with out with just what mavros was offering me already. In a nutshell wouldn’t you agree that issuing,

rostopic pub -1 /mavros/global_position/set_gp_origin geographic_msgs/GeoPointStamped ‘[0, now, base_link]’ ‘[49.8571735, 8.5784662, 100]’

could also me used to set the origin?

Now I’m starting to wonder if this might be related to the status of my EKF. When I look into your screenshot of Mission Planner with EKF Status, mine has a couple of red flags (screenshot below). On the other hand this red flags could be related by the fact that I have a GPS and I’m currently testing inside, although I did set:

GPS_TYPE = 0 (previously 9)
EK2_GPS_TYPE = 3 (previously 0)

By the way, out of curiosity, and because like I said, I’ll have to create my own nodes for sure, what would you say is the best source to have look into the cpp API for mavros?

Hi @Henrik,

Ah yes… of course!!.. Independently of the way we measuring it, we do need to know when we traveled enough distance to get there. Good point!

Last I heard Optical flow sensors are not supported by ardurover yet and the problem with the beacons is that… well… it would only work on places where I had beacons installed :slight_smile: (unless I missing something). I think, up until now, the most appropriate solution would be the wheel encoders.

Yeah… it still feels a bit far ahead. But while using Object detection I can retrieve a X,Y coordinates of where the object is in the picture. So, I would have to translate that into the /mavros/setpoint_position/local somehow and be clever with the updates.

Thanks @Henrik! :slight_smile: No… I wouldn’t hurt the cat… either way, the RC platform I will be using is a RC crawler type… so. generally they are not fast! :- :stuck_out_tongue_winking_eye:

Well… one thing is for sure… with 3D Fix I have no problem at all to set it to GUIDED Mode.

Possibly I have to have another look into the variables set. I should be missing something, whenever I’m not using GPS… maybe something related to this page on GPS/Non-GPS transition.

@Henrik, I’ve also realized that actually, using wheel encoders might not be the best option after all. The idea of the wheel encoders is to have a motor for each wheel, for instance, one for the left wheel and another for the right wheel so that we can calculate how much has each wheel traveled at a given instance and estimate its position in the coordinate axis (but you probably already knew this :slight_smile: ). Maybe Visual odometry…
Since I’ll be using a RC crawler type with motors already installed (1 for the rear axis and another for the front axis) I guess the whole idea of wheel encoders fall apart.

I still think that you can’t change to guided because it can’t calculate its position. When using GPS it can know where it is, but without it can’t. Setting origin is just a start, you also need something else to get new position updates(e.g. wheel encoders).

Non-GPS navigation is a challenge and will need some extra work. I agree that beacons might impose a too large restriction. However, I am not convinced that visual odometry is less work than retrofitting wheel encoders. You already have a companion computer, which simplifes an eventual visual odometry setup. If you have the budget you could consider the Intel T265 (e.g. this toutorial or this approach. I have tried neither.).

I think the problem may be that EKF2 won’t accept the set-origin command unless EK2_MAG_CAL = 3 (No GPS). This also means that the EKF won’t accept GPS input at all though so if you’re trying to do both GPS and Non-GPS then I think you’ll need to use Rover-4.1 and enable EKF3 and follow the instructions on the GPS/Non-GPS transitions wiki page (that you’ve already found).

I’ve actually done GPS+WheelEncoders for GPS/Non-GPS transitions using just AP and it worked pretty well. There’s a lua script available that helps decide when to switch.

That’s what I was thinking. Moreover I guess it wouldn’t be a simple retrofitting of wheel encoders, because I think I would still need to change the whole mechanics of the crawler to have a motor for the left side of the car and another to right instead of front axis and rear axis, I’m reckin… :thinking:

EDIT: Actually, after some more investigation I found that could probably get rotary/shaft encoders to equip each of the wheels independently. Sorry about this… I’m back and forth with the plan…

I really appreciate the help and ideas!

Hi @rmackay9,

Thanks for joining in into the discussion.

You’re referred to EK2_MAG_CAL, but value 3 means “After first climb yaw reset”. Just double checking if this was the variable that you meant. I have it set to “never” at the moment.

I´m pretty ignorant in some aspects. When you say “just AP”, are you referring to APSync?

Also, can I ask what rotary/shaft encoder did you use? Or… did you use motors already equipped with it?

Ah, sorry, I wrote EK2_MAG_CAL but I meant EK2_GPS_TYPE = 3 (No GPS).

When I said, “just AP” I mean “just ArduPilot”. So no companion computer at all. I used motors which already had wheel encoders equipped (from Pololu.com). The flight controller was a Hex CubeOrange which is powerful enough to run this lua script.