Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Distance sensor #95

Closed
wants to merge 1 commit into from
Closed

Distance sensor #95

wants to merge 1 commit into from

Conversation

xabierolaz
Copy link

@xabierolaz xabierolaz commented Feb 8, 2021 •
edited
Loading

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
Copy link
Author

xabierolaz commented Feb 8, 2021 •
edited
Loading

@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
Copy link
Contributor

@xabierolaz

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

@xabierolaz xabierolaz changed the title Distance sensor (Issue Distance sensor (Issue #79) Feb 8, 2021
@xabierolaz xabierolaz changed the title Distance sensor (Issue #79) Distance sensor Feb 8, 2021
@xabierolaz
Copy link
Author

xabierolaz commented Feb 19, 2021 •
edited
Loading

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

@xabierolaz xabierolaz closed this Feb 19, 2021
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers
No reviews
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
1 participant
@xabierolaz