Simulating Manual Control using Mavros

Hello,
Let me start off by saying im a rookie at this pixhawk and mavros stuff. I am using Ardusub V3.5.1.
My ultimate goal here is to make the pixhawk move my motors in any of the axes, X, Y and Z. Im running Ubuntu Mate on an ARMv7 chip. (ODroid XU3)
Note that i am not using any localisation techniques. I simply want to command a movement in a certain direction.
I have tried publishing to “/mavros/setpoint_velocity/cmd_vel” with no results. Of course, i armed it beforehand with “rosrun mavros mavsafety arm”

Here is the code i got online to publish cmd_vel:

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <stdio.h>
#include “geometry_msgs/TwistStamped.h”
#include “geometry_msgs/Vector3Stamped.h”
int M = 0;
int L = 0;

int main(int argc, char **argv)
{
geometry_msgs::TwistStamped msg;
ros::init(argc, argv, “move_pix”);
ros::NodeHandle nh;

ros::Publisher velocity_message = nh.advertise<geometry_msgs::TwistStamped>(“/mavros/setpoint_velocity/cmd_vel”,100);
ros::Rate loop_rate(10);

while(ros::ok()){
msg.header.stamp = ros::Time::now();
msg.header.seq=1;

   msg.twist.linear.x = 1;
   msg.twist.linear.y = 1;
   msg.twist.linear.z = 1;
   msg.twist.angular.x = 0;
   msg.twist.angular.y = 0;
   msg.twist.angular.z = 0;

   velocity_message.publish(msg);
   ros::spinOnce();
   loop_rate.sleep();
   M++;
   L++;

}
return 0;
}

Can someone advise on how i should go about achieving my goal? Thanks a bunch :slight_smile:

1 Like

Hi @MavNav,

I’ll itemize each step to make sure that everything is correct :slight_smile:
Probably will work with the real ROV, but I’ll write a simple guide to you to test with SITL.

  • Start SITL
    • cd ardupilot/ArduSub
    • sim_vehicle.py -j4
  • Connect mavros with SITL
    • roslaunch mavros apm.launch fcu_url:=udp://0.0.0.0:14550@
  • Arm the ROV and put it on guided mode.
    • rosrun mavros mavsafety arm
    • rosrun mavros mavsys mode -c GUIDED
      • It’ll appear Mode changed.

Now, we should have everything prepared to send linear and angular messages.
To make sure that everything is correct, we can subscribe in odom to see the drone position.

  • rostopic echo /mavros/local_position/odom/pose
pose: 
  position: 
    x: -15.353191375732417
    y: -2.969302654266354
    z: -39.34264373779297
  orientation: 
    x: 0.0005757744895501046
    y: 0.0023029090765363785
    z: -0.4099183091346866
    w: -0.9121191506210589

Now, we can send a linear and angular reference with rospub.

  • rostopic pub -r 10 /mavros/setpoint_velocity/cmd_vel geometry_msgs/TwistStamped "{header: auto, twist: {linear: {x: 10.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}"

And with this, you’ll see the x go up.

1 Like

Hey! Thanks for replying.
I do have a real ROV on hand to test with. If so where do i start off? Also another thing, I’m using an Odroid XU3 as my companion computer connected to the Pixhawk using USB so i’d presume no need for the URL IP thingy?

Are you using the /dev/ttyACM0 right ? that’s the default communication device of mavros, no need to set the ip address.
You should: Start mavros normally, arm the vehicle and set guided mode, like in Arm the ROV and put it on guided mode.

I recommend to test your code with SITL first, just to make sure that your part is running without problems.

Odroid XU3 is a powerful computer, any special reason for using it ?

Yes im using /dev/tty/ACM0. I start mavros using “roslaunch bluerov apm.launch” and it succesfully connects. However, when i try to change the mode to guided, it times out with “TImed Out!”.

I’ll try to work on SITL in the following days just as a proof of concept.

No real reason for the Odroid except i just happen to have one on hand and i eventually plan to make my AUV fully autonomous with smach.

UPDATE 23/11/17: I’ve managed to get it working in SITL and it does move. I can’t get it to change modes when i just connect to the pixhawk directly. Could you help?

@MavNav take a look in /diagnostics for problems or check the mavproxy console, probably something is restricting the mode.

Hey sorry it took awhile. These are my results when i echo the /diagnostics topic:

header:
seq: 104
stamp:
secs: 1512215007
nsecs: 470157004
frame_id: ‘’
status:

