Help with the communication side with APM

I'm trying to develop a modification of this plugin to simulate a Rover and I'm not able to make it work. I'm stuck in the communication side. Here is the repo what I have done so far. I don't fully understand how the communication works. For instance, how does it start? How is the first message sent? I'm able to open both sockets without any issue, but when trying to receive packets from APM I don't receive anything, all the messages are -1 size, instead of 64. Any clue?

Here is an execution example:

$ roslaunch rover_sitl_gazebo_plugin rover_worlds.launch 
... logging to /home/erle/.ros/log/74d04126-e059-11e5-8766-d8cb8aaac711/roslaunch-erle-12777.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
WARNING: disk usage in log directory [/home/erle/.ros/log] is over 1GB.
It's recommended that you use the 'rosclean' command.

started roslaunch server http://erle:37849/

SUMMARY
========

PARAMETERS
 * /rosdistro: indigo
 * /rosversion: 1.11.16
 * /use_sim_time: True

NODES
  /
    gazebo (gazebo_ros/gzserver)
    gazebo_gui (gazebo_ros/gzclient)

auto-starting new master
process[master]: started with pid [12788]
ROS_MASTER_URI=http://localhost:11311

setting /run_id to 74d04126-e059-11e5-8766-d8cb8aaac711
process[rosout-1]: started with pid [12801]
started core service [/rosout]
process[gazebo-2]: started with pid [12808]
process[gazebo_gui-3]: started with pid [12820]
Gazebo multi-robot simulator, version 6.1.0
Copyright (C) 2012-2015 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[ INFO] [1456911052.632931457]: Finished loading Gazebo ROS API Plugin.
[ INFO] [1456911052.633156072]: waitForService: Service [/gazebo/set_physics_properties] has not been advertised, waiting...
[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11345
[Msg] Publicized address: 192.168.1.41
[ INFO] [1456911053.559088656]: New socket created [fd = 29]
[ INFO] [1456911053.559141923]: New socket created [fd = 30]
[ INFO] [1456911053.559238838]: Waiting for rover model to be inserted...
[ INFO] [1456911053.559275189]: Rover model inserted!
[ INFO] [1456911053.559379806]: Model name:      rover
[ INFO] [1456911053.559620937]: front_left joint found
[ INFO] [1456911053.560034097]: SteeringRatio[31.412000] MaxGas[0.200000]

[ INFO] [1456911053.560621200]: Gazebo side initialized
[ INFO] [1456911053.574834505]: ROVER: Services declared !
[ INFO] [1456911053.574867716]: ROVER: ROS side initialized
[ INFO] [1456911053.574900513]: ROVER: Binding to listening port from ArduPilot...

[ INFO] [1456911053.574972140]: ROVER: SUCCESS in binding to port from ArduPilot

[ INFO] [1456911053.575027768]: ROVER: Connecting send port to ArduPilot...

[ INFO] [1456911053.575048909]: ROVER: Opened ArduPilot fdm socket

[ INFO] [1456911053.575081725]: ROVER: APM/Ardupilot side initialized
[ INFO] [1456911053.575092647]: ROVER: Initialization finished. Every side has been initialized.
[ INFO] [1456911053.575341084]: ROVER: Starting listening loop for ArduPilot messages
[ INFO] [1456911053.676006602]: recv = -1
[ INFO] [1456911053.776237050]: recv = -1
[ INFO] [1456911053.876476631]: recv = -1
[ INFO] [1456911053.976730075]: recv = -1
[ INFO] [1456911054.076967662]: recv = -1
[ INFO] [1456911054.177216897]: recv = -1
[ INFO] [1456911054.277373962]: recv = -1
[ INFO] [1456911054.377594135]: recv = -1
[ INFO] [1456911054.477809827]: recv = -1

I was unable to find the direct cause to this communication problem. But here are a few ideas to try to solve it:

Make sure the fdm_packet structure is identical on both sides: https://github.com/AurelienRoy/ardupilot/blob/wall_follow/libraries/SITL/SIM_Gazebo.h And https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/include/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin.h

Make sure the connection adress / ports are identical.

Launch first Ardupilot. It initializes then waits for the RC link connection (mavros). Then launch ROS (the simulation plugin). The plugin main loop will wait until Ardupilot sends a message. Meanwhile mavros starts, connects to Ardupilot, which resumes Ardupilot. Once Ardupilot is connected to RC (mavros), it sends messages to the simulation plugin (the servos / fdm packets).

Add breakpoints or prints in Ardupilot to make sure its execution reaches the file SIM_Gazebo.cpp

Good luck!

I already checked those:

fdm_packet struct is identical in both sides
Connection address / ports are also identical
I launch ardupilot first by going to APMrover2 folder and running ../Tools/autotest/sim_vehicle.sh -j 4 -f Gazebo --map --console

That opens a new console with the following output:

Started model Gazebo at -35.363261,149.165230,584,353 at speed 1.0
Starting sketch 'APMrover2'
Starting SITL input
bind port 5760 for 0
Serial port 0 on TCP port 5760
Waiting for connection ....

Then I launch Gazebo/ROS with this launch file and this world file. Which includes this model file for the rover. Note that the rover model is a sdf file, not an urdf. Is that a problem?

The problem, I guess, might be hidden somewhere in the code I had to add. Because removing the parachute methods as I did shouldn't affect the communication, right?

I might have fixed the communication issue. I'm not completely sure, thought. I changed the rover model format from .sdf to .urdf so I could insert all the plugins needed for the sensors (such as IMU, GPS, etc). By doing that it seems that the communication starts:

Started model Gazebo at -35.363261,149.165230,584,353 at speed 1.0
Starting sketch 'APMrover2'
Starting SITL input
bind port 5760 for 0
Serial port 0 on TCP port 5760
Waiting for connection ....
bind port 5762 for 2
Serial port 2 on TCP port 5762
bind port 5763 for 3
Serial port 3 on TCP port 5763
Opened Gazebo servos send socket
Opened Gazebo fdm receive socket
 servo[4] modified into 1000
 servo[5] modified into 1000
 servo[6] modified into 1000
 servo[7] modified into 1800
 servo[8] modified into 1000
 servo[9] modified into 1000
 servo[10] modified into 1000
 servo[11] modified into 1000
 servo[12] modified into 1000
 servo[13] modified into 1000
------------------------

But Gazebo gets stuck trying to insert the new urdf model, so simulation won't start. I guess this is due to errors in the model. I hope that fixing those the problem gets solved.

AurelienRoy / ardupilot_sitl_gazebo_plugin

Help with the communication side with APM #6