RB5 Drone and ROS

alfa budiman · 11 Jul 2022, 18:51

My understanding is that the RB5 drone runs PX4 and uses it to connect to the host PC running Q Ground Control. I'd like to run ROS on my host PC and use that to control the RB5 drone / process sensor data instead of Q Ground Control. Though, I'm also open to working on top of Q Ground Control (ROS controls the drone through Q Ground Control).

I've read about MAVLINK / MAVROS as well as Dronekit but am running into issues installing / troubleshooting those packages. What approach do you reccomend to facilitate computer control of the RB5 Drone using ROS?

Cliff Wong · 11 Jul 2022, 20:26

Easiest is to get ros-base installed to run mavros. To install:

Install ros-melodic-ros-base via http://wiki.ros.org/melodic/Installation/Ubuntu
(-desktop-full should work too, but to be safe, based on system image version you may have, to avoid any dependency issues).

execute: sudo apt-get install ros-melodic-mavros
execute: sudo apt-get install ros-melodic-tf2-geometry-msgs ros-melodic-tf2-sensor-msgs
execute: sudo geographiclib-get-geoids egm96-5
execute: /opt/ros/melodic/lib/mavros/install_geographiclib_datasets.sh
execute sudo apt-get install ros-melodic-stereo-image-proc ros-melodic-navigation

ROS and RB5 work the same as other PX4/VOXL products: that it leverages PX4 offboard mode to send mavros command/data. An example ROS (to px4 via mavros) application can be found here that you can drop into your ros workspace.

alfa budiman · 13 Jul 2022, 16:18

I executed those commands successfully, I have MAVROS on my computer, and also got the example to build properly.
However, my issue is that I can't connect the drone to MAVROS.

I read a post here about MAVROS and the RB5 drone. That didn't work either: https://forum.modalai.com/topic/725/mavros-integration-fcu-url/5

I run

$ roslaunch mavros px4.launch fcu_url:=udp://127.0.0.1:14540@:14558 tgt_system:=1

$ rostopic echo /diagnostics

And this is the result

header: 
  seq: 22
  stamp: 
    secs: 1657728868
    nsecs: 113630722
  frame_id: ''
status: 
  - 
    level: 1
    name: "mavros: FCU connection"
    message: "not connected"
    hardware_id: "udp://127.0.0.1:14540@:14558"
    values: 
      - 
        key: "Received packets:"
        value: "0"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "0"
      - 
        key: "Tx sequence number:"
        value: "0"
      - 
        key: "Rx total bytes:"
        value: "0"
      - 
        key: "Tx total bytes:"
        value: "0"
      - 
        key: "Rx speed:"
        value: "0.000000"
      - 
        key: "Tx speed:"
        value: "0.000000"
  - 
    level: 2
    name: "mavros: GPS"
    message: "No satellites"
    hardware_id: "udp://127.0.0.1:14540@:14558"
    values: 
      - 
        key: "Satellites visible"
        value: "0"
      - 
        key: "Fix type"
        value: "0"
      - 
        key: "EPH (m)"
        value: "Unknown"
      - 
        key: "EPV (m)"
        value: "Unknown"
  - 
    level: 2
    name: "mavros: Heartbeat"
    message: "No events recorded."
    hardware_id: "udp://127.0.0.1:14540@:14558"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "0"
      - 
        key: "Frequency (Hz)"
        value: "0.000000"
      - 
        key: "Vehicle type"
        value: "Generic micro air vehicle"
      - 
        key: "Autopilot type"
        value: "Generic autopilot"
      - 
        key: "Mode"
        value: ''
      - 
        key: "System status"
        value: "Uninit"
  - 
    level: 0
    name: "mavros: System"
    message: "Normal"
    hardware_id: "udp://127.0.0.1:14540@:14558"
    values: 
      - 
        key: "Sensor present"
        value: "0x00000000"
      - 
        key: "Sensor enabled"
        value: "0x00000000"
      - 
        key: "Sensor health"
        value: "0x00000000"
      - 
        key: "CPU Load (%)"
        value: "0.0"
      - 
        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: 2
    name: "mavros: Battery"
    message: "No data"
    hardware_id: "udp://127.0.0.1:14540@:14558"
    values: 
      - 
        key: "Voltage"
        value: "-1.00"
      - 
        key: "Current"
        value: "0.0"
      - 
        key: "Remaining"
        value: "0.0"
  - 
    level: 2
    name: "mavros: Time Sync"
    message: "No events recorded."
    hardware_id: "udp://127.0.0.1:14540@:14558"
    values: 
      - 
        key: "Timesyncs since startup"
        value: "0"
      - 
        key: "Frequency (Hz)"
        value: "0.000000"
      - 
        key: "Last RTT (ms)"
        value: "0.000000"
      - 
        key: "Mean RTT (ms)"
        value: "0.000000"
      - 
        key: "Last remote time (s)"
        value: "0.000000000"
      - 
        key: "Estimated time offset (s)"
        value: "0.000000000"
---

The RB5 drone is turned on and it is on the same wifi network as my computer; I am ssh'd into it. I am also able to view its camera and VIO data from my computer. The lights indicating PX4 connectivity are also on, and I confirmed PX4 was active as per https://docs.modalai.com/Qualcomm-Flight-RB5-user-guide-px4/

How do I connect MAVROS with the RB5 drone?

alfa budiman · 13 Jul 2022, 19:52

I was able to connect the RB5 drone with MAVROS by running

roslaunch mavros px4.launch fcu_url:="udp://:14540@{IP_ADDR}:14558"

where {IP_ADDRESS} is the same as the one I use to ssh into the drone.

