UDP ports and Connection off

[ahcorde]

Hello @MaxxUAV and @ AurelienRoy

I have read your work with Gazebo and APM in DIYdrones and I think it's really promising. I'm trying to test your gazebo plugin on my computer. But I have some troubles.

The first one is the UDP port: this plugin use the ports 5003 and 5002 [1] but the recommended branch or the master branch of DIYDrones use the ports 9002 and 9003 [2] [3] :

Once I solved this issues, I have seen in the comments of the code that the steps of the simulation have to be call by the plugin to ensures that Ardupilot does not miss a single step of the physics solver. The plugins is trying to connect all the time to ArduCopter (it's makes the simulation very slow). The plugin shows this messages:

[ INFO] [1450696099.238722914, 6.190000000]: ARI: Connected with Ardupilot
[ INFO] [1450696099.240133858, 6.192500000]: ARI: Ardupilot connection off, no messages

When MAVProxy.py starts it shows me this message: Waiting for heartbeat from tcp:127.0.0.1:5760. If I wait a long time ArduCopter starts to calibrate the barometer and gyroscopes. But I'm no able to change the mode of the copter. The simulation is too slow.

I have tested the other plugin documented in the Ardupilot Wiki[4] and It works. I'm using Gazebo 6.1. I don't know if I'm missing something.

I hope this is the right place to comment this issues.

[1] https://github.com/alexbuyval/ardupilot/blob/RangeFinderSITL2/libraries/SITL/SIM_Gazebo.cpp#L38 [2] https://github.com/diydrones/ardupilot/blob/master/libraries/SITL/SIM_Gazebo.cpp#L38 [3]https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/include/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin.h#L99 [4]http://dev.ardupilot.com/wiki/using-rosgazebo-simulator-with-sitl/

[AurelienRoy]

Hi Alejandro,

First thank you for trying out the plugin. It will indeed be helpful in making sure it works on everyone's system.

---

About UDP address ports: Original work from Alexander was based on ports 5002/5003 (in file SIM_arducopter_ros_sitl.cpp), and the JSBSim SITL was also using similar ports. So we continued on them. It was later that the Gazebo interface moved to 9002/9003. I just saw CRCSim interface uses 9002/9003, maybe that is why. But your right, we should move to 9002/9003. Thanks for the info, just did the correction commit.

---

We have not yet updated the tutorial, the ardupilot Gazebo plugin requires hector's GPS plugin to run. Do you have it ? Otherwise, from your catkin workspace type in a terminal: git clone https://github.com/tu-darmstadt-ros-pkg/hector_gazebo.git cd hector_gazebo git checkout indigo-devel (I am booted on Windows, so I am unable to test these commands) I just saw we did not list this plugin requirement in the compilation makelist. I will add it.

---

The plugin shows messages "ARI: Connected with Ardupilot" / "ARI: Ardupilot connection off", but actually it does not open/close the connection. It just tells if it has received a message from Ardupilot or not. It checks the socket buffer every 0.5 ms. Does your computer show these messages continuously ? or just once/twice at the initialization ?

---

On the telemetry communication side, the connection is like this: Ardupilot telemetry (5760) <--> (5760) the ROS plugin MAVROS (14550) <--> (14550) MavProxy So actually MavProxy should not be listening on 5760, because Mavros is already hooked up on this port. MavProxy should be listening on messages retransmitted by Mavros, on port 14550. This is parametered in the file : (catkin_ws) /mavros/launch/apm_sitl.launch In case you do not have this file, it was created by Alexander and is downloaded on the tutorial step_5 script: https://github.com/AurelienRoy/ardupilot_sitl_ros_tutorial/blob/master/scripts/step_5_create_ros_workspace.sh Or to retrieve this Mavros version, from the catkin workspace, type in the commands: wstool init src wstool set -t src mavros --git https://github.com/alexbuyval/mavros.git wstool update -t src

---

Let me know if these solve your issues.

Aurelien

[ahcorde]

I didn't have the hector_gazebo plugin but the simulation doesn't break. Now It's included in my workspace. ;). This plugin is going to affect to autonomous modes, right?

---

The plugin shows messages "ARI: Connected with Ardupilot" / "ARI: Ardupilot connection off" continuously.

---

If I wait a long time I'm able to connect with APM to the autopilot through MAVROS. But only for seconds because the simulation is too slow.

[AurelienRoy]

I did not imagine it would compile without hector plugin. Without it there are no GPS topics emitted on ROS, so (maybe) Ardupilot would believe its location to be 0°North 0° East.

---

Do you launch the simulation with sim_vehicle.sh ? With what arguments do you call it ? We are using ROS Indigo, and it came with Gazebo 4.xx included. Which ROS version do you use ?

[ahcorde]

I'm using ROS Indigo and Gazebo 6.1.

To launch ArduCopter:

sim_vehicle.sh -j 4 -f Gazebo

To launch the simulation:

roslaunch ardupilot_sitl_gazebo_plugin  iris_spawn.launch

[ahcorde]

I didn't mention before that I have change this line [1]. The size of the package that I recieved is 16 but the sizeof(servo_packet) is 64.

I try something like this:

if (szRecv != -1)

If I don't change this if I'm not able to start the simulation.

[1] https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/src/apm_plugin_apm_side.cpp#L118

[AurelienRoy]

Thanks Alejandro for these precisions. I did not think Gazebo 6.xx would work with ROS Indigo. It is great news for there are many new features since Gazebo 4.

---

Ok, I think I am now understanding it, you are running the simulation with the current APM master, or your custom version, while along the Gazebo plugin we also modified the Ardupilot code of the gazebo interface. That is why the ports did not match, and message sizes/structures do not match either. Indeed message structures must match on both side, for both struct servo_packet [1A] [1G] and struct fdm_packet [2A] [2G].

You can find the modifications to Ardupilot's SIM_Gazebo.cpp on the branch "sim_ros_gazebo" in: https://github.com/AurelienRoy/ardupilot/tree/sim_ros_gazebo Note: If you are interested, the branch "wall_follow" contains the distance controller to vertical surfaces.

Much of what has been added to SIM_Gazebo.h/.cpp is not essential (safety checks, ROS launch, handling of simulated range finders). But the 3 modifications that make it fail if working with the APM master are:

• ports, like you found out
• servo controls packet, float array extended from 4 to 16 (plane and parachute need more than 4 servos)
• fdm packet size, in which the line double position_latlonalt[3]; is essential because it holds the GPS measure [3A]

---

If you do not wish to use our Ardupilot branch, then you should update your SIM_Gazebo.h with:

• in struct servo_packet, replace float motor_speed[4]; with float motor_speed[16];
• in struct fdm_packet, append at the end:
  • double position_latlonalt[3];
  • double sonar_down;
  • double sonar_front; Even if you do not use the 2 sonar variables, it will make sure message structures are identical on both sides.

And update your SIM_Gazebo.cpp with:

• in function void Gazebo::send_servos(const struct sitl_input &input), add
  • pkt.motor_speed[4] = (input.servos[4]-1000) / 1000.0f; from 4 and up to 15.
• in function void Gazebo::recv_fdm(const struct sitl_input &input), add:
  • // get GPS position [degrees]->[degrees * 10^7], [m]->[cm]
  • location.lat = pkt.position_latlonalt[0] * 1.0e7;
  • location.lng = pkt.position_latlonalt[1] * 1.0e7;
  • location.alt = pkt.position_latlonalt[2] *1.0e2;
  • (handling of sonar fields is not necessary)
• in function void Gazebo::update(const struct sitl_input &input):
  • remove the call to update_position();.

[1A] https://github.com/AurelienRoy/ardupilot/blob/sim_ros_gazebo/libraries/SITL/SIM_Gazebo.h#L57 [1G] https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/include/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin.h#L175

[2A] https://github.com/AurelienRoy/ardupilot/blob/sim_ros_gazebo/libraries/SITL/SIM_Gazebo.h#L65 [2G] https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/include/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin.h#L182

[3A] https://github.com/AurelienRoy/ardupilot/blob/sim_ros_gazebo/libraries/SITL/SIM_Gazebo.h#L72

---

You may run into HOME position problems, because the HOME position sent to Ardupilot is not transmitted to the GPS on Gazebo. To manually solve it, modify the location in the file: https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/urdf/gps_home_location.xacro In our branch we modified _simvehicle.sh so that it transmits the HOME location to Ardupilot and rewrites this GPS home file.

---

When we started out with the simulation, we ran into FPE errors (Floating Point Error - division by 0) in Ardupilot's code, due to the autopilot running multiple loops with the same timestamp. In case you encounter the same problem, add the line: https://github.com/AurelienRoy/ardupilot/blob/sim_ros_gazebo/libraries/SITL/SIM_Gazebo.cpp#L338

---

You seem to have had to go through the code line by line to solve the errors. I apologize for your trouble.

[ahcorde]

Guau! @AurelienRoy,

Many thanks for your last message! The main trouble was the SITL/SIM_Gazebo version.

I have tested the quadcopter and it looks amazing!

Today I'm going to try the plane. Something to have on mind before to simulate the plane?

Let me know if I can help you with the documentation ;)

