I successfully connected copter to Gazebo using the ardupilot_gazebo_plugin. I managed to get it to fly, but I am having two issues. I am trying to integrate a lidar on the iris model. I have the SF40c, but anything that approximates that is ok. Basically I want DISTANCE_SENSOR mavlink messages from gazebo based on the environment. Without any modification, I get one sensor message pointed at a pitch of 270. I assume this is the IMU reporting altitude? I only see an IMU and an odometry_sensor.
I managed to link a hokuyo lidar using a modified iris sdf file, however, it doesn’t report to ardupilot with a DISTANCE_SENSOR message. It also seems to make the iris amazingly unstable and causes a crash. I tried lowering the mass to 0.000001, but it still does this. Looking at the rendered rays, it doesn’t look like it is quite fixed to the body as it seems to lag behind the body, causing an oscillation.
Anyway, there is an ardpilot issue here and perhaps a gazebo issue. This being an ardupilot forum, I’m mainly wondering what I need to do to get that DISTANCE_SENSOR message over to my ardupilot code.
concerning the instability with your hokuyo lidar sensor, I had the same problem than you. In the model of the sensor, try to set the inertia to really low value. It fixed this problem for me.
<inertial>
<mass>0.016</mass>
<inertia>
<ixx>0.0001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0001</iyy>
<iyz>0</iyz>
<izz>0.0001</izz>
<!-- low intertia necessary to avoid to disturb the drone -->
</inertia>
</inertial>
About the communication, I guess we followed the same tutorial. I succeed to add the sensor to my model like you did, and to add the sensor range in the packet which is send back to ArduPilot (mostly by un-commenting some line in ArduPilotPlugin.cc), but I have no clue how to read the value in SITL (they should read other value but I don’t get how it is interacting and where I have to go to read my sensor range…). Did you succeed to send the DISTANCE_SENSOR message to your ardupilot code ?
I went down several paths trying to get gazebo send a DISTANCE_SENSOR message. My latest attempt is to get a ROS sensor up and convert it through mavros back to ardupilot. I haven’t to this point used ROS, so I followed the Erle Robotics tutorial on setting up Gazebo with ROS and their plugins. ErleRobotics has a flight controller that natively supports ROS and runs APM. I got their environment up and running with the ErleCopter and lidar light. I have the ROS topic for the lidar publishing, and mavros enabled and the plugin configured to subscribe to ROS rangefinder messages and publish a distance sensor… it isn’t coming in, however, nor is it showing up on wireshark. One big negative about the Erle Robotics stuff is they seem to clone everything, modify it and not keep current with the latest elsewhere. Ardupilot is on 3.4, which I think distance sensor messages started being supported in 3.5… It would be nice if I could get it working natively with the current plugin and gazebo rather than having to use a big custom environment that is older (I had to downgrade to Ubuntu Trusty and make an insanely fat Docker image to get this working with tons of clones/compiles of their branches.)
Looking at the rangefinder section, it only supports one sensor, which I need 8 in different orientations to represent the lightware. So, it is either heavily modify that plugin with custom stuff to get it to work or get ROS to send the mavlink message I need and deal with all of the custom branches that are out of date.
No, I haven’t seen how they parse that message on the SITL side, seems they must ingest that custom message type and output mavlink at some point… hopefully.
Sorry for the long response time… I haven’t use gazebo plugin since long time.
The current version doesn’t have support for rangefinder nor 360 lidar. The simplier for now will be to use MAVROS to send the data to SITL.
Wow, that document is very useful. I figured out most of it by scanning the code, except for the ID needing to match the ardupilot side. I’m not sure what ID that is referring to. I just set the id to 0 since it is currently the only sensor I have on the model. Do I need to register/create a lidar in SITL somewhere?
Thanks for the response, that is kindof what I’m attempting now. I am trying to setup gazebo7 with the packaged version of mavros on kinetic, xenial, so I can use arducopter 3.5. My current code base is against dronekit, so I need the plugin to connect to SITL and simulate in gazebo, so I plan on making my own catkin environment borrowing a bunch from elsewhere to create the quad and sensors in URDF. (I have an SDF model of an iris and lidar, but that is in SDF, and I am under the impression that I will need to go URDF for the ROS route.)
In summary, I plan on using ROS to simulate the lidar, which should integrate with gazebo_ros to interact with the simulation and mavros to send the distance_sensor over to the FCU, which will forward it to my code. The plugin will be used to connect the SITL to gazebo and control flight, etc.
I agree with about ErleRobotics, it is outdated… The tutorial is well done, but downgrading to a lower version of ArduPilot and Gazebo to make it works doesn’t make sense for me. ROS is also a really powerful tool, however it requires a lot of knowledge to make it works properly, which I do not have yet. I’ll be glad to get some news from you if you succeed in the settings of your sensor.
Using the plugin of SwiftGust and without working with ROS, I succeed to get the data of the sensor back to SITL for simulation. I created a new branch of the plugin to use a sensor (mainly uncomment lines) and add a new sensor in the model file. On the ArduPilot side I modified the code based on Aurelien Roy branch, but I think the version of ardupilot should works if it’s modified correctly. To understand what’s happening, have a look to these files:
libraries/SITL/SITL h and cpp: contains the struct sitl_fdm which contains all the sensor receive from the simulation (a “range” is already implemented)
libraries/SITL/SIM_Gazebo h and cpp: communication with gazebo plugin. The struct fdm_packet in header file have to match the packet of the plugin on the gazebo side ! You can read your value in the function recv_fdm
libraries/SITL/SIM_Aircraft h and cpp: parent of SIM_Gazebo. Contains the fonction fill_fdm which is call by libraries/AP_HAL_SITL/SITL_State.cpp which will send the data to the appropriate pin number (with sitl_rangefinder.cpp in my case)
Really brief explanation, but it can help if you have to understand the ArduPilot side. I mainly understand the code by checking what has been edited in the branch of Aurelien. You may also have a look as it seems that they implemented a ROS communication.
Update: I updated to copter 3.5 with the original Aurelien plugin. I managed to get an SDF model with a modified hokuyo lidar to output on ros and show up in gazebo. Still having issues getting mavros to send the message. (Not showing up in wireshark and my application isn’t getting it)
It is connecting over tcp to the FCU and UDP to the GCS. I’m not seeing any mavlink traffic to the FCU, but the GCS has quite a bit. Looking at the distance_sensor mavros plugin code, it is sending it to the FCU…
Edit: Did some logging and it looks like it is never getting the range_cb, even though I can echo the topic and the type of message is correct (and I verified the subscribed topic name.)
This is partially solved. I re-read the document khancyr provided and had missed that I needed to publish on /mavros/distance_sensor/range rather than /range. I am now getting the subscribed message, but it isn’t showing up over in my application.
Finally got a working solution. Ended up moving away from using DISTANCE_SENSOR messages due to the limitation of the number of sensors and their directions built into ArduCopter.
I used the ROS laser plugin to generate a LaserScan message and created a ROS emulator for the Lightware where I implemented some of its feature set via services. This emulator takes the raw feed from the gazebo plugin and does post processing to emulate things like search light, etc. I then just consumed the data natively in my companion app rather than funneling it through the flight controller.