My issue now is I can't arm the drone. I tried arming the drone from Q Ground Control, and it tells me it needs GPS signal. This is a problem because I will be using it indoors where it can't get any GPS signal. I did try bringing the drone and arming it outside, the GPS was able to pick up, but the signal was better but not good enough; I got several GPS related errors on Q ground control and still couldn't arm the drone.

How do I arm the drone without GPS?

I tried arming it using rosservice call /mavros/cmd/arming True and rosrun mavros mavsafety arm and it still wouldn't arm. When I run these commands with Q Ground Control Running, I see the same GPS related errors.

This is the result from rostopic echo /diagnostics

header: 
  seq: 404
  stamp: 
    secs: 1657741334
    nsecs: 457078498
  frame_id: ''
status: 
  - 
    level: 0
    name: "mavros: FCU connection"
    message: "connected"
    hardware_id: "udp://:14540@192.168.82.128:14558"
    values: 
      - 
        key: "Received packets:"
        value: "35173"
      - 
        key: "Dropped packets:"
        value: "0"
      - 
        key: "Buffer overruns:"
        value: "0"
      - 
        key: "Parse errors:"
        value: "0"
      - 
        key: "Rx sequence number:"
        value: "246"
      - 
        key: "Tx sequence number:"
        value: "0"
      - 
        key: "Rx total bytes:"
        value: "1317646"
      - 
        key: "Tx total bytes:"
        value: "167304"
      - 
        key: "Rx speed:"
        value: "2251.000000"
      - 
        key: "Tx speed:"
        value: "320.000000"
  - 
    level: 2
    name: "mavros: GPS"
    message: "No satellites"
    hardware_id: "udp://:14540@192.168.82.128:14558"
    values: 
      - 
        key: "Satellites visible"
        value: "0"
      - 
        key: "Fix type"
        value: "0"
      - 
        key: "EPH (m)"
        value: "99.99"
      - 
        key: "EPV (m)"
        value: "99.99"
  - 
    level: 0
    name: "mavros: Heartbeat"
    message: "Normal"
    hardware_id: "udp://:14540@192.168.82.128:14558"
    values: 
      - 
        key: "Heartbeats since startup"
        value: "514"
      - 
        key: "Frequency (Hz)"
        value: "1.036965"
      - 
        key: "Vehicle type"
        value: "Quadrotor"
      - 
        key: "Autopilot type"
        value: "PX4 Autopilot"
      - 
        key: "Mode"
        value: "MANUAL"
      - 
        key: "System status"
        value: "Standby"
  - 
    level: 2
    name: "mavros: System"
    message: "Sensor health"
    hardware_id: "udp://:14540@192.168.82.128:14558"
    values: 
      - 
        key: "Sensor present"
        value: "0x1020002C"
      - 
        key: "Sensor enabled"
        value: "0x1021002C"
      - 
        key: "Sensor health"
        value: "0x0020000F"
      - 
        key: "3D magnetometer"
        value: "Ok"
      - 
        key: "absolute pressure"
        value: "Ok"
      - 
        key: "GPS"
        value: "Fail"
      - 
        key: "rc receiver"
        value: "Fail"
      - 
        key: "AHRS subsystem health"
        value: "Ok"
      - 
        key: "pre-arm check status. Always healthy when armed"
        value: "Fail"
      - 
        key: "CPU Load (%)"
        value: "0.0"
      - 
        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: 0
    name: "mavros: Battery"
    message: "Normal"
    hardware_id: "udp://:14540@192.168.82.128:14558"
    values: 
      - 
        key: "Voltage"
        value: "12.28"
      - 
        key: "Current"
        value: "0.7"
      - 
        key: "Remaining"
        value: "0.0"
  - 
    level: 0
    name: "mavros: Time Sync"
    message: "Normal"
    hardware_id: "udp://:14540@192.168.82.128:14558"
    values: 
      - 
        key: "Timesyncs since startup"
        value: "5139"
      - 
        key: "Frequency (Hz)"
        value: "9.999317"
      - 
        key: "Last RTT (ms)"
        value: "14.893911"
      - 
        key: "Mean RTT (ms)"
        value: "25.217730"
      - 
        key: "Last remote time (s)"
        value: "1840.252266000"
      - 
        key: "Estimated time offset (s)"
        value: "1657739494.100372314"
---

alfa budiman · 15 Jul 2022, 17:42

@Cliff-Wong For the offboard example, how would it know that it's actually at a height of 2 m and how would it translate to the actual RB5 drone?

    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 example publishes the desired position (height of 2m) through the topic "mavros/setpoint_position/local"

It doesn't appear to subscribe to any topic describing the drone's position.

I am able to arm the drone and start it as well as rostopic echo several topics and they appear to be working as intended.

However, I've tried using rqt to try publishing to several topics such as /mavros/setpoint_attitude/cmd_vel and it appeared to have no effect.

The RB5 drone is armed with the propellers off. Its motors are spinning and when I use rqt to publish /mavros/setpoint_attitude/cmd_vel its motors should change velocity and should sound different. However, when I try this, it appears to have no effect.

Chad Sweet · 2 Aug 2022, 05:06

I don't think we have tried mavros on RB5 Flight SDK. The VOXL SDK is close to being ready for the RB5 Flight drone. That update should enable more functionality as well as be open source. Hopefully we can post to beta in the next few weeks, we are working on the last few issues.

alfa budiman · 2 Aug 2022, 14:20

@Chad-Sweet
Alright, if MAVROS hasn't been tested on the RB5 drone then what is the currently established method of programmatically flying it?