[ahcorde]

When I try to change the flight mode of the plane a segmentation faults appears. I have changed two lines:

the UAV_MODEL_NAME [1]

#define UAV_MODEL_NAME                 "iris"

to

#define UAV_MODEL_NAME                 "cessna"

and NB_SERVOS_MOTOR_SPEED [2]:

#define NB_SERVOS_MOTOR_SPEED     4

to

#define NB_SERVOS_MOTOR_SPEED     5

[1] https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/include/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin.h#L85 [2] https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/include/ardupilot_sitl_gazebo_plugin/ardupilot_sitl_gazebo_plugin.h#L104

[MaxxUAV]

Hi Alejandro ! My turn to help you. Please do not change the following lines. It's depreceated. You only have to launch sim_vehicle.sh with ArduPlane and empty_world_cessna arguments. I'm not on Linux I will reboot to paste you the good cmds.

[MaxxUAV]

Alright. Try this : ./sim_vehicle.sh -v ArduPlane -f Gazebo -r LFLY_world_cessna -L LFLY --console --map This will compile ArduPlane on LFLY location in LFLY world. Particularity of Arduplane : we override new values to the plugin directly on the world file (LFLY_world_cessna/empty.world). No more need to change the code when switching between Iris and Cessna :

<plugin name="ardupilot_sitl_gazebo_plugin" filename="libardupilot_sitl_gazebo_plugin.so"> <UAV_MODEL>cessna</UAV_MODEL> <NB_SERVOS_MOTOR_SPEED>5</NB_SERVOS_MOTOR_SPEED> </plugin> [1]

