ArduRover integration with ROS navigation stack (obstacle avoidance) not working properly)

samar.ahmed · November 3, 2020, 8:51am

I am Integrating ArduRover V4.0.0 with ROS navigation stack I followed the instructions from this link
https://ardupilot.org/dev/docs/ros-object-avoidance.html
the ros planner trajectory is displayed correct in rviz map but the rover is not moving in the correct directions I checked the ENU->NED conversions in mavros and they exist .I appreciate if anyone can help

Thanks

soldierofhell · November 3, 2020, 10:33am

Are /cmd_vel generated by controller? (edit: sorry, probably they are because rover is moving, nevertheless it’s worth to check if they are correct, you can also try rostopic pub some cmd_vels)

samar.ahmed · November 3, 2020, 11:03am

Yes the controller is publishing the velocities message to the /cmd_vel correctly the problem is that it moves to wrong direction

soldierofhell · November 3, 2020, 1:34pm

And then you send them with mavros setpoint_velocity, right?

samar.ahmed · November 3, 2020, 1:48pm

I remapped the /mavros/setpoint_velocity/cmd_vel_unstamped topic to /cmd_vel topic in mavros/node.launch so it is sent directly to mavros

soldierofhell · November 3, 2020, 4:00pm

Ok, then maybe check all the options of mav_frame service in /setpoint_velocity and compare what you’re setting with mavlink message sent. If that doesn’t work, post all your launch files here, maybe someone will have time to verify. In general this kind of mismatch should be easy to debug, at least if you already know that this is somewhere between mavros and ardupilot

samar.ahmed · November 3, 2020, 7:51pm

I will try that. Thank you

samar.ahmed · November 9, 2020, 10:01am

@soldierofhell thank you for your advice I changed the mav_frame for setpoint_velocity in apm_config.yaml file to BODY_NED and the velocities conversions are as I expect however sometimes the rover still drifts from the actual planner path do you have any idea of what could causing this issue

soldierofhell · November 9, 2020, 11:15am

You mean drift from local planner not from global? Is this Ackermann rover? What local planner are you using? Maybe also read about rover controller in Ardupilot dev guide

samar.ahmed · November 9, 2020, 11:36am

No its not an Ackermann rover . I am using Trajectory planner as my local planner and for the drift it drifts from both local and global planners’ plans . I appreciate if you can share the rover controller in Ardupilot link you mean

soldierofhell · November 9, 2020, 12:33pm

Here’s a general description of Ardurover’s modes: https://ardupilot.org/dev/docs/rover-adding-a-new-drive-mode.html

In general investigate carefully https://github.com/ArduPilot/ardupilot/blob/master/Rover/mode_guided.cpp

IMO here’s the relevant code and seems like nothing like simple PIDs is done here so maybe only PID tuning is required: https://github.com/ArduPilot/ardupilot/blob/87564f145b1fa8892cc81f62e466bd73be465e87/Rover/mode_guided.cpp#L74

But this steering/throttle control is better suited for ackermann, you must check how it’s converted to diffdrive robot you have

Let us know what you find

samar.ahmed · November 9, 2020, 1:11pm

thank you I will check these links and get back to you

soldierofhell · November 9, 2020, 2:07pm

steering/throttle -> throttle left/right function for diffdrive:

github.com

ArduPilot/ardupilot/blob/87564f145b1fa8892cc81f62e466bd73be465e87/Rover/AP_MotorsUGV.cpp#L664


    set_limits_from_input(armed, steering, throttle);


    // constrain steering
    steering = constrain_float(steering, -4500.0f, 4500.0f);


    // always allow steering to move
    SRV_Channels::set_output_scaled(SRV_Channel::k_steering, steering);
}


// output to skid steering channels
void AP_MotorsUGV::output_skid_steering(bool armed, float steering, float throttle, float dt)
{
    if (!have_skid_steering()) {
        return;
    }


    // clear and set limits based on input
    set_limits_from_input(armed, steering, throttle);


    // constrain steering
    steering = constrain_float(steering, -4500.0f, 4500.0f);

soldierofhell · November 9, 2020, 4:10pm

I was thinking a bit about this control for diffdrive in Ardurover and I don’t get it. I think the Twist (linear and angular velocity) should be converted to left and right wheel linear velocity with standard kinematics and then for each wheel PID vel -> throttle shoud be applied. Something like in ros_controllers: https://github.com/ros-controls/ros_controllers/blob/698f85b2c3467dfcc3ca5743d68deba03f3fcff2/diff_drive_controller/src/diff_drive_controller.cpp#L390.
But maybe it’s just because the default stream is for ackermann throttle/steering and it was not designed for diffdrive

samar.ahmed · November 9, 2020, 4:24pm

our driving mode is skid steering and as I can see in AP_MotorsUGV.cpp it is supported it is strange that it still drifts

samar.ahmed · November 11, 2020, 10:25am

@soldierofhell , I found this discussing it mention the steering/throttle rates tuning do you think it might could help the ardupilot to control the rover better ?