Sending arm, takeoff, or changing mode commands of vehicle for ArduCopter SITL in ROS/Gazebo

[sahiljuneja]

I am referring to http://dev.ardupilot.com/wiki/using-rosgazebo-simulator-with-sitl/

Based on the code, the following is my understanding of what's happening -

• You receive input to ROS (mavros) via UDP socket as motor_speeds. These inputs are received from the SITL and ArduCopter code.
• You publish these motor speeds to the corresponding topic for Gazebo simulation.
• Data from the simulated vehicle is then send back to SITL code, over the UDP socket.

However, I still don't know how I can use Mavros alongside with SITL to be able to send commands to arm, disarm, takeoff, or change the mode over the socket.

Directly trying to call the /cmd/arming service, won't help I believe. I am looking at alternatives or any better way for this.

(Please let me know if any clarification is required)

[vooon]

1. Just do step-by-step that instruction. You should get rotors_simulator, gazebo and mavros running.
2. Mavros not involved in simulation data transfers for APM, they uses special protocol «FDM», so arducopter_sitl_ros provide node for that.
3. Starting sim_vehicle.sh you should get preconfigured mavros too.

After you got running mavros, check /mavros/state topic. You should be connected.
Then use mavsafety, mavsys, mavparam, mavwp and other scripts.

[sahiljuneja]

@vooon Thanks for your reply. I already followed the instructions. And I already tried calling on the /mavros/cmd/arming service and it did nothing. This was after I had the sim_vehicle.sh running ofcourse.

I still don't know whether I need to pass the arming command over the socket (utilizing fdm udp) or just call that service (the latter would make more sense). I am calling the service which, like I said, does nothing. The reason I think sending any such command over the socket won't work because as per SITL code from ardupilot, and the ardupilot_sitl_ros code, only a specific set of data is packed into a struct and sent over the socket. So an "arming command" can't manage that.

The mavsafety and other scripts you mention also rely on the same services for arming etc., so I don't know why directly trying to call the service from the arducopter_sitl_ros doesn't do anything. I am not, however, relying on teleop for this, I am trying to have the vehicle arm and disarm on it's own after the simulation starts.

If you could point to a better explanation of how to achieve this goal I would appreciate that a lot. Otherwise I will look into your suggestion for the scripts and try again. Thanks again!

[vooon]

What diagnostics says?

rostopic echo -n1 /diagnostics

[sahiljuneja]

header: 
  seq: 34
  stamp: 
    secs: 29
    nsecs: 0
  frame_id: ''
status: 
  - 
    level: 0
    name: mavros: FCU connection
    message: connected
    hardware_id: tcp://127.0.0.1:5760
    values: 
      - 
        key: Received packets:
        value: 22
      - 
        key: Dropped packets:
        value: 0
      - 
        key: Buffer overruns:
        value: 0
      - 
        key: Parse errors:
        value: 0
      - 
        key: Rx sequence number:
        value: 21
      - 
        key: Tx sequence number:
        value: 30
      - 
        key: Rx total bytes:
        value: 766
      - 
        key: Tx total bytes:
        value: 558
      - 
        key: Rx speed:
        value: 17.000000
      - 
        key: Tx speed:
        value: 0.000000
  - 
    level: 0
    name: mavros: GCS bridge
    message: connected
    hardware_id: tcp://127.0.0.1:5760
    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: 623
      - 
        key: Rx speed:
        value: 0.000000
      - 
        key: Tx speed:
        value: 17.000000
  - 
    level: 2
    name: mavros: GPS
    message: No satellites
    hardware_id: tcp://127.0.0.1:5760
    values: 
      - 
        key: Satellites visible
        value: 0
      - 
        key: Fix type
        value: 0
      - 
        key: EPH (m)
        value: Unknown
      - 
        key: EPV (m)
        value: Unknown
  - 
    level: 0
    name: mavros: Heartbeat
    message: Normal
    hardware_id: tcp://127.0.0.1:5760
    values: 
      - 
        key: Heartbeats since startup
        value: 14
      - 
        key: Frequency (Hz)
        value: 0.900000
      - 
        key: Vehicle type
        value: Quadrotor
      - 
        key: Autopilot type
        value: ArduPilotMega
      - 
        key: Mode
        value: STABILIZE
      - 
        key: System status
        value: Active
  - 
    level: 0
    name: mavros: System
    message: Normal
    hardware_id: tcp://127.0.0.1:5760
    values: 
      - 
        key: Sensor present
        value: 0x00000000
      - 
        key: Sensor enabled
        value: 0x00000000
      - 
        key: Sensor helth
        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: tcp://127.0.0.1:5760
    values: 
      - 
        key: Voltage
        value: -1.00
      - 
        key: Current
        value: 0.0
      - 
        key: Remaining
        value: 0.0

Also, when I call the service for enabling guided, I get -

success: False
result: 3

[vooon]

Looks like connection exists, but lacks most of messages. Seems only HEARTBEAT.

I'm not tried sim_vehicle.sh script with ros sitl. Does it show log output? If not then use rqt console to see what FCU reports after arming reqest.

I do not see any obvious problem on mavros side, please ask @alexbuyval (author of this work) for support.

[alexbuyval]

Hi @sahiljuneja
I am using 'cmd/arming' service to arm the simulated copter. You can use the following line in your ROS python node to arm:

arming_cl = rospy.ServiceProxy(args.mavros_ns + "/cmd/arming", CommandBool)

Also try to launch the APM planner at the same time with the simulation. Maybe there are some problem with arming.

[vooon]

@alexbuyval note:

import mavros
from mavros import command

mavros.set_namespace()
command.arming(True)

[sahiljuneja]

Thank you for your replies @vooon and @alexbuyval. Sorry for the delayed reply.

args.mavros_ns raises the error "name 'args' is not defined"

whereas,

@vooon doesn't mavros.set_namespace() require an input argument? For example

mavros.set_namespace(args.mavros_ns)

I guess it would throw me the same error.

I am looking into the reason for this error myself at the moment and trying to fix it. If you guys have any feedback I would appreciate that! Thanks again!

[vooon]

@sahiljuneja Did you added argument to argparse module? Nothing strange that you do not have variable if you do not define it. You may use any scripts as example (that main purpose to write it...).

If you look at source: mavros/init.py

def set_namespace(ns=DEFAULT_NAMESPACE):
    """
    Sets namespace of mavros node
    """

So argument is optional.

[sahiljuneja]

That's not how set_namespace is defined for me. I'll update mavros first and update asap. Thanks for pointing that out.

Alex's mavros repo is quite behind the original mavros, so need to handle that appropriately.