[1] https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/worlds/LFLY_world_cessna/empty.world#L87-L90

[ahcorde]

When I change the flight mode in MAVProxy or APM a segmentation fault appears in Gazebo.

ERROR: Floating pointer exception - aborting

Also I'm trying to override the RC in MANUAL mode (like I have done with the copter) but I'm not able to move the ailerons, rudder, etc.

Any ideas?

[MaxxUAV]

It seems you do not have good topics names. First of all there is some ROS prints on Ros launch: ROS_INFO("Model name: %s", _modelName.c_str()); ROS_INFO("Nb motor servos: %d", _nbMotorSpeed); [1] What values do you get on the console ? You should have : cessna 5

The 5 servos motor speed are moving the joints on the gazebo model. If it does not move especially the elevator when the ArduPlane init, there is a topic issue. Try to visualize it with Rqt.

topicNameBuf = std::string("/") + _modelName + "/command/motor_speed"; _motorSpd_publisher = _rosnode->advertise<mav_msgs::CommandMotorSpeed>(topicNameBuf.c_str(), 10); [2] On this case you would have cessna prefixes on topic names.

Keep me in touch. [1] https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/src/apm_plugin_gazebo_side.cpp#L56-L57

[2] https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/src/apm_plugin_ros_side.cpp#L51-L74

