Distance Sensor Plugin prerequisites

[xabierolaz]

Is your feature request related to a problem? Please describe.
No, brand new feature (Distance Sensor Plugin

Describe the solution you'd like
Right now GymFC's model needs a distance sensor plugin to measure distance to the ground and enable future navigation support

Describe alternatives you've considered
This is the first step for having the full Navigation Feature ( wil3/gymfc#79) Working

Additional context
The forked repository link https://github.com/xabierolaz/gymfc-aircraft-plugins

Updated/Created files:

Cmakelists (updated)
Distance.proto (created)
gazebo_distance_plugin.cpp (created)
gazebo_distance_plugin.h (created)

Cmake test results

Following Readme, right now we got these errors in the output when make
cuda@cuda:~/workspace/aircraft-plugins-new/build$ make

[ 62%] Built target sensor_msgs
[ 75%] Built target control_msgs
[ 83%] Built target gazebo_motor_model
[ 91%] Built target gazebo_imu_plugin
[ 95%] Building CXX object CMakeFiles/gazebo_distance_plugin.dir/src/gazebo_distance_plugin.cpp.o
In file included from /home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:21:0:
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.h: In constructor ‘gazebo::GazeboDistancePlugin::GazeboDistancePlugin()’:
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.h:63:58: error: no matching function for call to ‘ignition::math::v4::Vector3<double>::Vector3(int)’
       ground_distance_units_(ground_distance_units::METER){
                                                          ^
In file included from /usr/include/ignition/math4/ignition/math/Spline.hh:23:0,
                 from /usr/local/include/gazebo-10/gazebo/common/Animation.hh:22,
                 from /usr/local/include/gazebo-10/gazebo/common/common.hh:5,
                 from /home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.h:23,
                 from /home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:21:
/usr/include/ignition/math4/ignition/math/Vector3.hh:77:15: note: candidate: ignition::math::v4::Vector3<T>::Vector3(const ignition::math::v4::Vector3<T>&) [with T = double]
       public: Vector3(const Vector3<T> &_v)
               ^~~~~~~
/usr/include/ignition/math4/ignition/math/Vector3.hh:77:15: note:   no known conversion for argument 1 from ‘int’ to ‘const ignition::math::v4::Vector3<double>&’
/usr/include/ignition/math4/ignition/math/Vector3.hh:68:15: note: candidate: ignition::math::v4::Vector3<T>::Vector3(const T&, const T&, const T&) [with T = double]
       public: Vector3(const T &_x, const T &_y, const T &_z)
               ^~~~~~~
/usr/include/ignition/math4/ignition/math/Vector3.hh:68:15: note:   candidate expects 3 arguments, 1 provided
/usr/include/ignition/math4/ignition/math/Vector3.hh:57:15: note: candidate: ignition::math::v4::Vector3<T>::Vector3() [with T = double]
       public: Vector3()
               ^~~~~~~
/usr/include/ignition/math4/ignition/math/Vector3.hh:57:15: note:   candidate expects 0 arguments, 1 provided
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp: In member function ‘void gazebo::GazeboDistancePlugin::OnUpdate(const gazebo::common::UpdateInfo&)’:
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:145:27: error: missing template arguments before ‘position’
   ignition::math::Vector3 position = T_W_I.Pos();
                           ^~~~~~~~
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:3: error: ‘height’ was not declared in this scope
   height = set_h(position.Z())
   ^~~~~~
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:3: note: suggested alternative: ‘ceilh’
   height = set_h(position.Z())
   ^~~~~~
   ceilh
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:18: error: ‘position’ was not declared in this scope
   height = set_h(position.Z())
                  ^~~~~~~~
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:18: note: suggested alternative: ‘copyPosition’
   height = set_h(position.Z())
                  ^~~~~~~~
                  copyPosition
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:12: error: ‘set_h’ was not declared in this scope
   height = set_h(position.Z())
            ^~~~~
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:12: note: suggested alternative: ‘seq_’
   height = set_h(position.Z())
            ^~~~~
            seq_
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:160:21: error: ‘class sensor_msgs::msgs::Distance’ has no member named ‘set_seq’
   distance_message_.set_seq(seq_++);
                     ^~~~~~~
CMakeFiles/gazebo_distance_plugin.dir/build.make:62: recipe for target 'CMakeFiles/gazebo_distance_plugin.dir/src/gazebo_distance_plugin.cpp.o' failed
make[2]: *** [CMakeFiles/gazebo_distance_plugin.dir/src/gazebo_distance_plugin.cpp.o] Error 1
CMakeFiles/Makefile2:218: recipe for target 'CMakeFiles/gazebo_distance_plugin.dir/all' failed
make[1]: *** [CMakeFiles/gazebo_distance_plugin.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2

[xabierolaz]

Hi @wil3 .To sum up, distance sensor plugin files have been created following imu as example as you suggested.

You can try to reproduce make result by using the fork https://github.com/xabierolaz/gymfc-aircraft-plugins
errors are mostly now about position not being declared.

I looked at your original imu plugin https://github.com/wil3/gymfc-aircraft-plugins/blob/master/src/gazebo_imu_plugin.cpp#L275
There is a comment saying
//TODO(burrimi): Check tf.
what does that Mean? I think I might need to use that for the position
Thanks,
Xabier

[wil3]

Can you build gymfc-aircraft-plugins on master or is it only failing on your branch? If it's failing on master than you are missing environment dependencies, if it's failing on your branch there are errors in your code.

Please provide the output to all the build commands including cmake not just make.

[xabierolaz]

Im running it on an unmodified fully working gymfc workspace with all Nf and plugins examples working

Sure, full output:
cuda@cuda:~/workspace/aircraft-plugins-new/build$ cmake ..

-- Boost version: 1.65.1
-- Found the following Boost libraries:
--   system
--   thread
--   timer
--   chrono
--   date_time
--   atomic
-- Found DART: /usr/local/include (Required is at least version "6.6") found components:  dart 
-- 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...
-- 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
-- Checking for module 'uuid'
--   Found uuid, version 2.31.1
-- Checking for module 'tinyxml2'
--   Found tinyxml2, version 6.0.0
-- Looking for dlfcn.h - found
-- Looking for libdl - found
-- FreeImage.pc not found, we will search for FreeImage_INCLUDE_DIRS and FreeImage_LIBRARIES
-- Checking for module 'gts'
--   Found gts, version 0.7.6
-- Checking for module 'libswscale'
--   Found libswscale, version 4.8.100
-- Checking for module 'libavdevice >= 56.4.100'
--   Found libavdevice , version 57.10.100
-- Checking for module 'libavformat'
--   Found libavformat, version 57.83.100
-- Checking for module 'libavcodec'
--   Found libavcodec, version 57.107.100
-- Checking for module 'libavutil'
--   Found libavutil, version 55.78.100
-- Checking for module 'jsoncpp'
--   Found jsoncpp, version 1.7.4
-- Checking for module 'yaml-0.1'
--   Found yaml-0.1, version 0.1.7
-- Checking for module 'libzip'
--   Found libzip, version 1.1.2
-- Found Protobuf: /usr/lib/x86_64-linux-gnu/libprotobuf.so;-lpthread;-lpthread;-lpthread (found version "3.0.0") 
Gazebo version: 10.1
-- Configuring done
-- Generating done
-- Build files have been written to: /home/cuda/workspace/aircraft-plugins-new/build

cuda@cuda:~/workspace/aircraft-plugins-new/build$ make

[ 62%] Built target sensor_msgs
[ 75%] Built target control_msgs
[ 83%] Built target gazebo_motor_model
[ 91%] Built target gazebo_imu_plugin
[ 95%] Building CXX object CMakeFiles/gazebo_distance_plugin.dir/src/gazebo_distance_plugin.cpp.o
In file included from /home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:21:0:
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.h: In constructor ‘gazebo::GazeboDistancePlugin::GazeboDistancePlugin()’:
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.h:63:58: error: no matching function for call to ‘ignition::math::v4::Vector3<double>::Vector3(int)’
       ground_distance_units_(ground_distance_units::METER){
                                                          ^
In file included from /usr/include/ignition/math4/ignition/math/Spline.hh:23:0,
                 from /usr/local/include/gazebo-10/gazebo/common/Animation.hh:22,
                 from /usr/local/include/gazebo-10/gazebo/common/common.hh:5,
                 from /home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.h:23,
                 from /home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:21:
/usr/include/ignition/math4/ignition/math/Vector3.hh:77:15: note: candidate: ignition::math::v4::Vector3<T>::Vector3(const ignition::math::v4::Vector3<T>&) [with T = double]
       public: Vector3(const Vector3<T> &_v)
               ^~~~~~~
/usr/include/ignition/math4/ignition/math/Vector3.hh:77:15: note:   no known conversion for argument 1 from ‘int’ to ‘const ignition::math::v4::Vector3<double>&’
/usr/include/ignition/math4/ignition/math/Vector3.hh:68:15: note: candidate: ignition::math::v4::Vector3<T>::Vector3(const T&, const T&, const T&) [with T = double]
       public: Vector3(const T &_x, const T &_y, const T &_z)
               ^~~~~~~
/usr/include/ignition/math4/ignition/math/Vector3.hh:68:15: note:   candidate expects 3 arguments, 1 provided
/usr/include/ignition/math4/ignition/math/Vector3.hh:57:15: note: candidate: ignition::math::v4::Vector3<T>::Vector3() [with T = double]
       public: Vector3()
               ^~~~~~~
/usr/include/ignition/math4/ignition/math/Vector3.hh:57:15: note:   candidate expects 0 arguments, 1 provided
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp: In member function ‘void gazebo::GazeboDistancePlugin::OnUpdate(const gazebo::common::UpdateInfo&)’:
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:145:27: error: missing template arguments before ‘position’
   ignition::math::Vector3 position = T_W_I.Pos();
                           ^~~~~~~~
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:3: error: ‘height’ was not declared in this scope
   height = set_h(position.Z())
   ^~~~~~
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:3: note: suggested alternative: ‘ceilh’
   height = set_h(position.Z())
   ^~~~~~
   ceilh
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:18: error: ‘position’ was not declared in this scope
   height = set_h(position.Z())
                  ^~~~~~~~
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:18: note: suggested alternative: ‘copyPosition’
   height = set_h(position.Z())
                  ^~~~~~~~
                  copyPosition
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:12: error: ‘set_h’ was not declared in this scope
   height = set_h(position.Z())
            ^~~~~
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:147:12: note: suggested alternative: ‘seq_’
   height = set_h(position.Z())
            ^~~~~
            seq_
/home/cuda/workspace/aircraft-plugins-new/src/gazebo_distance_plugin.cpp:160:21: error: ‘class sensor_msgs::msgs::Distance’ has no member named ‘set_seq’
   distance_message_.set_seq(seq_++);
                     ^~~~~~~
CMakeFiles/gazebo_distance_plugin.dir/build.make:62: recipe for target 'CMakeFiles/gazebo_distance_plugin.dir/src/gazebo_distance_plugin.cpp.o' failed
make[2]: *** [CMakeFiles/gazebo_distance_plugin.dir/src/gazebo_distance_plugin.cpp.o] Error 1
CMakeFiles/Makefile2:218: recipe for target 'CMakeFiles/gazebo_distance_plugin.dir/all' failed
make[1]: *** [CMakeFiles/gazebo_distance_plugin.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2

[wil3]

It's failing on set_seq, are you sure that method exists for the distance sensor message? It's saying it doesn't

[xabierolaz]

Hi @wil3 ,
Finally got it compiled without any errors! I made a pull request. #5
Could you test it too?
Next step would be adding the actual .sdf distance sensor, right ?

[wil3]

Nice! I'll try and review/build it tonight. Once that's done we can merge it into this repo and start the gymfc integration. We'll have to come up with a good simple test to verify its working as expected.

[wil3]

@xabierolaz did a pass on the PR, please use Reviewable (button link in the PR) to address comments.

[xabierolaz]

@wil3 did use reviewable, addressed comments there

[xabierolaz]

Nice! I'll try and review/build it tonight. Once that's done we can merge it into this repo and start the gymfc integration. We'll have to come up with a good simple test to verify its working as expected.

what would be your call to make a good simple test to verify everything?