Servers by jDrones

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:

Servers by jDrones