ROS cmd_vel not keeping Rover at Constant Velocity

The Problem: We are using ROS to publish cmd/velocity to the pixhawk from a Nvidia Jetson TX1. We are using the following command in the terminal:

rostopic pub -r 10 /mavros/setpoint_velocity/cmd_vel_unstamped geometry_msgs/Twist ‘{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}’

Clues:

  1. We can operate the rover perfectly fine in manual mode with the controller.
  2. We can control the rovers steering servo perfectly using the above command by changing the angular coordinates to our chosen values.
  3. If we send the above command with -1 instead of -r the rover continues to repeat the command against our wishes.
  4. We tried adjusting the PID values with little to no change in the rover’s performance. The rover still does its ramp up then off routine.
  5. We attached the log file as a .txt. of QGroundcontrol PID’s desired vs. Achieved and an accompanying screenshot.
  6. Link to a video of the rover’s problem: https://www.youtube.com/watch?v=CooMsO7JrUQ

Test.log (151.5 KB)

Ideas:

  1. The ESC we are using isn’t compatible and is misinterpreting the Pixhawks output.
  2. Some sort of integer overflow is occurring.

I appreciate the help.

David,

Thanks for the report.

I suspect the starting-stopping issue is because the velocity commands are not being sent continuously. They need to be sent at least every 3seconds or the vehicle will stop. It’s possible we could modify AP to make this timeout adjustable (or just longer) but for now it’s hardcoded to require a velocity command at least once every 3 seconds.

I’m not too familiar with ROS so I’m not sure what the “-1” does. My guess is that it keeps on sending the same velocity request over and over… that would explain why ardupilot keeps moving the vehicle forward… because it just sees these new velocity requests coming in so it does what it’s told. This is a guess though… perhaps i could try and reproduce it…

The log file that we need is the onboard dataflash log file which can be downloaded from the flight controller.

Hello rmackay9,

Thank you for the response. Just to clarify a few things.

We are sending commands at a rate of 10 Hertz. Theoretically, it should meet the 3-second requirement for continuous velocity. However, when we adjust the rate using the above command, it has no effect on the rover.
log_0_2_12_2019_ROVER.bin.zip (941.9 KB)

We are using QGroundcontrol so are having trouble producing the .log file. I hope the attached .bin file will suffice.

We were able to send the velocity command just once using the notation outlined in the ROS documentation by using the following:

rostopic pub /mavros/setpoint_velocity/cmd_vel_unstamped geometry_msgs/Twist -1 ‘{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}’

If you are able to reproduce the same problem, that will be interesting to hear. Thank you for your time.

0.1 is really slow, I think the issue is that with GPS we cannot maintain this slow speed

Ah, is the velocity set to 0.1? That could interfere with the MOT_STOP_SPEED parameter which also defaults to 0.1. Perhaps try reducing MOT_STOP_SPEED to 0.05 or increase the target velocity.

Thank you for the reply. We tried increasing the velocities (all the way up to 10) with no change. The rover didn’t change its speed regardless of the value we gave it.

We are not using a GPS for our project, using a fake software GPS to spoof it. Maybe that could be leading to the issue?

Thank You rmackay9 and khancyr for the hel

We verified in steering mode that the throttle loop tuning was not able to accurately control the set speed. We were under the impression that the IMU data was sufficient for providing the EKF with a velocity value. Is there a particular reason why the Pixhawk cannot integrate acceleration with respect to time in order to obtain a velocity value?

Anyway, we decided to order some wheel encoders and in parallel are looking into using some ROS packages to obtain some vision position estimates. For visual odometry it appears we will need to use the newly supported vision_position_estimate message to pass our velocity from the ros wrapper into the ekf.

It seems as if the wheel encoders are the way to go. Any insight into the above two approaches is much appreciated.

Thank you again for all your help!

@lacks, I’ll have a look at the log linked above…

IMU + and external position source will allow the EKF to calculate an estimated velocity. I’ve done this before with the ROS+Cartographer setup described here on the wiki - there’s a video at the bottom of the page that shows the vehicle moving pretty smoothly.

I had a quick look at the logs and there doesn’t appear to be an external position source (no GPS, VISO or VISP messages in the logs). I’m afraid that IMUs (i.e. accelerometers) are not sufficient to calculate velocity because of noise and drift. Without a higher level sensor providing velocity or positions, simply integrating the accelerometer values will lead to incredible speeds within 10seconds or so.

Wheel encoders should work but it’s very easy for compass errors to lead to very inaccurate position indoors. Perhaps try the ROS/Cartographer setup linked above.

The Rover looks like it’s running 3.4.2, any chance it could be upgrade to Rover-3.5?

It looks like the ARMING_CHECK parameter has been set to 64 so nearly all checks are turned off. This is how the vehicle could be armed in Guided mode even though it doesn’t have a position source. I’d recommend turning the arming checks back on actually. I understand that can be annoying but it’s likely that the checks will tell the driver/pilot of real issues.

Thanks and good luck.

@rmackay9

Thank you for your help.

We upgraded the Rover to 3.5 and ordered some wheel encoders. In the meantime, we are trying our hands at using visual odometry.

Using ROS we were able to publish a dummy vision velocity. We can see the value under Vision_Speed_Estimate.x in the analyzer of QGroundControl. However, It appears the EKF is not taking this input into account. The rovers PID.achieved continues to not match the PID.desired. We tried the following parameters to no avail.

AHRS_EKF_TYPE 2
EKF2_ENABLE 1
EKF3_ENABLE 0
GPS_TYPE 0
AHRS_GPS_USE 0
VISO_TYPE 1

What parameters do we need to set up in order to have the EKF use the ‘dummy’ vision velocity value?

We checked out the ROS/Cartographer, is it compatible with the Intel Real Sense D435 Stereo Camera or do you need a LIDAR?

Link to bin file: https://drive.google.com/file/d/1OmXWtiM2wrRUxceGYUu8PFqD2_TC98yz/view?usp=sharing

-David Lackner

David,

The visual odometry using ZED is described here on the wiki. One key thing is from the parameters listed above it looks like EKF2 is enabled instead of EKF3.

One important concept in UAVs is the separation of estimation (which is done in the EKF) and control which is done in the speed and heading controllers. The two are very separate so not being able to achieve the desired speed or turn rate could be a control issue rather than an estimation issue. The controllers rely on the EKF estimate being accurate but it’s a separate part of the system.

Hey rmackay9,

Thank you for the help this year. We are just about to finish up our school project and I wanted to share a clip from one of our test runs.

Docking Rover

Hello,
This is very nice !
If you got some time, could make us a resume from you project with a video for the blog !I am sure that people will be interesting