Home        Store        Docs        Blog

Simulating Manual Control using Mavros


(MavNav) #1

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:


[Autonomous BlueROV2] MavLink/Ardusub help
(Patrick José Pereira) #2

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.


(MavNav) #3

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?


(Patrick José Pereira) #4

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 ?


(MavNav) #5

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.


(MavNav) #6

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?


(Patrick José Pereira) #7

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


(MavNav) #8

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?


(Patrick José Pereira) #9

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.


(MavNav) #10

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?


(Patrick José Pereira) #11

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.


(MavNav) #12

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.


Simulating Manual Control using Mavros
(Patrick José Pereira) #13

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