[ahcorde]

Yes, I see:

Model name: cessna 
Nb motor servos: 5

when I visualize the topic `/cessna/command/motor_speed:

header: 
  seq: 61035
  stamp: 
    secs: 319
    nsecs: 277500000
  frame_id: quad
motor_speed: [500.0, 500.0, 0.0, 500.0, 500.0]

why appears 'frame_id: quad'?

[MaxxUAV]

frame_id : 'quad' do not interfere, it's only a header I didn't change for cessna... std::string frame_id = "quad"; cmdMotSpd_msg->header.frame_id = frame_id; cmdMotSpd_msg->header.stamp = ros::Time::now();[1]

Here is the param file of my actual config : cessna_default.txt Try to load it and check.

I have few questions :

• do you have a right GPS location on the map corresponding to the LFLY airport ?
• do you have stable IMU values ?
• Can you change modes, etc.
• Does the 3D model move its elevator surface during the Arduplane init ?

Verify if you do not have issues on the launch. Here are my recorded consoles. console mavproxy.txt console_xterm.txt console_xterm2.txt

[1] https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/ardupilot_sitl_gazebo_plugin/src/apm_plugin_ros_side.cpp#L267-L271

[ahcorde]

Thanks for having the time to answers my questions :+1:

I think it is something relative with the parameters. Because with your params I'm able to fly (more o less)

Yes, I have the correct GPS position, change flight modes and good IMU values.

But the elevator surface ( flaps?) are on landing position. How can I change this?

The outputs of my terminals are very similar (or the same).

[MaxxUAV]

Great news ! The plane is more or less flying ?! If you have a joystick please add expos and lower the min and max RC values. This plane is full scale, I didn't have time to change the. dae files like the Aero-M and RC behaviour (which would be awesome for a real UAV simulation experience).

As I suspected I think it's bad RC values that crashed the model, maybe something to strengthen. Flap value is set to the motor_speed 5 as output. See : static const unsigned int Flap = 4;

On Mission planner change the value on the dedicated tab. Also, with my params you will have autoflaps on takeoff and landings.

[1] https://github.com/AurelienRoy/ardupilot_sitl_gazebo_plugin/blob/master/aircraft_plugin/include/aircraft_plugin/aircraft_plugin.h#L93

[ahcorde]

I have done two different test:

• Try to fly in AUTO mode (with your parameters), but the plane done very strange behaviour. In this case I think the RC is not going to affect the flight, right?. The roll of the plane change more than 90º and the model crash. This is strange because in the AUTO parameters (BANK_MAX is 45º), also with the throttle min if it's 0, the model crash I need to put this value to 45 o 50. Finally in your parameters FLAPERON_OUTPUT = 0 This value disable the flaps, right?
• Overriding the RC. I don't know why but I'm not able to move the plane using my PS3 joystick. I have changed SYSID_MYGCS to 252, because I'm using APM planner (This process works with the copter)

[MaxxUAV]

Hi Alejandro. Did you solve your issues ? It's weird because angle of the plane is controlled on Auto mode so the plane would not roll above the limit angle. Maybe the WP_radius is the problem, if it's too low it will try to turn very hard so roll:90 and pitch:Max. Compare your settings with mine and test it with Mission planner if you have a windows laptop near you. Mission Planner is compatible with any joystick si it would works well... My RC are like MIN:1300 MAX:1700. It would be not recommended to assign a lower or higher value to the model and may crash the system. (To validate) It did not try it without joystick, instead of multicopter this plane is very fast and Full Scale.

If you are still stuck I will investigate by tests with my plane. Sorry you have so much issues. Please keep me in touch.