Distance sensor

[xabierolaz]

New distance sensor based on original issue #79
Distance Sensor dependencies PR wil3/gymfc-aircraft-plugins#5

This allows to use a lidar with NF1 model in order to get distance to the ground and allow navigation support for features such as takeoff

Files change/modifications:

• Distance.proto
• fc_env.py
• FlightControllerPlugin.cpp
• FlightControllerPlugin.hh
• Model.sdf (fc_stack_1 refers to the actual lidar in the sdf)

Right now

BUILD TEST
./build_plugin.sh output (successful)

(env) cuda@cuda:~/workspace/gymfc/gymfc/envs/assets/gazebo/plugins$ ./build_plugin.sh 
-- The C compiler identification is GNU 7.3.0
-- The CXX compiler identification is GNU 7.3.0
-- Check for working C compiler: /usr/bin/cc
-- Check for working C compiler: /usr/bin/cc -- works
-- Detecting C compiler ABI info
-- Detecting C compiler ABI info - done
-- Detecting C compile features
-- Detecting C compile features - done
-- Check for working CXX compiler: /usr/bin/c++
-- Check for working CXX compiler: /usr/bin/c++ -- works
-- Detecting CXX compiler ABI info
-- Detecting CXX compiler ABI info - done
-- Detecting CXX compile features
-- Detecting CXX compile features - done
-- Found PkgConfig: /usr/bin/pkg-config (found version "0.29.1") 
-- Checking for module 'bullet>=2.82'
--   Found bullet, version 2.87
-- Found Simbody: /usr/include/simbody  
-- Found CCD: /usr/include (found version "2.0") 
-- Found FCL: /usr/include (found version "0.5.0") 
-- Found ASSIMP: /usr/include (found version "4.1.0") 
-- Found DART: /usr/local/include (Required is at least version "6.6") found components:  dart 
-- Looking for pthread.h
-- Looking for pthread.h - found
-- Looking for pthread_create
-- Looking for pthread_create - not found
-- Looking for pthread_create in pthreads
-- Looking for pthread_create in pthreads - not found
-- Looking for pthread_create in pthread
-- Looking for pthread_create in pthread - found
-- Found Threads: TRUE  
-- Boost version: 1.65.1
-- Found the following Boost libraries:
--   thread
--   system
--   filesystem
--   program_options
--   regex
--   iostreams
--   date_time
--   chrono
--   atomic
-- Found Protobuf: /usr/lib/x86_64-linux-gnu/libprotobuf.so;-lpthread (found version "3.0.0") 
-- Boost version: 1.65.1
-- Looking for OGRE...
-- OGRE_PREFIX_WATCH changed.
-- Checking for module 'OGRE'
--   Found OGRE, version 1.9.0
-- Found Ogre Ghadamon (1.9.0)
-- Found OGRE: optimized;/usr/lib/x86_64-linux-gnu/libOgreMain.so;debug;/usr/lib/x86_64-linux-gnu/libOgreMain.so
-- Looking for OGRE_Paging...
-- Found OGRE_Paging: optimized;/usr/lib/x86_64-linux-gnu/libOgrePaging.so;debug;/usr/lib/x86_64-linux-gnu/libOgrePaging.so
-- Looking for OGRE_Terrain...
-- Found OGRE_Terrain: optimized;/usr/lib/x86_64-linux-gnu/libOgreTerrain.so;debug;/usr/lib/x86_64-linux-gnu/libOgreTerrain.so
-- Looking for OGRE_Property...
-- Found OGRE_Property: optimized;/usr/lib/x86_64-linux-gnu/libOgreProperty.so;debug;/usr/lib/x86_64-linux-gnu/libOgreProperty.so
-- Looking for OGRE_RTShaderSystem...
-- Found OGRE_RTShaderSystem: optimized;/usr/lib/x86_64-linux-gnu/libOgreRTShaderSystem.so;debug;/usr/lib/x86_64-linux-gnu/libOgreRTShaderSystem.so
-- Looking for OGRE_Volume...
-- Found OGRE_Volume: optimized;/usr/lib/x86_64-linux-gnu/libOgreVolume.so;debug;/usr/lib/x86_64-linux-gnu/libOgreVolume.so
-- Looking for OGRE_Overlay...
-- Found OGRE_Overlay: optimized;/usr/lib/x86_64-linux-gnu/libOgreOverlay.so;debug;/usr/lib/x86_64-linux-gnu/libOgreOverlay.so
-- Found Protobuf: /usr/lib/x86_64-linux-gnu/libprotobuf.so;-lpthread;-lpthread (found suitable version "3.0.0", minimum required is "2.3.0") 
-- Config-file not installed for ZeroMQ -- checking for pkg-config
-- Checking for module 'libzmq >= 4'
--   Found libzmq , version 4.2.5
-- Found ZeroMQ: TRUE (Required is at least version "4") 
-- Checking for module 'uuid'
--   Found uuid, version 2.31.1
-- Found UUID: TRUE  
-- Checking for module 'tinyxml2'
--   Found tinyxml2, version 6.0.0
-- Looking for dlfcn.h - found
-- Looking for libdl - found
-- Found DL: TRUE  
-- FreeImage.pc not found, we will search for FreeImage_INCLUDE_DIRS and FreeImage_LIBRARIES
-- Checking for module 'gts'
--   Found gts, version 0.7.6
-- Found GTS: TRUE  
-- Checking for module 'libswscale'
--   Found libswscale, version 4.8.100
-- Found SWSCALE: TRUE  
-- Checking for module 'libavdevice >= 56.4.100'
--   Found libavdevice , version 57.10.100
-- Found AVDEVICE: TRUE (Required is at least version "56.4.100") 
-- Checking for module 'libavformat'
--   Found libavformat, version 57.83.100
-- Found AVFORMAT: TRUE  
-- Checking for module 'libavcodec'
--   Found libavcodec, version 57.107.100
-- Found AVCODEC: TRUE  
-- Checking for module 'libavutil'
--   Found libavutil, version 55.78.100
-- Found AVUTIL: TRUE  
-- Found CURL: /usr/lib/x86_64-linux-gnu/libcurl.so (found version "7.58.0") 
-- Checking for module 'jsoncpp'
--   Found jsoncpp, version 1.7.4
-- Found JSONCPP: TRUE  
-- Checking for module 'yaml-0.1'
--   Found yaml-0.1, version 0.1.7
-- Found YAML: TRUE  
-- Checking for module 'libzip'
--   Found libzip, version 1.1.2
-- Found ZIP: TRUE  
-- Found Protobuf: /usr/lib/x86_64-linux-gnu/libprotobuf.so;-lpthread;-lpthread;-lpthread (found version "3.0.0") 
-- Checking for module 'protobuf'
--   Found protobuf, version 3.0.0
Gazebo version: 10.1
-- Configuring done
-- Generating done
-- Build files have been written to: /home/cuda/workspace/gymfc/gymfc/envs/assets/gazebo/plugins/build
[  4%] Running C++ protocol buffer compiler on /usr/local/include/gazebo-10/gazebo/msgs/proto/quaternion.proto
[  8%] Running C++ protocol buffer compiler on msgs/Float.proto
[ 12%] Running C++ protocol buffer compiler on msgs/Imu.proto
[ 16%] Running C++ protocol buffer compiler on msgs/EscSensor.proto
[ 20%] Running C++ protocol buffer compiler on msgs/State.proto
[ 25%] Running C++ protocol buffer compiler on msgs/Action.proto
[ 29%] Running C++ protocol buffer compiler on msgs/Distance.proto
[ 33%] Running C++ protocol buffer compiler on /usr/local/include/gazebo-10/gazebo/msgs/proto/vector3d.proto
Scanning dependencies of target sensor_msgs
[ 37%] Building CXX object CMakeFiles/sensor_msgs.dir/Float.pb.cc.o
[ 41%] Building CXX object CMakeFiles/sensor_msgs.dir/Imu.pb.cc.o
[ 45%] Building CXX object CMakeFiles/sensor_msgs.dir/EscSensor.pb.cc.o
[ 50%] Building CXX object CMakeFiles/sensor_msgs.dir/State.pb.cc.o
[ 54%] Building CXX object CMakeFiles/sensor_msgs.dir/Action.pb.cc.o
[ 58%] Building CXX object CMakeFiles/sensor_msgs.dir/Distance.pb.cc.o
[ 62%] Building CXX object CMakeFiles/sensor_msgs.dir/vector3d.pb.cc.o
[ 66%] Building CXX object CMakeFiles/sensor_msgs.dir/quaternion.pb.cc.o
[ 70%] Linking CXX shared library libsensor_msgs.so
[ 70%] Built target sensor_msgs
[ 75%] Running C++ protocol buffer compiler on msgs/MotorCommand.proto
Scanning dependencies of target control_msgs
[ 79%] Building CXX object CMakeFiles/control_msgs.dir/MotorCommand.pb.cc.o
[ 83%] Linking CXX shared library libcontrol_msgs.so
[ 83%] Built target control_msgs
Scanning dependencies of target FlightControllerPlugin
[ 87%] Building CXX object CMakeFiles/FlightControllerPlugin.dir/FlightControllerPlugin.cpp.o
[ 91%] Linking CXX shared library libFlightControllerPlugin.so
[ 91%] Built target FlightControllerPlugin
Scanning dependencies of target AircraftConfigPlugin
[ 95%] Building CXX object CMakeFiles/AircraftConfigPlugin.dir/AircraftConfigPlugin.cpp.o
[100%] Linking CXX shared library libAircraftConfigPlugin.so
[100%] Built target AircraftConfigPlugin