level: 0
name: mavros: FCU connection
message: connected
hardware_id: /dev/pixhawk:115200
values: 
  - 
    key: Received packets:
    value: 36094
  - 
    key: Dropped packets:
    value: 0
  - 
    key: Buffer overruns:
    value: 0
  - 
    key: Parse errors:
    value: 0
  - 
    key: Rx sequence number:
    value: 25
  - 
    key: Tx sequence number:
    value: 157
  - 
    key: Rx total bytes:
    value: 1022089
  - 
    key: Tx total bytes:
    value: 2983
  - 
    key: Rx speed:
    value: 11458.000000
  - 
    key: Tx speed:
    value: 17.000000
  • level: 0
    name: mavros: Heartbeat
    message: Normal
    hardware_id: /dev/pixhawk:115200
    values:

    key: Heartbeats since startup
    value: 133
    
    • key: Frequency (Hz)
      value: 1.037217
    • key: Vehicle type
      value: Submarine
    • key: Autopilot type
      value: ArduPilotMega / ArduCopter
    • key: Mode
      value: MANUAL
    • key: System status
      value: Critical
  • level: 0
    name: mavros: System
    message: Normal
    hardware_id: /dev/pixhawk:115200
    values:

    key: Sensor present
    value: 0x0020FC07
    
    • key: Sensor enabled
      value: 0x00209C07
    • key: Sensor helth
      value: 0x0020FC07
    • key: 3D gyro
      value: Ok
    • key: 3D accelerometer
      value: Ok
    • key: 3D magnetometer
      value: Ok
    • key: 3D angular rate control
      value: Ok
    • key: attitude stabilization
      value: Ok
    • key: yaw position
      value: Ok
    • key: motor outputs / control
      value: Ok
    • key: AHRS subsystem health
      value: Ok
    • key: CPU Load (%)
      value: 32.6
    • key: Drop rate (%)
      value: 0.0
    • key: Errors comm
      value: 0
    • key: Errors count #1
      value: 0
    • key: Errors count #2
      value: 0
    • key: Errors count #3
      value: 0
    • key: Errors count #4
      value: 0
  • level: 1
    name: mavros: Battery
    message: Low voltage
    hardware_id: /dev/pixhawk:115200
    values:

    key: Voltage
    value: 0.00
    
    • key: Current
      value: -0.0
    • key: Remaining
      value: -1.0

I have observed that the system status is “Critical”. Does it have any effect on the mode change command timing out?

Hi @MavNav, critical appears when one of the failsafes are enable: battery, ekf and others.
First, you’ll need to disable then to arm, after that try to send some rc commands via /mavros/rc/override.

Ah alright. How do i disable them? And i was sending messages to setpoint_velocity/cmd_vel. So do i just change the topic name to mavros/rc/override?

You can disable then with QGC.


(This parameters are only for test ! Do not use in a real case.)

Try first to control the rc output using the manual mode to test what you are doing.

I’ve disabled arming checks as seen here:

I get this on the terminal running mavros when i run the mode change command:
“[ WARN] [1512236670.910592758]: CMD: Unexpected command 11, result 4”

Mode change to Guided still times out. I’m usng QGC on Windows to change parameters because i can’t get QGC to work in Linux.

Hi @MavNav,
I have sent you a private message, probably this will be the fastest way to understand and solve your problem.

Hi @patrickelectric
I have encounter the same problem. would you like to sent me the fastest way to understand and solve the problem?:rofl::rofl::rofl:

Hi,

Are you running a new version of ArduSub ?
Are you able to send RC values in rc/override in MANUAL mode ?

My best regards,

Yes i am running the new version of Ardusub, 3.5.2.

I don’t know what to send to Rc/Override so perhaps you could advise?

On a side note, I thought i have to use guided mode to send cmd_vel?

Ok, To send rc commands, try to use rostopic pub -r 1 /mavros/rc/override mavros_msgs/OverrideRCIn '[1300, 1300, 1300, 1300, 0, 0, 0, 0]'

Yes, you’ll need to change the mode before sending cmd_vel, try first to do basic communication, like sending rc commands and etc. Just to make sure that everything will work.

Good news! I have managed to get it to run as per your steps, in Manual mode, publishing the rc override code.

How do i progress from here, to get to guided mode cmd vel?

Good !

Now, try first to set the guided and then arm the vehicle,
maybe this can trick mavros to set the vehicle.

Minor Problem: I cannot control the speed of Main Out 2 on the pixhawk using that command, it just runs at full speed. I got it to output correctly with speed control with only 1 ESC plugged in. But when i plug 2 in it runs it on Max output.

Bigger Issue: Guided mode still times out when i try to initialise it both when running, and not.
Mavros Terminal: [ WARN] [1512617445.197459316]: CMD: Unexpected command 11, result 4

Command Terminal: ~$ rosrun mavros mavsys mode -c GUIDED
Timed out!