TEST_AXIS.PY

(env) cuda@cuda:~/workspace/gymfc$ python3 tests/test_axis.py examples/gymfc_nf/twins/nf1/model.sdf --verbose
Sending motor control signals to port  9279
Gazebo Model Path = /usr/local/share/gazebo-10/models::/home/cuda/workspace/gymfc/gymfc/envs/assets/gazebo/models:/home/cuda/workspace/gymfc/examples/gymfc_nf/twins
Gazebo Plugin Path = /usr/local/lib/gazebo-10/plugins::/home/cuda/workspace/gymfc/gymfc/envs/assets/gazebo/plugins/build:/home/cuda/workspace/gymfc/examples/gymfc_nf/twins/nf1/plugins/build
Starting gzserver with process ID= 10659
Roll Left
-----------------------------------
Gazebo multi-robot simulator, version 10.1.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org

[Msg] Waiting for master.
[Msg] Connected to gazebo master @ http://127.0.0.1:11375
[Msg] Publicized address: 192.168.18.58
[Wrn] [DARTPhysics.cc:96] Gravity vector is (0, 0, 0). Objects will float.
[Dbg] [DARTModel.cc:72] Initializing DART model attitude_control_training_rig
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'pivot' and joint 'base_joint'.
[Err] [DARTJoint.cc:195] DARTJoint: SetAnchor is not implemented.
[Dbg] [FlightControllerPlugin.cpp:229] Binding on port 9279
[Dbg] [FlightControllerPlugin.cpp:430] CoT link=battery
[Dbg] [FlightControllerPlugin.cpp:432] Got COT from plugin 0 0 0.058
[Dbg] [FlightControllerPlugin.cpp:435] Num motors 4
[Dbg] [FlightControllerPlugin.cpp:469] Inserting digital twin from SDF, examples/gymfc_nf/twins/nf1/model.sdf.
[Dbg] [DARTModel.cc:72] Initializing DART model nf1
[Dbg] [DARTModel.cc:128] Building DART BodyNode for link 'frame' with a free joint.
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'battery' and joint 'battery_joint'.
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'motor_1' and joint 'motor_1_joint'.
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'motor_2' and joint 'motor_2_joint'.
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'motor_3' and joint 'motor_3_joint'.
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'motor_4' and joint 'motor_4_joint'.
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'prop_1' and joint 'prop_1_joint'.
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'prop_2' and joint 'prop_2_joint'.
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'prop_3' and joint 'prop_3_joint'.
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'prop_4' and joint 'prop_4_joint'.
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'fc_stack' and joint 'fc_stack_joint'.
[Dbg] [DARTModel.cc:166] Building DART BodyNode for link 'fc_stack_1' and joint 'fc_stack_1_joint'.
[Dbg] [gazebo_motor_model.cpp:204] Loading Motor number=0 Subscribed to /aircraft/command/motor
[Dbg] [gazebo_motor_model.cpp:204] Loading Motor number=1 Subscribed to /aircraft/command/motor
[Dbg] [gazebo_motor_model.cpp:204] Loading Motor number=2 Subscribed to /aircraft/command/motor
[Dbg] [gazebo_motor_model.cpp:204] Loading Motor number=3 Subscribed to /aircraft/command/motor
[Dbg] [gazebo_imu_plugin.cpp:55] Loading IMU sensor
[Dbg] [gazebo_distance_plugin.cpp:33] Loading DISTANCE  sensor
[Dbg] [FlightControllerPlugin.cpp:553] Setting link to center battery
[Dbg] [FlightControllerPlugin.cpp:568] Aircraft model fixed to world
[Dbg] [DARTLink.cc:482] DARTLink::SetAngularVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:411] DARTLink::SetLinearVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:482] DARTLink::SetAngularVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:411] DARTLink::SetLinearVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:482] DARTLink::SetAngularVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:411] DARTLink::SetLinearVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:482] DARTLink::SetAngularVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:411] DARTLink::SetLinearVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:482] DARTLink::SetAngularVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:411] DARTLink::SetLinearVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:482] DARTLink::SetAngularVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:411] DARTLink::SetLinearVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [gazebo_motor_model.cpp:263]  Motor 0 force=0 vel=0
[Dbg] [gazebo_motor_model.cpp:263]  Motor 1 force=0 vel=0
[Dbg] [gazebo_motor_model.cpp:263]  Motor 2 force=0 vel=0
[Dbg] [gazebo_motor_model.cpp:263]  Motor 3 force=0 vel=0
[Dbg] [DARTLink.cc:482] DARTLink::SetAngularVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:411] DARTLink::SetLinearVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:482] DARTLink::SetAngularVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:411] DARTLink::SetLinearVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:482] DARTLink::SetAngularVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:411] DARTLink::SetLinearVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:482] DARTLink::SetAngularVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [DARTLink.cc:411] DARTLink::SetLinearVel() doesn't make sense if the parent joint is not free joint (6-dof).
[Dbg] [gazebo_motor_model.cpp:263]  Motor 0 force=0 vel=0
[Dbg] [gazebo_motor_model.cpp:263]  Motor 1 force=0 vel=0
[Dbg] [gazebo_motor_model.cpp:263]  Motor 2 force=0 vel=0
[Dbg] [gazebo_motor_model.cpp:263]  Motor 3 force=0 vel=0

Timeout communicating with flight control plugin.

Simulation Stats
-----------------
steps                  1
packets_dropped        0
time_start_seconds     1612809296.6612885
time_lapse_hours       0.03391727805137634


Killing Gazebo process with ID= 10659
Killing Gazebo process with ID= 10660
Timeout communicating with flight control plugin.

[xabierolaz]

@wil3 this is the second part of the task, builds succesfully but somewhat does time out when test_axis, any guess?
Have been following this tutorial for building the lidar into the NF1 sdf http://gazebosim.org/tutorials?cat=guided_i&tut=guided_i1

[allcontributors]

@xabierolaz

I couldn't determine any contributions to add, did you specify any contributions?
Please make sure to use valid contribution names.

[xabierolaz]

Hi @wil3 , I will clear the code as you suggested, and there are more changes that are needed . I will make those and make another PR or re-open this one whenever everything is ready and tested. Sorry for the inconvenience