From 13dd0f95eb5684aa15e2a35a93f0414ede24340c Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Tue, 17 Dec 2019 16:12:03 +0100 Subject: [PATCH] Fixed issue in compiling rotors_gazebo_plugins package --- rotors_gazebo_plugins/CHANGELOG.rst | 566 +++++++++ rotors_gazebo_plugins/CMakeLists.txt | 532 +++++++++ rotors_gazebo_plugins/autobackport/command.sh | 8 + .../autobackport/commandpre7.sh | 15 + rotors_gazebo_plugins/autobackport/seds.txt | 69 ++ .../autobackport/seds_pre7.txt | 69 ++ rotors_gazebo_plugins/cmake/FindEigen.cmake | 81 ++ rotors_gazebo_plugins/cmake/FindGlog.cmake | 155 +++ .../include/liftdrag_plugin/liftdrag_plugin.h | 139 +++ .../include/rotors_gazebo_plugins/common.h | 210 ++++ .../depth_noise_model.hpp | 68 ++ .../external/gazebo_geotagged_images_plugin.h | 65 + .../gazebo_gimbal_controller_plugin.h | 110 ++ .../external/gazebo_gst_camera_plugin.h | 81 ++ .../external/gazebo_lidar_plugin.h | 67 ++ .../external/gazebo_optical_flow_plugin.h | 83 ++ .../rotors_gazebo_plugins/fw_parameters.h | 305 +++++ .../rotors_gazebo_plugins/gazebo_bag_plugin.h | 267 +++++ .../gazebo_controller_interface.h | 116 ++ .../gazebo_fw_dynamics_plugin.h | 163 +++ .../rotors_gazebo_plugins/gazebo_gps_plugin.h | 110 ++ .../rotors_gazebo_plugins/gazebo_imu_plugin.h | 180 +++ .../gazebo_magnetometer_plugin.h | 99 ++ .../gazebo_mavlink_interface.h | 297 +++++ .../gazebo_motor_model.h | 192 +++ .../gazebo_multirotor_base_plugin.h | 121 ++ .../gazebo_noisydepth_plugin.h | 112 ++ .../gazebo_octomap_plugin.h | 95 ++ .../gazebo_odometry_plugin.h | 173 +++ .../gazebo_pressure_plugin.h | 125 ++ .../gazebo_ros_interface_plugin.h | 336 ++++++ .../gazebo_wind_plugin.h | 209 ++++ .../geo_mag_declination.h | 47 + .../motor_controller.hpp | 53 + .../rotors_gazebo_plugins/motor_model.hpp | 57 + .../include/rotors_gazebo_plugins/msgbuffer.h | 80 ++ .../rotors_gazebo_plugins/multi_copter.hpp | 82 ++ .../rotors_gazebo_plugins/sdf_api_wrapper.hpp | 57 + rotors_gazebo_plugins/msgs/Actuators.proto | 17 + .../msgs/CommandMotorSpeed.proto | 6 + .../msgs/ConnectGazeboToRosTopic.proto | 35 + .../msgs/ConnectRosToGazeboTopic.proto | 22 + rotors_gazebo_plugins/msgs/Float32.proto | 7 + .../msgs/FluidPressure.proto | 16 + rotors_gazebo_plugins/msgs/Header.proto | 13 + rotors_gazebo_plugins/msgs/Imu.proto | 22 + rotors_gazebo_plugins/msgs/JointState.proto | 15 + rotors_gazebo_plugins/msgs/Lidar.proto | 10 + .../msgs/MagneticField.proto | 18 + rotors_gazebo_plugins/msgs/NavSatFix.proto | 43 + rotors_gazebo_plugins/msgs/Odometry.proto | 17 + rotors_gazebo_plugins/msgs/OpticalFlow.proto | 18 + rotors_gazebo_plugins/msgs/Point.proto | 11 + rotors_gazebo_plugins/msgs/PointStamped.proto | 12 + rotors_gazebo_plugins/msgs/PoseStamped.proto | 13 + .../msgs/PoseWithCovariance.proto | 11 + .../msgs/PoseWithCovarianceStamped.proto | 12 + .../msgs/RollPitchYawrateThrust.proto | 29 + rotors_gazebo_plugins/msgs/Transform.proto | 10 + .../msgs/TransformStamped.proto | 10 + .../msgs/TransformStampedWithFrameIds.proto | 18 + rotors_gazebo_plugins/msgs/Twist.proto | 12 + rotors_gazebo_plugins/msgs/TwistStamped.proto | 13 + .../msgs/TwistWithCovariance.proto | 11 + .../msgs/Vector3dStamped.proto | 13 + rotors_gazebo_plugins/msgs/WindSpeed.proto | 10 + rotors_gazebo_plugins/msgs/Wrench.proto | 11 + .../msgs/WrenchStamped.proto | 11 + rotors_gazebo_plugins/package.xml | 41 + .../src/depth_noise_model.cpp | 73 ++ .../gazebo_geotagged_images_plugin.cpp | 181 +++ .../gazebo_gimbal_controller_plugin.cpp | 406 +++++++ .../src/external/gazebo_gst_camera_plugin.cpp | 272 +++++ .../src/external/gazebo_lidar_plugin.cpp | 106 ++ .../external/gazebo_optical_flow_plugin.cpp | 168 +++ .../src/gazebo_bag_plugin.cpp | 422 +++++++ .../src/gazebo_controller_interface.cpp | 185 +++ .../src/gazebo_fw_dynamics_plugin.cpp | 409 +++++++ .../src/gazebo_gps_plugin.cpp | 245 ++++ .../src/gazebo_imu_plugin.cpp | 366 ++++++ .../src/gazebo_magnetometer_plugin.cpp | 205 ++++ .../src/gazebo_mavlink_interface.cpp | 943 +++++++++++++++ .../src/gazebo_motor_model.cpp | 456 +++++++ .../src/gazebo_multirotor_base_plugin.cpp | 173 +++ .../src/gazebo_noisydepth_plugin.cpp | 298 +++++ .../src/gazebo_octomap_plugin.cpp | 276 +++++ .../src/gazebo_odometry_plugin.cpp | 617 ++++++++++ .../src/gazebo_pressure_plugin.cpp | 166 +++ .../src/gazebo_ros_interface_plugin.cpp | 1055 +++++++++++++++++ .../src/gazebo_wind_plugin.cpp | 445 +++++++ .../src/geo_mag_declination.cpp | 138 +++ .../src/liftdrag_plugin/liftdrag_plugin.cpp | 396 +++++++ 92 files changed, 14102 insertions(+) create mode 100644 rotors_gazebo_plugins/CHANGELOG.rst create mode 100644 rotors_gazebo_plugins/CMakeLists.txt create mode 100644 rotors_gazebo_plugins/autobackport/command.sh create mode 100644 rotors_gazebo_plugins/autobackport/commandpre7.sh create mode 100644 rotors_gazebo_plugins/autobackport/seds.txt create mode 100644 rotors_gazebo_plugins/autobackport/seds_pre7.txt create mode 100644 rotors_gazebo_plugins/cmake/FindEigen.cmake create mode 100644 rotors_gazebo_plugins/cmake/FindGlog.cmake create mode 100644 rotors_gazebo_plugins/include/liftdrag_plugin/liftdrag_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/common.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/depth_noise_model.hpp create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_geotagged_images_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_gimbal_controller_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_gst_camera_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_lidar_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_optical_flow_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/fw_parameters.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_bag_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_controller_interface.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_fw_dynamics_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_gps_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_imu_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_magnetometer_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_mavlink_interface.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_motor_model.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_multirotor_base_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_noisydepth_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_octomap_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_odometry_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_pressure_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_ros_interface_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_plugin.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/geo_mag_declination.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/motor_controller.hpp create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/motor_model.hpp create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/msgbuffer.h create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/multi_copter.hpp create mode 100644 rotors_gazebo_plugins/include/rotors_gazebo_plugins/sdf_api_wrapper.hpp create mode 100644 rotors_gazebo_plugins/msgs/Actuators.proto create mode 100644 rotors_gazebo_plugins/msgs/CommandMotorSpeed.proto create mode 100644 rotors_gazebo_plugins/msgs/ConnectGazeboToRosTopic.proto create mode 100644 rotors_gazebo_plugins/msgs/ConnectRosToGazeboTopic.proto create mode 100644 rotors_gazebo_plugins/msgs/Float32.proto create mode 100644 rotors_gazebo_plugins/msgs/FluidPressure.proto create mode 100644 rotors_gazebo_plugins/msgs/Header.proto create mode 100644 rotors_gazebo_plugins/msgs/Imu.proto create mode 100644 rotors_gazebo_plugins/msgs/JointState.proto create mode 100644 rotors_gazebo_plugins/msgs/Lidar.proto create mode 100644 rotors_gazebo_plugins/msgs/MagneticField.proto create mode 100644 rotors_gazebo_plugins/msgs/NavSatFix.proto create mode 100644 rotors_gazebo_plugins/msgs/Odometry.proto create mode 100644 rotors_gazebo_plugins/msgs/OpticalFlow.proto create mode 100644 rotors_gazebo_plugins/msgs/Point.proto create mode 100644 rotors_gazebo_plugins/msgs/PointStamped.proto create mode 100644 rotors_gazebo_plugins/msgs/PoseStamped.proto create mode 100644 rotors_gazebo_plugins/msgs/PoseWithCovariance.proto create mode 100644 rotors_gazebo_plugins/msgs/PoseWithCovarianceStamped.proto create mode 100644 rotors_gazebo_plugins/msgs/RollPitchYawrateThrust.proto create mode 100644 rotors_gazebo_plugins/msgs/Transform.proto create mode 100644 rotors_gazebo_plugins/msgs/TransformStamped.proto create mode 100644 rotors_gazebo_plugins/msgs/TransformStampedWithFrameIds.proto create mode 100644 rotors_gazebo_plugins/msgs/Twist.proto create mode 100644 rotors_gazebo_plugins/msgs/TwistStamped.proto create mode 100644 rotors_gazebo_plugins/msgs/TwistWithCovariance.proto create mode 100644 rotors_gazebo_plugins/msgs/Vector3dStamped.proto create mode 100644 rotors_gazebo_plugins/msgs/WindSpeed.proto create mode 100644 rotors_gazebo_plugins/msgs/Wrench.proto create mode 100644 rotors_gazebo_plugins/msgs/WrenchStamped.proto create mode 100644 rotors_gazebo_plugins/package.xml create mode 100644 rotors_gazebo_plugins/src/depth_noise_model.cpp create mode 100644 rotors_gazebo_plugins/src/external/gazebo_geotagged_images_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/external/gazebo_gimbal_controller_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/external/gazebo_gst_camera_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/external/gazebo_lidar_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/external/gazebo_optical_flow_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_bag_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_controller_interface.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_fw_dynamics_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_gps_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_imu_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_magnetometer_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_mavlink_interface.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_motor_model.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_multirotor_base_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_noisydepth_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_octomap_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_odometry_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_ros_interface_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/gazebo_wind_plugin.cpp create mode 100644 rotors_gazebo_plugins/src/geo_mag_declination.cpp create mode 100644 rotors_gazebo_plugins/src/liftdrag_plugin/liftdrag_plugin.cpp diff --git a/rotors_gazebo_plugins/CHANGELOG.rst b/rotors_gazebo_plugins/CHANGELOG.rst new file mode 100644 index 0000000..beaed69 --- /dev/null +++ b/rotors_gazebo_plugins/CHANGELOG.rst @@ -0,0 +1,566 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rotors_gazebo_plugins +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.1.1 (2017-04-27) +----------- +* Merge pull request `#377 `_ from ethz-asl/feature/pressure-plugin + Feature/pressure-plugin +* More verbose variable names in pressure calculations. +* update maintainers +* Giving more verbose names to the constants +* Adapting the pressure plugin to work with Gazebo transport interface +* Adding Gazebo interface for fluid pressure message +* Initial commit for pressure plugin. +* Contributors: Timo Hinzmann, fmina, pvechersky + +2.1.0 (2017-04-08) +----------- +* fixes issue 371 +* Correct gazebo versioning +* Adding yaml_cpp_catkin, along with catkin_simple, as a dependency for ROS configuration +* Adding find_library to FindYamlCpp and linking to it +* Adding FindYamlCpp cmake module to locate yaml-cpp include dirs +* Making some vectors, YAML nodes, and long calculation variables as const +* Minor text fixes. +* Minor fixes +* Some minor fixes to fw dynamics +* Implementing fixed-wing operation with a joystick +* Fixing the TwistStamped message publishing +* Add conversion of RollPitchYawrateThrust msgs from ROS to Gazebo to allow Joystick node input in Gazebo plugins +* Implementing the rest of parameter parsing and forces + moments calculations +* Implementing handling of actuator command to fixed-wing dynamics plugin +* Adding max and min angle of attack as parameters and some minor renaming +* Making Gazebo Actuators msg same as mav_msgs Actuators msg +* Making fixed-wing params more descriptive +* Implementing YAML parsing of fixed-wing aerodynamic parameters +* Parameterizing techpod mesh files and aerodynamic parameters +* First outline of the fixed-wing dynamics plugin +* Adding wind speed recording to rosbag plugin. +* Adapting Rotors plugin to use the new external force and wind speed mav_comm topics +* Adapting wind plugin to publish the WIND SPEED message +* Adding ability to convert Gazebo wind speed msgs into ROS wind speed msgs +* Adding wind speed publishing to the wind plugin +* Fixing a message type typo in the comment +* Merge pull request `#343 `_ from acfloria/features/octomap_plugin_gazebo_origin + Octomap Plugin Gazebo Origin +* run clang-format through all our plugins +* fix style and removed some comments +* Removed debug code, tidied up comments. +* Added helper methods for GZ to ROS and ROS to GZ header conversion. +* Added collision check for map which stored ConnectHelperStorage items by Gazebo topic name. +* ROS interface plugin no longer checks and uses the namespace variable from SDF file. +* Fixed code style of constants in MAVLink interface .cpp file. +* Added todo 'add noise' to IMU plugin. +* GPS plugin now uses default message topics from mav_msgs. +* Fixed up TwistStamped message type, and moved from a sensor to geomtery message namespace. +* Motor model now uses default message types defined in mav_msgs. Made types more descriptive. +* Re-enabled used of default topics from mav_msgs package in IMU plugin. +* Tidied up code style in MAVlink interface plugin. +* Removed unused and duplicate MotorSpeed.proto message type. +* Moved the gimbal controller, GST camera, LIDAR and optical flow plugins to 'external' directories. +* Moved Geotagged images plugin code files into 'external' directories. +* Added default values for passed-in variables to rotors_gazebo_plugins CMake file. +* Use ignition::math::Vector3d instead of gazebo::math::Vector3 for Gazebo version 6 or newer in the octomap plugin. +* Explicitly added specific MAVLink header for debugging. +* Fixed Gazebo topic namespaces into the MAVlink interface plugin. +* All Gazebo plugins now use topics on the default Gazebo namespace. +* ROS interface plugin now uses /gazebo/rotors/ namespace for the two 'connect' topics. +* Fixed up protobuf message numbering. +* Gazebo namespace is now required for all ROS to Gazebo messages. All plugins should be now setting this correctly. +* All ROS to Gazebo message types now support a Gazebo namespace setting. +* ACTUATORS message type now uses Gazebo namespace to create ROS to Gazebo connection. +* Gazebo namespace is now required for the connect gazebo to ros messages. +* Magnetometer, motor model, multirotor base and wind plugins now set Gazebo namespace. +* Controller interface, GPS and IMU plugins now set the Gazebo namespace value in the ROS interface message. +* All topics in odometry plugin now configure Gazebo namespace for ROS interface plugin. +* Gazebo namespace variable is now used in ROS interface plugin if provided. +* Added support into GazeboRosInterfacePlugin to use a provided Gazebo namespace. +* Added Gazebo namespace string to 'connect to ROS' messages. +* Revert "Added mavlink header files (for mavlink plugin)." + This reverts commit 0898d181476b8646b3fd1dfad9e229299be4dbd7. +* Dropped prefix to MAVLink header include. +* Fixed up MAVLink header check in CMakeFile.txt. +* Added ability for CMakeFiles.txt to search for MAVLink header files, unless provided by command-line variable. +* Tidied up header inclusions. +* Revert "Added mavlink header files (for mavlink plugin)." + This reverts commit 285468c184486cdd1bb2b48268b24088d7155a4f. +* Added mavlink header files (for mavlink plugin). + (cherry picked from commit 285468c184486cdd1bb2b48268b24088d7155a4f) +* fix linking on os x +* Fixed up if statement in CMakeLists.txt. +* Added CMake check for Gazebo >= v5.0 if building optical flow plugin. +* Tidied up CMakeLists.txt by creating list that collects targets to be installed rather than manually specifying them at the end. +* Removed system.hh include from geotagged images plugin for Gazebo 5 support. +* Revert "Added namespace functionality to the ROS interface plugin. The 'connect' messages now get passed a Gazebo namespace as well as a topic name." + This reverts commit 30ec6fea1e404badf5df6c5c84f31dbb63f12155. +* Added namespace functionality to the ROS interface plugin. The 'connect' messages now get passed a Gazebo namespace as well as a topic name. +* Removed duplicated protobuf message types in favour of using Gazebo ones instead. +* Fixed header inclusion naming error. +* Tidied up code, removed dead, commented-out ROS references. +* Renamed protobuf namespace to , closes `#339 `_. +* Renamed 'SensorImu.proto' to 'Imu.proto', closes `#341 `_. +* Added commeneted out debug print code for MAVLink GPS message. +* Added debug print statements to MAVLink interface plugin (commented out for now). +* The IMU orientation quaternion is now done the 'PX4' way. +* Fixed typo in MAVLink interface plugin where usec=nsec*1000, replaced with usec=nsec/1000. +* Commented out debug prints in MAVLink interface plugin. +* Removed debug print for motor velocities. +* Added debug printing to MAVLink msg callbacks. +* Added missing semicolon. +* Added better motor ref. velocity debug printing. +* Added more debug printing to MAVLink interface plugin. +* Added todo for absolute gps_position topic (used by MAVlink interface and geotagged image plugins). +* Added debug printing to the MAVLink interface plugin. +* Added status messages to CMakeLists.txt which prints input parameter info. +* Updated call to calcFlow() in gazebo_optical_flow_plugin to match API changes made in OpticalFlow repo. +* Re-added transform broadcaster support to odometry plugin (through ROS interface plugin). Issue `#332 `_. +* Removed duplicate initialisation of topic name in gazebo_motor_model.h. +* Re-added default topic names from mav_msgs dependency back into all plugin header files. Tidied up code comments. +* Re-enabled default msg topics from mav_msgs in the Odometry plugin. +* Fixed typo in CMakeLists.txt when including ADDITIONAL_INCLUDE_DIRS. +* Changed the passed in CMake variable from INCLUDE_DIRECTORIES to ADDITIONAL_INCLUDE_DIRS. +* Added single dependency to mav_comm into Odometry plugin (as test). Printing INCLUDE_DIRECTORIES variable from CMakeLists.txt. +* Standarised commenting in ROS interface plugin and moved method descriptions into the header file. +* Added missing include of 'common.h' in the optical flow plugin. +* Tidied up commenting in ROS interface plugin. +* Removed debug msg print from end of Load() method. +* Removed dead comments and fixed up frame ID for the GPS message. +* Tidied up comments in 'common.h'. +* Added missing commmon.h includes to two .cpp plugin files. +* Added debug print switch for the plugin's Load() method. Added debug print switch comments. +* Removed debug print 'Gazebo node created at...'. +* Added external library liftdrag_plugin.cpp/.h. +* Updated variable in CMakeLists.txt to BUILD_MAVLINK_INTERFACE_PLUGIN. +* Tweaked BUILD_MAVLINK_PLUGIN variable to follow convention of other boolean flags in CMakeLists.txt. +* Added boolean constants to enable/disable debug printing. +* Silenced debug print in wind plugin. +* Fixed typo in setting forces in the wrench message in the wind plugin. +* Removed dead commented-out code from CMakeLists.txt. +* Replaced manual list of .proto files in CMakeLists.txt with file(GLOB ...) syntax. +* Added SHARED qualifer to Gazebo plugin libraries in CMakeLists.txt. +* Removed dead ROS message include from Magnetometer plugin. +* Removed dead ROS message header include from IMU plugin. +* Removed tf::transform related variables from Odometry plugin header. +* Removed ROS ros::Duration() calls from the odometry plugin. +* Removed dead ROS includes from the odometry plugin. +* Removed ROS dependency from the Gazebo GPS plugin, tweaked the NavSatFix protobuf message to accomodate this. +* Implemented GzTransformStampedMsgCallback(). +* Implemented GzPositionStampedMsgCallback(). +* Implemented GzPoseWithCovarianceStampedMsgCallback(). +* Fixed seg fault, needed to resize part of ROS message in the joint states callback. +* Implemented GzJointStateMsgCallback(), but now seg fault is occurring (due to something in the last two commits?). +* Removed ROS dependencies from the Gazebo wind plugin. +* Octomap plugin is now built on a conditional basis, silenced debug printing from the ROS interface plugin during runtime. +* Tidied up debug printing and comments. +* Replaced include_directories() call with target_include_directories(). +* Added Boost as a dependancy of the optical flow plugin. +* Added debug info. +* Linked OpitcalFlow library to Gazebo plugin. +* Fixed bug in checking for header file include variable, and return if variable not found. +* Added check for OpticalFlow_INCLUDE_DIRS. +* Readded write to CMAKE_MODULE_PATH, but now appends rather than overwrites. +* Stopped CMAKE_MODULE_PATH being set to ./cmake in rotors_gazebo_plugins. +* Added debug printing to CMakeLists.txt. +* Added debug printing to CMakeLists.txt. +* Added debug printing to CMakeLists.txt. +* Printing out CMAKE_MODULE_PATH during build. +* CMakeLists.txt now used find_package(OpticalFlow). +* Gazebo optical flow plugin is now built as a shared library. +* Added when optical flow submodule is build. +* Added the OpticalFlow/include directory via include_directories() command in CMakeLists.txt. +* Fixed error in path to OpticalFlow submodule. +* Fixed naming issue with CMAKE_CURRENT_SOURCE_DIR. +* Optical flow subdirectory command is now passed a path depending on CMAKE_CURRENT_SOURCE_DIRECTORY. +* Fixed path for optical flow plugin in add_subdirectory() command. +* Updated path to OpticalFlow 'subdirectory' in CMakeLists.txt. +* Added CMake code to build PX4 optical code module (experimental). +* Added CMake code for optical flow plugin, and is now only built if a cmake command-line argument is set to true. +* LIDAR plugin is now built as a shared library. +* Added CMake code for LIDAR plugin. +* Added CMake code for geotagged images plugin. +* Added remaining plugins from sitl_gazebo repo (.cpp and .h files). Have not updated CMake files yet. +* Turned the 'connect gazebo to ros' and 'connect ros to gazebo' topic names into global variables (couldn't work out how to make them global from the .world file, so this was the next best option). +* Converted gazebo_ros_interface_plugin from a model plugin to a world plugin. +* The ROS interface plugin is now attached to a static model in the world rather than being attahed to the firefly MAV. +* Added beginnings of Gazebo model for the purpose of inserting the ROS interface into the Gazebo world. +* Removed unused variables and dead comments. +* Fixed Gazebo topic name for joint_states. +* Improved the debug printing statements in .cpp files. +* Fixed topic names used in gazebo_odometry_plugin.cpp. +* Fixed typo. +* Added check to GazeboRosInterfacePlugin to make sure publisher doesn't already exist. +* Refactored arrangement of function definitions. +* Fixed include in geo_mag_declination.cpp. +* Added missing geo_mag_declination.cpp file. +* Fixed Gazebo topic name for the Gazebo plugin. +* Reverted back to commit 357ed0f254823e83e392e239a3ab7d32b595125e (Monday's commit just before .xacro files were merged). +* Added debug messages to python files, moving .xacro files from rotors_gazebo/models/rotors_description/urdf into rotors_description/models/urdf. +* Added more debug info. +* Updated debug printing. +* Updated debug printing info in GazeboMotorModel. +* Fixed up topic paths in GazeboMotorModel. +* Printing out the motor velocity topic path. +* Changed Gazebo topic name for the actual motor speed topics. +* Added print message when GazeboMavlinkInterface gets loaded. +* Specifically added the protobuf message library as a dependency to the MAVlink plugin. +* Removed SHARED qualifier for gazebo_mavlink_interface in the cmake file. +* Added geo_mag_declination from sitl_gazebo repo. +* Added mavlink header files (for mavlink plugin). +* Added Lidar, MotorSpeed and OpticalFlow messages from sitl_gazebo. +* Fixed include. +* Adjusted CMakeLists.txt for mavlink plugin. +* Changed behaviour of MAVLINK_INTERFACE variable in CMakeLists so PX4 firmware can set it to TRUE. +* Replaced gazebo mavlink interface plugin with version from sitl_gazebo. +* Changed CMakeList variable name that controls ROS dependency inclusion to 'NO_ROS'. CMakeLists.txt now checks to see if it's defined outside of it's own code (designed to be set via command-line argument). +* Modified CMakeLists whitespace formatting. +* The ROS interface plugin is no longer built if ROS_DEPENDENCY = FALSE. cmake/make builds o.k. with no ROS dependencies! +* Removed unused ROS dependency includes from odometry plugin. +* Removed un-used headers from gazebo_motor_model.h. +* Added commented code from sitl_gazebo r.e. modelling the change in propulsion on rotor due to relative air velocity. Added comments about why this code is not active. +* Finished removing ROS dependencies from GazeboMotorModel. +* Added new protobuf messages for GazeboMotorModel plugin. Half-way through removing ROS depdencies in GazeboMotorModel. +* Removing dependencies on mav_msgs package (for ease of testing purposes). +* Removed unused header include (was a ROS dependency). +* Pubs and subs are now created on first call to OnUpdate() so to be sure the ROS interface plugin has been loaded. +* Hovering example now working with new way of setting up the ROS interface plguin. +* Basic functionality of ROS->Gazebo message converter working. +* Adding ROS to Gazebo message conversion functionality. +* Refactoring in preparation for adding from ROS to Gazebo message conversion support. +* Converting GazeboControllerInterface so to have no ROS dependencies. +* Reverted GazeboControllerInterface just to test new non-singleton way of connecting messages using the ROS interface. Hovering example works. +* Removed references to singleton pattern for ROS interface plugin. +* Removed static .getInstance method to register gazebo to ROS connections, now using another message type instead. +* GazeboBagPlugin is only built if ROS is present. +* Fixed COPY function call. +* Add cmake module for finding Eigen package. Adjusting CMakeLists to build without ROS. +* Fixed paste error in .cpp file. +* Added Transform related messages and transformation publishing from the Odometry plugin. +* Fixed bug when building a new msg by pointing to parts of existing msgs, by using CopyFrom() instead. +* Added comments to ConnectToRos() helper classes. +* All converted plugins now use ConnectToRos(). +* ConnectToRos() now working for multiple message types. +* Refactored method names associated with connecting Gazebo topics to ROS topics. +* Odometry messages are now being correctly published to ROS framework via AttachTo() function. +* Gazebo subscriber callback is now being called via AttachTo() function. AttachHelper() is now a member funciton. +* Commit before making AttachHelper a member function. +* Started adding a generic AttachTo() method for the ROS interface plugin. Compiling but not yet linking basic odometry message yet. +* Odometry message now being captured by ROS interface plugin and published to ROS framework. Hovering example now works again. +* Started modifying GazeboOdometryPlugin to publish Gazebo messages and removing the ROS dependencies. +* ROS interface plugin publishing actuators and link state messages to ROS platform +* Working on the conversion of Gazebo Actuators and JointState messages into ROS messages. +* GazeboMultirotorBase is now publishing Gazebo messages. Repeated Header type in protobuf messages has been extracted and is now shared between other message types, closes `#326 `_. Added debug print to plugin Load() methods to see what plugins are been run by hovering example. +* Renamed gazebo_msg_interface_plugin to gazebo_ros_interface_plugin, closes `#324 `_. +* ROS message interface plugin now converts magnetic field messages and publishes to ROS. +* Added protobuf message type for magnetometer sensor. Magnetometer plugin now publishes Gazebo messages. +* Changed the name of the robot location GPS message to nav_sat_fix (since there is more than one GPS message type). +* Fixed issue with topic name differences between IMU/GPS plugins and the interface plugin. +* Converted ROS asserts to Gazebo asserts, removed ROS header file inclusions from IMU files. +* Adjusting the topic names and removing duplicate model names from namespace. +* Added TwistedStamp protobuf message type for sending ground position messages within Gazebo. +* Renamed GPS message. Gazebo GPS plugin should now emit NavSatFix messages on the Gazebo framework. +* Added GPS protobuf message type. +* Modifying Gazebo GPS plugin to publish Gazebo messages instead of ROS msgs. Commit before adding GPS protobuf message. +* All fields from Gazebo IMU msg copied into ROS IMU msg. +* Working on gazebo to ROS interface plugin. Fixed bug with IMU message header types. +* New Gazebo message interface plugin is loading correctly when hover sim is launched. +* Gazebo is now outputting debug messages to the console (verbose mode is turned on through launch file). +* Fixed error where Google protobuf message indexes where outside limits. +* Added template class for new Gazebo plugin to act as message interface to both Mavlink and ROS. Code is just a template, no functionality yet implemented. +* Fixed bug with un-resolved symbol. Hovering sim now works fine, although it shouldn't be getting any IMU data anymore. +* IMU plugin is now compiling. Crashing on hover sim start due to undefined symbol. +* Removed un-used extra config variable from cmake file. IMU message type is now a custom type. +* Compiled protobuf files are now being copied into devel space, and can be included from other C++ files. +* Begun reworking IMU plugin to publish Gazebo messages. Protobuf files being built/included using CMakeLists.txt. +* Added method/class comments. +* Added namespace comment as per Google style guide. +* fix linking on os x +* Fixed up if statement in CMakeLists.txt. +* Added CMake check for Gazebo >= v5.0 if building optical flow plugin. +* Tidied up CMakeLists.txt by creating list that collects targets to be installed rather than manually specifying them at the end. +* Removed system.hh include from geotagged images plugin for Gazebo 5 support. +* Revert "Added namespace functionality to the ROS interface plugin. The 'connect' messages now get passed a Gazebo namespace as well as a topic name." + This reverts commit 30ec6fea1e404badf5df6c5c84f31dbb63f12155. +* Added namespace functionality to the ROS interface plugin. The 'connect' messages now get passed a Gazebo namespace as well as a topic name. +* Removed duplicated protobuf message types in favour of using Gazebo ones instead. +* Return the origin of the gazebo coordinates in lat/long/alt as part of the octomap service response. +* Fixed header inclusion naming error. +* Tidied up code, removed dead, commented-out ROS references. +* Renamed protobuf namespace to , closes `#339 `_. +* Renamed 'SensorImu.proto' to 'Imu.proto', closes `#341 `_. +* Added commeneted out debug print code for MAVLink GPS message. +* Added debug print statements to MAVLink interface plugin (commented out for now). +* The IMU orientation quaternion is now done the 'PX4' way. +* Fixed typo in MAVLink interface plugin where usec=nsec*1000, replaced with usec=nsec/1000. +* Commented out debug prints in MAVLink interface plugin. +* Removed debug print for motor velocities. +* Added debug printing to MAVLink msg callbacks. +* Added missing semicolon. +* Added better motor ref. velocity debug printing. +* Added more debug printing to MAVLink interface plugin. +* Added todo for absolute gps_position topic (used by MAVlink interface and geotagged image plugins). +* Added debug printing to the MAVLink interface plugin. +* Added status messages to CMakeLists.txt which prints input parameter info. +* Updated call to calcFlow() in gazebo_optical_flow_plugin to match API changes made in OpticalFlow repo. +* Re-added transform broadcaster support to odometry plugin (through ROS interface plugin). Issue `#332 `_. +* Removed duplicate initialisation of topic name in gazebo_motor_model.h. +* Re-added default topic names from mav_msgs dependency back into all plugin header files. Tidied up code comments. +* Re-enabled default msg topics from mav_msgs in the Odometry plugin. +* Fixed typo in CMakeLists.txt when including ADDITIONAL_INCLUDE_DIRS. +* Changed the passed in CMake variable from INCLUDE_DIRECTORIES to ADDITIONAL_INCLUDE_DIRS. +* Added single dependency to mav_comm into Odometry plugin (as test). Printing INCLUDE_DIRECTORIES variable from CMakeLists.txt. +* Standarised commenting in ROS interface plugin and moved method descriptions into the header file. +* Added missing include of 'common.h' in the optical flow plugin. +* Tidied up commenting in ROS interface plugin. +* Removed debug msg print from end of Load() method. +* Removed dead comments and fixed up frame ID for the GPS message. +* Tidied up comments in 'common.h'. +* Added missing commmon.h includes to two .cpp plugin files. +* Added debug print switch for the plugin's Load() method. Added debug print switch comments. +* Removed debug print 'Gazebo node created at...'. +* Added external library liftdrag_plugin.cpp/.h. +* Updated variable in CMakeLists.txt to BUILD_MAVLINK_INTERFACE_PLUGIN. +* Tweaked BUILD_MAVLINK_PLUGIN variable to follow convention of other boolean flags in CMakeLists.txt. +* Added boolean constants to enable/disable debug printing. +* Silenced debug print in wind plugin. +* Fixed typo in setting forces in the wrench message in the wind plugin. +* Removed dead commented-out code from CMakeLists.txt. +* Replaced manual list of .proto files in CMakeLists.txt with file(GLOB ...) syntax. +* Added SHARED qualifer to Gazebo plugin libraries in CMakeLists.txt. +* Removed dead ROS message include from Magnetometer plugin. +* Removed dead ROS message header include from IMU plugin. +* Removed tf::transform related variables from Odometry plugin header. +* Removed ROS ros::Duration() calls from the odometry plugin. +* Removed dead ROS includes from the odometry plugin. +* Removed ROS dependency from the Gazebo GPS plugin, tweaked the NavSatFix protobuf message to accomodate this. +* Implemented GzTransformStampedMsgCallback(). +* Implemented GzPositionStampedMsgCallback(). +* Implemented GzPoseWithCovarianceStampedMsgCallback(). +* Fixed seg fault, needed to resize part of ROS message in the joint states callback. +* Implemented GzJointStateMsgCallback(), but now seg fault is occurring (due to something in the last two commits?). +* Removed ROS dependencies from the Gazebo wind plugin. +* Octomap plugin is now built on a conditional basis, silenced debug printing from the ROS interface plugin during runtime. +* Tidied up debug printing and comments. +* Replaced include_directories() call with target_include_directories(). +* Added Boost as a dependancy of the optical flow plugin. +* Added debug info. +* Linked OpitcalFlow library to Gazebo plugin. +* Fixed bug in checking for header file include variable, and return if variable not found. +* Added check for OpticalFlow_INCLUDE_DIRS. +* Readded write to CMAKE_MODULE_PATH, but now appends rather than overwrites. +* Stopped CMAKE_MODULE_PATH being set to ./cmake in rotors_gazebo_plugins. +* Added debug printing to CMakeLists.txt. +* Added debug printing to CMakeLists.txt. +* Added debug printing to CMakeLists.txt. +* Printing out CMAKE_MODULE_PATH during build. +* CMakeLists.txt now used find_package(OpticalFlow). +* Gazebo optical flow plugin is now built as a shared library. +* Added when optical flow submodule is build. +* Added the OpticalFlow/include directory via include_directories() command in CMakeLists.txt. +* Fixed error in path to OpticalFlow submodule. +* Fixed naming issue with CMAKE_CURRENT_SOURCE_DIR. +* Optical flow subdirectory command is now passed a path depending on CMAKE_CURRENT_SOURCE_DIRECTORY. +* Fixed path for optical flow plugin in add_subdirectory() command. +* Updated path to OpticalFlow 'subdirectory' in CMakeLists.txt. +* Added CMake code to build PX4 optical code module (experimental). +* Added CMake code for optical flow plugin, and is now only built if a cmake command-line argument is set to true. +* LIDAR plugin is now built as a shared library. +* Added CMake code for LIDAR plugin. +* Added CMake code for geotagged images plugin. +* Added remaining plugins from sitl_gazebo repo (.cpp and .h files). Have not updated CMake files yet. +* Turned the 'connect gazebo to ros' and 'connect ros to gazebo' topic names into global variables (couldn't work out how to make them global from the .world file, so this was the next best option). +* Converted gazebo_ros_interface_plugin from a model plugin to a world plugin. +* The ROS interface plugin is now attached to a static model in the world rather than being attahed to the firefly MAV. +* Added beginnings of Gazebo model for the purpose of inserting the ROS interface into the Gazebo world. +* Removed unused variables and dead comments. +* Fixed Gazebo topic name for joint_states. +* Improved the debug printing statements in .cpp files. +* Fixed topic names used in gazebo_odometry_plugin.cpp. +* Fixed typo. +* Added check to GazeboRosInterfacePlugin to make sure publisher doesn't already exist. +* Refactored arrangement of function definitions. +* Fixed include in geo_mag_declination.cpp. +* Added missing geo_mag_declination.cpp file. +* Fixed Gazebo topic name for the Gazebo plugin. +* Reverted back to commit 357ed0f254823e83e392e239a3ab7d32b595125e (Monday's commit just before .xacro files were merged). +* Added debug messages to python files, moving .xacro files from rotors_gazebo/models/rotors_description/urdf into rotors_description/models/urdf. +* Added more debug info. +* Updated debug printing. +* Updated debug printing info in GazeboMotorModel. +* Fixed up topic paths in GazeboMotorModel. +* Printing out the motor velocity topic path. +* Changed Gazebo topic name for the actual motor speed topics. +* Added print message when GazeboMavlinkInterface gets loaded. +* Specifically added the protobuf message library as a dependency to the MAVlink plugin. +* Removed SHARED qualifier for gazebo_mavlink_interface in the cmake file. +* Added geo_mag_declination from sitl_gazebo repo. +* Added mavlink header files (for mavlink plugin). +* Added Lidar, MotorSpeed and OpticalFlow messages from sitl_gazebo. +* Fixed include. +* Adjusted CMakeLists.txt for mavlink plugin. +* Changed behaviour of MAVLINK_INTERFACE variable in CMakeLists so PX4 firmware can set it to TRUE. +* Replaced gazebo mavlink interface plugin with version from sitl_gazebo. +* Changed CMakeList variable name that controls ROS dependency inclusion to 'NO_ROS'. CMakeLists.txt now checks to see if it's defined outside of it's own code (designed to be set via command-line argument). +* Modified CMakeLists whitespace formatting. +* The ROS interface plugin is no longer built if ROS_DEPENDENCY = FALSE. cmake/make builds o.k. with no ROS dependencies! +* Removed unused ROS dependency includes from odometry plugin. +* Removed un-used headers from gazebo_motor_model.h. +* Added commented code from sitl_gazebo r.e. modelling the change in propulsion on rotor due to relative air velocity. Added comments about why this code is not active. +* Finished removing ROS dependencies from GazeboMotorModel. +* Added new protobuf messages for GazeboMotorModel plugin. Half-way through removing ROS depdencies in GazeboMotorModel. +* Removing dependencies on mav_msgs package (for ease of testing purposes). +* Removed unused header include (was a ROS dependency). +* Pubs and subs are now created on first call to OnUpdate() so to be sure the ROS interface plugin has been loaded. +* Hovering example now working with new way of setting up the ROS interface plguin. +* Basic functionality of ROS->Gazebo message converter working. +* Adding ROS to Gazebo message conversion functionality. +* Refactoring in preparation for adding from ROS to Gazebo message conversion support. +* Converting GazeboControllerInterface so to have no ROS dependencies. +* Reverted GazeboControllerInterface just to test new non-singleton way of connecting messages using the ROS interface. Hovering example works. +* Removed references to singleton pattern for ROS interface plugin. +* Removed static .getInstance method to register gazebo to ROS connections, now using another message type instead. +* Merge branch 'master' into feature/px4_merge + Pulling in changes from master. +* GazeboBagPlugin is only built if ROS is present. +* Fixed COPY function call. +* Add cmake module for finding Eigen package. Adjusting CMakeLists to build without ROS. +* Fixed paste error in .cpp file. +* Added Transform related messages and transformation publishing from the Odometry plugin. +* Fixed bug when building a new msg by pointing to parts of existing msgs, by using CopyFrom() instead. +* Added comments to ConnectToRos() helper classes. +* All converted plugins now use ConnectToRos(). +* ConnectToRos() now working for multiple message types. +* Refactored method names associated with connecting Gazebo topics to ROS topics. +* Odometry messages are now being correctly published to ROS framework via AttachTo() function. +* Gazebo subscriber callback is now being called via AttachTo() function. AttachHelper() is now a member funciton. +* Commit before making AttachHelper a member function. +* Started adding a generic AttachTo() method for the ROS interface plugin. Compiling but not yet linking basic odometry message yet. +* Odometry message now being captured by ROS interface plugin and published to ROS framework. Hovering example now works again. +* Started modifying GazeboOdometryPlugin to publish Gazebo messages and removing the ROS dependencies. +* ROS interface plugin publishing actuators and link state messages to ROS platform +* Working on the conversion of Gazebo Actuators and JointState messages into ROS messages. +* GazeboMultirotorBase is now publishing Gazebo messages. Repeated Header type in protobuf messages has been extracted and is now shared between other message types, closes `#326 `_. Added debug print to plugin Load() methods to see what plugins are been run by hovering example. +* Use gzlog and ROS_ERROR instead of std::cout in service callback of the gazebo octomap plugin. +* Renamed gazebo_msg_interface_plugin to gazebo_ros_interface_plugin, closes `#324 `_. +* ROS message interface plugin now converts magnetic field messages and publishes to ROS. +* Add SDF tag for octomapPubTopic and octomapServiceName and load the strings in the gazebo octomap plugin. +* Added protobuf message type for magnetometer sensor. Magnetometer plugin now publishes Gazebo messages. +* Changed the name of the robot location GPS message to nav_sat_fix (since there is more than one GPS message type). +* Fixed issue with topic name differences between IMU/GPS plugins and the interface plugin. +* Converted ROS asserts to Gazebo asserts, removed ROS header file inclusions from IMU files. +* Adjusting the topic names and removing duplicate model names from namespace. +* Added TwistedStamp protobuf message type for sending ground position messages within Gazebo. +* Renamed GPS message. Gazebo GPS plugin should now emit NavSatFix messages on the Gazebo framework. +* Added GPS protobuf message type. +* Add option to publish octomap in the ServiceCallback of the gazebo_octomap_plugin. +* Modifying Gazebo GPS plugin to publish Gazebo messages instead of ROS msgs. Commit before adding GPS protobuf message. +* All fields from Gazebo IMU msg copied into ROS IMU msg. +* Working on gazebo to ROS interface plugin. Fixed bug with IMU message header types. +* New Gazebo message interface plugin is loading correctly when hover sim is launched. +* Gazebo is now outputting debug messages to the console (verbose mode is turned on through launch file). +* Fixed error where Google protobuf message indexes where outside limits. +* Added template class for new Gazebo plugin to act as message interface to both Mavlink and ROS. Code is just a template, no functionality yet implemented. +* Fixed bug with un-resolved symbol. Hovering sim now works fine, although it shouldn't be getting any IMU data anymore. +* IMU plugin is now compiling. Crashing on hover sim start due to undefined symbol. +* Removed un-used extra config variable from cmake file. IMU message type is now a custom type. +* Compiled protobuf files are now being copied into devel space, and can be included from other C++ files. +* Begun reworking IMU plugin to publish Gazebo messages. Protobuf files being built/included using CMakeLists.txt. +* Added method/class comments. +* Added namespace comment as per Google style guide. +* Fixing the order of operations in stopping the recording of a rosbag +* Comment clean-up +* Replacing Vector3Stamped with TwistStamped for ground speed publishing +* Adding precompiler checks in gps plugin to fix Gazebo API compatibility +* Possible fix for compilation error with Gazebo API version 5 +* Adding the plugin to publish data from a GPS sensor on a ROS topic +* Adding a wrapper for some deprecated Gazebo API calls in sensors::GPSSensor +* Magnetometer refactoring to make use of constexpr and proper transform convention +* Creating a magnetometer plugin that is independent of Gazebo API +* Adding the magnetometer gazebo plugin +* Adding a const for initial default value for is_recording\_ in rosbag plugin +* Moving a wrapper for deprecated sdf API moved to a separate class +* Adding ability to start and stop rosbag recording on command +* Adding ability to start and stop rosbag recording on command +* Adding a wrapper class for sdf::Vector3 accessors +* Fixing warnings for deprecated SDF usage +* Fixing a couple of small bugs in mavlink interface plugin +* Refactoring mavlink interface plugin more +* Removing unfinished parts of px4 dummy controller and gazebo mavlink interface. Refactoring gazebo mavlink interface. +* Removing the unfinished wing plugin and the VTOL model +* added comments explaining octomap limitations +* corrected formatting +* improved counter and fixed typo +* cleaned up code a little, added progeress counter +* mark unseen cubes as solid +* switching to edge detection + floodfill method for making octomaps +* switching to edge detection + floodfill method for making octomaps +* Fix octomap plugin hang. +* Merge pull request `#269 `_ from ethz-asl/feature/odometry_plugin_cleanup + general cleanup of plugins and fixes for TFs +* updated comment +* removed spam +* improved IMU plugin in gazebo5 +* Updated to use mavros_msgs. Optionally add mavlink_interface to gazebo models +* style fixes +* set all queue lengths to 1 +* Modified CMakelist to optionally build the mavlink_interface_plugin +* removed mavros from build_depend run_depend +* Silly formating commit 2 +* Silly formating commit +* Added the mavlink interface plugin to the iris model via sdf file +* Added mavlink interface plugin +* Moved mavros dependent stuff out of other plugins into mavlink interface plugin +* Removed message runtime from cmakelist +* Changes required to get posix_sitl with mavros bridge running +* Removed message runtime from cmakelist +* Merge remote-tracking branch 'origin/feature/tfdependency' +* Merge pull request `#16 `_ from PX4/feature/tfdependency + gazebo plugins: depend on tf +* gazebo plugins: depend on tf + contributed by @devbharat +* manually apply 4f1cf03aafca38590fec45d0695ef52383e48645 +* Merge remote-tracking branch 'upstream/master' into px4_nodes_upstreammerge3 +* Revert "remove usage of deprecated function" + This reverts commit 2663d9d664f0a6cb759be2f18152bdc1c47db3f9. +* remove usage of deprecated function +* update launch and xacro files to new names, update use of odometry plugin +* remove whitespace difference to upstream +* move px4 files to new directories +* Contributors: Fadri Furrer, Geoffrey Hunter, Helen Oleynikova, Jon Binney, Julius Bullinger, Michael Burri, Pavel, Thomas Gubler, Zachary Taylor, acfloria, devbharat, pvechersky, z + +2.0.1 (2015-08-10) +------------------ +* fixed the bag plugin and the evaluation +* Contributors: Fadri Furrer + +2.0.0 (2015-08-09) +------------------ +* Changed to new mav_comm messages. +* Changed default topics to be those from mav_msgs/default.h. +* Contributors: Haoyao Chen, Helen Oleynikova, Michael Burri + +1.1.6 (2015-06-11) +------------------ + +1.1.5 (2015-06-09) +------------------ +* added install targets + +1.1.4 (2015-05-28) +------------------ +* added std_srvs dependency + +1.1.3 (2015-05-28) +------------------ +* added installation of controller libraries + +1.1.2 (2015-05-27) +------------------ + +1.1.1 (2015-04-24) +------------------ +* switched from opencv to cv_bridge + +1.1.0 (2015-04-24) +------------------ +* initial Ubuntu package release diff --git a/rotors_gazebo_plugins/CMakeLists.txt b/rotors_gazebo_plugins/CMakeLists.txt new file mode 100644 index 0000000..301944c --- /dev/null +++ b/rotors_gazebo_plugins/CMakeLists.txt @@ -0,0 +1,532 @@ +# Optional arguments to be passed into file +# ADDITIONAL_INCLUDE_DIRS string Additional include directories to add to every build target (PX4 uses this). +# BUILD_MAVLINK_INTERFACE_PLUGIN bool Build mavlink_interface_plugin (requires mav dependency). +# BUILD_OCTOMAP_PLUGIN bool Build the optical map plugin (requires Octomap). +# BUILD_OPTICAL_FLOW_PLUGIN bool Build the optical flow plugin (requires OpenCV). +# MAVLINK_HEADER_DIR string Location of MAVLink header files. If not provided, this CMakeLists.txt file will +# search the default locations (e.g. ROS) for them. This variable is only required +# if BUILD_MAVLINK_INTERFACE_PLUGIN=TRUE. +# NO_ROS bool Build without any ROS dependencies. + +cmake_minimum_required(VERSION 2.8.3) +project(rotors_gazebo_plugins) + +# +#if ( ${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR} ) +# message( FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt." ) +#endif() + +#message(STATUS "rotors_gazebo_plugins CMakeLists.txt called.") + +# =============================================================================================== # +# ========================== SET DEFAULTS FOR PASSED-IN VARIABLES =============================== # +# =============================================================================================== # + +find_package(mavlink) +if(${mavlink_FOUND}) + message(STATUS " mavlink found, building MAVLINK_INTERFACE_PLUGIN.") + set(BUILD_MAVLINK_INTERFACE_PLUGIN TRUE) +else() + message(STATUS " mavlink not found, not building MAVLINK_INTERFACE_PLUGIN.") + set(BUILD_MAVLINK_INTERFACE_PLUGIN FALSE) +endif() + +if(NOT DEFINED BUILD_OCTOMAP_PLUGIN) + message(STATUS "BUILD_OCTOMAP_PLUGIN variable not provided, setting to FALSE.") + set(BUILD_OCTOMAP_PLUGIN FALSE) +endif() + +if(NOT DEFINED BUILD_OPTICAL_FLOW_PLUGIN) + message(STATUS "BUILD_OPTICAL_FLOW_PLUGIN variable not provided, setting to FALSE.") + set(BUILD_OPTICAL_FLOW_PLUGIN FALSE) +endif() + +if(NOT DEFINED NO_ROS) + message(STATUS "NO_ROS variable not provided, setting to FALSE.") + set(NO_ROS FALSE) +endif() + +# Add any additional include directories as specified by the calling process (either user or another CMake file). +# ASL: Doesn't use this, catkin manages the mav_comm dependency +# PX4: Provides include directory for mav_msgs, so that "mav_msgs/default_topics.h" can be found and used. +include_directories(${ADDITIONAL_INCLUDE_DIRS}) + +set(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH}" "${CMAKE_CURRENT_SOURCE_DIR}/cmake") +message(STATUS "CMAKE_BINARY_DIR = ${CMAKE_BINARY_DIR}") + +# Initialise a list which will keep track of all targets +# that need to be installed. +set(targets_to_install "") + +# =============================================================================================== # +# ======================================== STATUS MESSAGES ====================================== # +# =============================================================================================== # + +message(STATUS "ADDITIONAL_INCLUDE_DIRS = ${ADDITIONAL_INCLUDE_DIRS}") + +if(BUILD_OCTOMAP_PLUGIN) + message(STATUS "BUILD_OCTOMAP_PLUGIN = TRUE, building gazebo_octomap_plugin.") +else () + message(STATUS "BUILD_OCTOMAP_PLUGIN = FALSE, NOT building gazebo_octomap_plugin.") +endif () + +if(BUILD_OPTICAL_FLOW_PLUGIN) + message(STATUS "BUILD_OPTICAL_FLOW_PLUGIN = TRUE, building gazebo_optical_flow_plugin.") +else () + message(STATUS "BUILD_OPTICAL_FLOW_PLUGIN = FALSE, NOT building gazebo_optical_flow_plugin.") +endif () + +if(NO_ROS) + message(STATUS "NO_ROS = TRUE, building rotors_gazebo_plugins WITHOUT any ROS dependancies.") +else() + message(STATUS "NO_ROS = FALSE, building rotors_gazebo_plugins WITH ROS dependancies.") +endif() + +# Specify C++11 standard +add_definitions(-std=c++11) + +# Provides a compiler flag notifying the preprocessor about +# the MAVLink Interface plugin build status +add_definitions( + -DMAVLINK_INTERFACE=${BUILD_MAVLINK_INTERFACE_PLUGIN} +) + +# To enable assertions when compiled in release mode. +add_definitions(-DROS_ASSERT_ENABLED) + +if (NOT NO_ROS) + find_package(catkin REQUIRED COMPONENTS + gazebo_plugins + cv_bridge + geometry_msgs + mav_msgs + octomap_msgs + octomap_ros + rosbag + roscpp + rotors_comm + rotors_control + std_srvs + tf + ) +endif() + +message(STATUS "CMAKE_BINARY_DIR = ${CMAKE_BINARY_DIR}" ) + +find_package(Eigen REQUIRED) +find_package(gazebo_dev QUIET) +if (NOT GAZEBO_DEV_FOUND) + find_package(gazebo REQUIRED) +endif() + +if(${gazebo_VERSION_MAJOR} LESS 9) + if(${gazebo_VERSION_MAJOR} LESS 7) + message(WARN "GAZEBO Version ${gazebo_VERSION_MAJOR}.${gazebo_VERSION_MINOR}.${gazebo_VERSION_PATCH}") + message(WARN "GAZEBO older than v9 detected. Autobackporting source files") + + execute_process(COMMAND "sh" "${CMAKE_CURRENT_SOURCE_DIR}/autobackport/commandpre7.sh" WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" RESULT_VARIABLE git_result) + + else() + message(WARN "GAZEBO Version ${gazebo_VERSION_MAJOR}.${gazebo_VERSION_MINOR}.${gazebo_VERSION_PATCH}") + message(WARN "GAZEBO older than v9 detected. Autobackporting source files") + + execute_process(COMMAND "sh" "${CMAKE_CURRENT_SOURCE_DIR}/autobackport/command.sh" WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}" RESULT_VARIABLE git_result) + endif() + +endif() + +find_package(OpenCV REQUIRED) +find_package(Glog REQUIRED) +link_directories(${GAZEBO_LIBRARY_DIRS}) +include_directories(${GAZEBO_INCLUDE_DIRS}) +include_directories(${OpenCV_INCLUDE_DIRS}) +include_directories(${CMAKE_CURRENT_BINARY_DIR}) + + +# if (BUILD_MAVLINK_INTERFACE_PLUGIN) +# include_directories(${mavros_msgs_INCLUDE_DIRS}) +# include_directories(${libmavconn_INCLUDE_DIRS}) +# endif() + +# ============================================== # +# =================== YAML_CPP ================= # +# ============================================== # + +find_package(yaml_cpp_catkin QUIET) +if(${yaml_cpp_catkin_FOUND}) + message(STATUS "Found yaml_cpp_catkin, using instead of system library.") + set(YamlCpp_LIBRARIES ${yaml_cpp_catkin_LIBRARIES}) + set(YamlCpp_INCLUDE_DIRS ${yaml_cpp_catkin_INCLUDE_DIRS}) +else() + message(STATUS "No yaml_cpp_catkin, using yaml-cpp system library instead.") + pkg_check_modules(YamlCpp REQUIRED yaml-cpp>=0.5) +endif() +include_directories(${YamlCpp_INCLUDE_DIR}) + +# ============================================== # +# =================== PROTOBUF ================= # +# ============================================== # + +set(PROTOBUF_IMPORT_DIRS "") +foreach(ITR ${GAZEBO_INCLUDE_DIRS}) + if(ITR MATCHES ".*gazebo-[0-9.]+$") + set(PROTOBUF_IMPORT_DIRS "${ITR}/gazebo/msgs/proto") + endif() +endforeach() +message(STATUS "PROTOBUF_IMPORT_DIRS = " ${PROTOBUF_IMPORT_DIRS}) + +# protobuf required for publishing/subscribing to Gazebo +# messages +# WARNING: THIS MUST BE RUN AFTER PROTOBUF_IMPORT_DIRS IS SETUP +find_package(Protobuf REQUIRED) +pkg_check_modules(PROTOBUF protobuf) + +# Protobuf version check +if ("${PROTOBUF_VERSION}" VERSION_LESS "2.5.0") + message(FATAL_ERROR "protobuf version: ${PROTOBUF_VERSION} not compatible, must be >= 2.5.0") +endif() + +# After running this block of code, +# we should have something similar to +# GAZEBO_MSG_INCLUDE_DIRS = /usr/include/gazebo-7/gazebo/msgs +set(GAZEBO_MSG_INCLUDE_DIRS) +foreach(ITR ${GAZEBO_INCLUDE_DIRS}) + if(ITR MATCHES ".*gazebo-[0-9.]+$") + set(GAZEBO_MSG_INCLUDE_DIRS "${ITR}/gazebo/msgs") + endif() +endforeach() + +# Get lists of all .proto files in the msgs directory +file(GLOB msgs msgs/*.proto) + +# Finally, generate the .cpp files from the .proto files +PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${msgs}) + +# Create a shared library of protobuf messages (.so extension on Linux platforms) +add_library(mav_msgs SHARED ${PROTO_SRCS}) +target_link_libraries(mav_msgs ${PROTOBUF_LIBRARY} gazebo_msgs) + +# This causes mav_msgs to be linked with every created library in this file from this +# point forward. +# NOTE: This is deprecated, should be using target_link_libraries instead +link_libraries(mav_msgs) + +# ============================================== # +# ==================== CATKIN ================== # +# ============================================== # + +if (NOT NO_ROS) + + if(${gazebo_dev_FOUND}) + catkin_package( + INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} + LIBRARIES rotors_gazebo_motor_model rotors_gazebo_controller_interface + CATKIN_DEPENDS cv_bridge geometry_msgs mav_msgs octomap_msgs octomap_ros rosbag roscpp rotors_comm rotors_control std_srvs tf + DEPENDS Eigen gazebo_dev octomap OpenCV + #CFG_EXTRAS rotors_gazebo_plugins.cmake + ) + else() + catkin_package( + INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS} + LIBRARIES rotors_gazebo_motor_model rotors_gazebo_controller_interface + CATKIN_DEPENDS cv_bridge geometry_msgs mav_msgs octomap_msgs octomap_ros rosbag roscpp rotors_comm rotors_control std_srvs tf + DEPENDS Eigen gazebo octomap OpenCV + #CFG_EXTRAS rotors_gazebo_plugins.cmake + ) + + endif() +endif() + + +# Including GAZEBO_MSG_INCLUDE_DIRS here allows the .cpp files generated by custom .proto files to find +# the .cpp/hpp files generated by the built-in Gazebo .proto files +include_directories(include ${catkin_INCLUDE_DIRS} ${GAZEBO_MSG_INCLUDE_DIRS}) +include_directories(${Eigen_INCLUDE_DIRS}) +set(target_linking_LIBRARIES ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) + +# =============================================================================================== # +# ========================================= USER LIBRARIES ====================================== # +# =============================================================================================== # + +# SORTED IN ALPHABETICAL ORDER (by "plugin" name, keep it this way!) + +#========================================= BAG PLUGIN ===========================================// +# Entire GazeboBagPlugin is a heavy ROS dependency, and so rather than passing messages to +# GazeboRosInterfacePlugin, this entire library is only included if ROS is present. +if (NOT NO_ROS) + add_library(rotors_gazebo_bag_plugin SHARED src/gazebo_bag_plugin.cpp) + target_link_libraries(rotors_gazebo_bag_plugin ${target_linking_LIBRARIES} ) + add_dependencies(rotors_gazebo_bag_plugin ${catkin_EXPORTED_TARGETS}) + list(APPEND targets_to_install rotors_gazebo_bag_plugin) +endif() + +#================================= CONTROLLER INTERFACE PLUGIN ==================================// +add_library(rotors_gazebo_controller_interface SHARED src/gazebo_controller_interface.cpp) +target_link_libraries(rotors_gazebo_controller_interface ${target_linking_LIBRARIES} ) +if (NOT NO_ROS) + add_dependencies(rotors_gazebo_controller_interface ${catkin_EXPORTED_TARGETS}) +endif() +list(APPEND targets_to_install rotors_gazebo_controller_interface) + +#=================================== GEOTAGGED IMAGES PLUGIN ====================================// + +message(STATUS "GAZEBO ${gazebo_VERSION_MAJOR}.${gazebo_VERSION_MINOR}.${gazebo_VERSION_PATCH}") +message(STATUS "${GAZEBO_VERSION_MAJOR}") + +if(${gazebo_VERSION_MAJOR} GREATER 4) + add_library(gazebo_geotagged_images_plugin SHARED src/external/gazebo_geotagged_images_plugin.cpp) + target_link_libraries(gazebo_geotagged_images_plugin ${target_linking_LIBRARIES} ) + if (NOT NO_ROS) + add_dependencies(gazebo_geotagged_images_plugin ${catkin_EXPORTED_TARGETS}) + endif() + list(APPEND targets_to_install gazebo_geotagged_images_plugin) +else() + message(STATUS "Gazebo version is less than 5, not building gazebo_geotagged_images_plugin.cpp.") +endif() + +#===================================== FW DYNAMICS PLUGIN =======================================// +add_library(rotors_gazebo_fw_dynamics_plugin SHARED src/gazebo_fw_dynamics_plugin.cpp) +target_link_libraries(rotors_gazebo_fw_dynamics_plugin ${target_linking_LIBRARIES} ${YamlCpp_LIBRARIES}) +if (NOT NO_ROS) + add_dependencies(rotors_gazebo_fw_dynamics_plugin ${catkin_EXPORTED_TARGETS}) +endif() +list(APPEND targets_to_install rotors_gazebo_fw_dynamics_plugin) + +#========================================= GPS PLUGIN ===========================================// +add_library(rotors_gazebo_gps_plugin SHARED src/gazebo_gps_plugin.cpp) +target_link_libraries(rotors_gazebo_gps_plugin ${target_linking_LIBRARIES} ) +if (NOT NO_ROS) + add_dependencies(rotors_gazebo_gps_plugin ${catkin_EXPORTED_TARGETS}) +endif() +list(APPEND targets_to_install rotors_gazebo_gps_plugin) + +#========================================= IMU PLUGIN ===========================================// +add_library(rotors_gazebo_imu_plugin SHARED src/gazebo_imu_plugin.cpp) +target_link_libraries(rotors_gazebo_imu_plugin ${target_linking_LIBRARIES} ) +if (NOT NO_ROS) + add_dependencies(rotors_gazebo_imu_plugin ${catkin_EXPORTED_TARGETS}) +endif() +list(APPEND targets_to_install rotors_gazebo_imu_plugin) + +#========================================= NoisyDepth PLUGIN ======================================// +if(${gazebo_dev_FOUND}) + add_library(rotors_gazebo_noisydepth_plugin SHARED src/gazebo_noisydepth_plugin.cpp + src/depth_noise_model.cpp) + + # As the depth camera plugins .so's are not part of any cmake accessible library, + # but the plugin path is path is, we iterate over all paths until plugin folder is found. + foreach(subdir ${gazebo_dev_LIBRARY_DIRS}) + if (${subdir} MATCHES "gazebo.*plugins") + set(gazebo_dev_DEPTHCAMERA_LIB ${subdir}/libDepthCameraPlugin${CMAKE_SHARED_LIBRARY_SUFFIX}) + endif() + endforeach() + + target_link_libraries(rotors_gazebo_noisydepth_plugin ${target_linking_LIBRARIES} ${gazebo_dev_DEPTHCAMERA_LIB}) + + if (NOT NO_ROS) + add_dependencies(rotors_gazebo_noisydepth_plugin ${catkin_EXPORTED_TARGETS}) + endif() + list(APPEND targets_to_install rotors_gazebo_noisydepth_plugin) +else() + message(STATUS "Gazebo version too old (no gazebo_dev package), not building gazebo_noisydepth_plugin.cpp.") +endif() + +#======================================== LIDAR PLUGIN ==========================================// +if(${gazebo_VERSION_MAJOR} GREATER 4) + add_library(rotors_gazebo_lidar_plugin SHARED src/external/gazebo_lidar_plugin.cpp) + target_link_libraries(rotors_gazebo_lidar_plugin ${target_linking_LIBRARIES} ) + if (NOT NO_ROS) + add_dependencies(rotors_gazebo_lidar_plugin ${catkin_EXPORTED_TARGETS}) + endif() + list(APPEND targets_to_install rotors_gazebo_lidar_plugin) +else() + message(STATUS "Gazebo version is less than 5, not building gazebo_lidar_plugin.cpp.") +endif() + +#===================================== MAGNETOMETER PLUGIN ======================================// +add_library(rotors_gazebo_magnetometer_plugin SHARED src/gazebo_magnetometer_plugin.cpp) +target_link_libraries(rotors_gazebo_magnetometer_plugin ${target_linking_LIBRARIES} ) +if (NOT NO_ROS) + add_dependencies(rotors_gazebo_magnetometer_plugin ${catkin_EXPORTED_TARGETS}) +endif() +list(APPEND targets_to_install rotors_gazebo_magnetometer_plugin) + +#================================= MAVLINK INTERFACE PLUGIN =====================================// + +if(BUILD_MAVLINK_INTERFACE_PLUGIN) + message(STATUS "BUILD_MAVLINK_INTERFACE_PLUGIN = TRUE, adding mavros dependency and building mavlink_interface_plugin.") +else () + message(STATUS "BUILD_MAVLINK_INTERFACE_PLUGIN = FALSE, NOT adding mavros dependency and NOT building mavlink_interface_plugin.") +endif () + +if (BUILD_MAVLINK_INTERFACE_PLUGIN) + + # We need the MAVLink headers. + set(MAVLINK_HEADERS_FOUND FALSE) + + + + # First, check to see if MAVLink headers were passed in as variable + if(NOT ${MAVLINK_HEADER_DIR} STREQUAL "") + message(WARN "MAVLINK_HEADER_DIR provided as '${MAVLINK_HEADER_DIR}'.") + include_directories(${MAVLINK_HEADER_DIR}) + set(MAVLINK_HEADERS_FOUND TRUE) + else() + message(WARN "MAVLINK_HEADER_DIR not provided, falling back to looking at default paths...") + # fist check if there's a mavlink dir in the current workspace + if(EXISTS ${mavlink_INCLUDE_DIRS}/mavlink/v2.0) + message(WARN "Found MAVLink headers in workspace at ' ${mavlink_INCLUDE_DIRS}/mavlink/v2.0/'.") + include_directories("${mavlink_INCLUDE_DIRS}/mavlink/v2.0/") + set(MAVLINK_HEADERS_FOUND TRUE) + # If ROS is installed, we should be able to find them at the path below + elseif(EXISTS /opt/ros/$ENV{ROS_DISTRO}/include/mavlink/v2.0/) + message(STATUS "Found MAVLink headers at '/opt/ros/$ENV{ROS_DISTRO}/include/mavlink/v2.0/'.") + include_directories("/opt/ros/$ENV{ROS_DISTRO}/include/mavlink/v2.0/") + set(MAVLINK_HEADERS_FOUND TRUE) + endif() + + endif() + + if(MAVLINK_HEADERS_FOUND) + # Note that this library includes TWO .cpp files. + add_library(rotors_gazebo_mavlink_interface SHARED src/gazebo_mavlink_interface.cpp src/geo_mag_declination.cpp) + target_link_libraries(rotors_gazebo_mavlink_interface ${target_linking_LIBRARIES} ${mav_msgs}) + #add_dependencies(rotors_gazebo_mavlink_interface ${catkin_EXPORTED_TARGETS} ${mavros_EXPORTED_TARGETS} ${mavros_msgs_EXPORTED_TARGETS}) + list(APPEND targets_to_install rotors_gazebo_mavlink_interface) + else() + message(ERROR "MAVLink headers were not found. They are required for building MavlinkInterfacePlugin. Not building MavlinkInterfacePlugin.") + endif() + +endif() + +#==================================== MOTOR MODEL PLUGIN ========================================// +add_library(rotors_gazebo_motor_model SHARED src/gazebo_motor_model.cpp) +target_link_libraries(rotors_gazebo_motor_model ${target_linking_LIBRARIES} ) +if (NOT NO_ROS) + add_dependencies(rotors_gazebo_motor_model ${catkin_EXPORTED_TARGETS}) +endif() +list(APPEND targets_to_install rotors_gazebo_motor_model) + +#==================================== MULTIROTOR BASE PLUGIN ====================================// +add_library(rotors_gazebo_multirotor_base_plugin SHARED src/gazebo_multirotor_base_plugin.cpp) +target_link_libraries(rotors_gazebo_multirotor_base_plugin ${target_linking_LIBRARIES} ) +if (NOT NO_ROS) + add_dependencies(rotors_gazebo_multirotor_base_plugin ${catkin_EXPORTED_TARGETS}) +endif() +list(APPEND targets_to_install rotors_gazebo_multirotor_base_plugin) + +#====================================== OCTOMAP PLUGIN ==========================================// + +# Conditionally built since it requires Octomap as a dependency +# ASL uses this, PX4 does not +if(BUILD_OCTOMAP_PLUGIN) + find_package(octomap REQUIRED) + add_library(rotors_gazebo_octomap_plugin SHARED src/gazebo_octomap_plugin.cpp) + target_link_libraries(rotors_gazebo_octomap_plugin ${target_linking_LIBRARIES} ) + if (NOT NO_ROS) + add_dependencies(rotors_gazebo_octomap_plugin ${catkin_EXPORTED_TARGETS}) + endif() + list(APPEND targets_to_install rotors_gazebo_octomap_plugin) +endif() + +#======================================= ODOMETRY PLUGIN ========================================// +add_library(rotors_gazebo_odometry_plugin SHARED src/gazebo_odometry_plugin.cpp) +target_link_libraries(rotors_gazebo_odometry_plugin ${target_linking_LIBRARIES} ${OpenCV_LIBRARIES}) +if (NOT NO_ROS) + add_dependencies(rotors_gazebo_odometry_plugin ${catkin_EXPORTED_TARGETS}) +endif() +list(APPEND targets_to_install rotors_gazebo_odometry_plugin) + +#===================================== OPTICAL FLOW PLUGIN ======================================// +# Since the optical flow plugin depends on external code (PX4/OpticalFlow), this is +# only conditionally built +if(BUILD_OPTICAL_FLOW_PLUGIN) + + # Check to make sure the correct Gazebo version is available. + if(${gazebo_VERSION_MAJOR} LESS 5) + message(FATAL_ERROR "Gazebo version needs to be >= v5.x. You specified BUILD_OPTICAL_FLOW_PLUGIN=TRUE, but Gazebo version was less than v5.x.") + endif() + + find_package(Boost 1.40.0 REQUIRED timer) + + # OpticalFlow needs to be built and installed by the calling script, as it is very + # unlikely that this package already exists on the users computer. + # In PX4 this is done with a call to ExternalProject_Add(). + find_package(OpticalFlow REQUIRED) + + add_library(rotors_gazebo_optical_flow_plugin SHARED src/external/gazebo_optical_flow_plugin.cpp) + target_include_directories(rotors_gazebo_optical_flow_plugin PUBLIC ${OpticalFlow_INCLUDE_DIRS}) + target_link_libraries(rotors_gazebo_optical_flow_plugin + ${target_linking_LIBRARIES} + ${Boost_LIBRARIES} + ${OpticalFlow_LIBRARIES}) + if (NOT NO_ROS) + add_dependencies(rotors_gazebo_optical_flow_plugin ${catkin_EXPORTED_TARGETS}) + endif() + list(APPEND targets_to_install rotors_gazebo_optical_flow_plugin) +endif() + +#======================================= PRESSURE PLUGIN ========================================// +add_library(rotors_gazebo_pressure_plugin SHARED src/gazebo_pressure_plugin.cpp) +target_link_libraries(rotors_gazebo_pressure_plugin ${target_linking_LIBRARIES} ${GLOG_LIBRARIES}) +if (NOT NO_ROS) + add_dependencies(rotors_gazebo_pressure_plugin ${catkin_EXPORTED_TARGETS}) +endif() +list(APPEND targets_to_install rotors_gazebo_pressure_plugin) + +#===================================== ROS INTERFACE PLUGIN =====================================// +# This entire plugin is only built if ROS is a dependency +if (NOT NO_ROS) + add_library(rotors_gazebo_ros_interface_plugin SHARED src/gazebo_ros_interface_plugin.cpp) + target_link_libraries(rotors_gazebo_ros_interface_plugin ${target_linking_LIBRARIES} ) + add_dependencies(rotors_gazebo_ros_interface_plugin ${catkin_EXPORTED_TARGETS}) + list(APPEND targets_to_install rotors_gazebo_ros_interface_plugin) +endif() + +#========================================= WIND PLUGIN ==========================================// +add_library(rotors_gazebo_wind_plugin SHARED src/gazebo_wind_plugin.cpp) +target_link_libraries(rotors_gazebo_wind_plugin ${target_linking_LIBRARIES} ) +if (NOT NO_ROS) + add_dependencies(rotors_gazebo_wind_plugin ${catkin_EXPORTED_TARGETS}) +endif() +list(APPEND targets_to_install rotors_gazebo_wind_plugin) + +# =============================================================================================== # +# ======================================= EXTERNAL LIBRARIES ==================================== # +# =============================================================================================== # + +# liftdrag_plugin is provided by Gazebo, but not guaranteed to be on system. +# Naming has not been changed to match rotors convetion, due to this being an external +# code soure. +# Linux is not consistent with plugin availability, even on Gazebo 7 +#if("${GAZEBO_VERSION}" VERSION_LESS "7.0") +if(${gazebo_VERSION_MAJOR} GREATER 4) + add_library(LiftDragPlugin SHARED src/liftdrag_plugin/liftdrag_plugin.cpp) + target_link_libraries(LiftDragPlugin ${target_linking_LIBRARIES} ) + if (NOT NO_ROS) + add_dependencies(LiftDragPlugin ${catkin_EXPORTED_TARGETS}) + endif() + list(APPEND targets_to_install LiftDragPlugin) +else() + message(STATUS "Gazebo version is less than 5, not building liftdrag_plugin.cpp.") +endif() + +message(STATUS "CMAKE_INSTALL_PREFIX = ${CMAKE_INSTALL_PREFIX}") +if (NOT NO_ROS) + set(BIN_DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + set(LIB_DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) +else() + set(BIN_DESTINATION ${CMAKE_BINARY_DIR}) + set(LIB_DESTINATION ${CMAKE_BINARY_DIR}) +endif() + +# Install all targets that were defined above (some conditionally) and added to the +# variable targets_to_install +install( + TARGETS + ${targets_to_install} + DESTINATION ${BIN_DESTINATION} + LIBRARY DESTINATION ${LIB_DESTINATION} +) + +#message(FATAL_ERROR "Reached EOF.") diff --git a/rotors_gazebo_plugins/autobackport/command.sh b/rotors_gazebo_plugins/autobackport/command.sh new file mode 100644 index 0000000..b8d7c65 --- /dev/null +++ b/rotors_gazebo_plugins/autobackport/command.sh @@ -0,0 +1,8 @@ + +find . -type f -name "*.cpp" | xargs sed -i.bak -r -f autobackport/seds.txt +find . -type f -name "*.h" | xargs sed -i.bak -r -f autobackport/seds.txt +find . -type f -name "*.hpp" | xargs sed -i.bak -r -f autobackport/seds.txt + +#sed -i.bak -r -f seds_cmake.txt ../CMakeLists.txt + + diff --git a/rotors_gazebo_plugins/autobackport/commandpre7.sh b/rotors_gazebo_plugins/autobackport/commandpre7.sh new file mode 100644 index 0000000..f32a0d4 --- /dev/null +++ b/rotors_gazebo_plugins/autobackport/commandpre7.sh @@ -0,0 +1,15 @@ +if [[ "$OSTYPE" == "darwin"* ]]; then + find . -type f -name "*.cpp" | xargs sed -i.bak -E -f autobackport/seds_pre7.txt + find . -type f -name "*.h" | xargs sed -i.bak -E -f autobackport/seds_pre7.txt + find . -type f -name "*.hpp" | xargs sed -i.bak -E -f autobackport/seds_pre7.txt +else + find . -type f -name "*.cpp" | xargs sed -i.bak -r -f autobackport/seds_pre7.txt + find . -type f -name "*.h" | xargs sed -i.bak -r -f autobackport/seds_pre7.txt + find . -type f -name "*.hpp" | xargs sed -i.bak -r -f autobackport/seds_pre7.txt +fi + + + +#sed -i.bak -r -f seds_cmake.txt ../CMakeLists.txt + + diff --git a/rotors_gazebo_plugins/autobackport/seds.txt b/rotors_gazebo_plugins/autobackport/seds.txt new file mode 100644 index 0000000..f9e2cf6 --- /dev/null +++ b/rotors_gazebo_plugins/autobackport/seds.txt @@ -0,0 +1,69 @@ +# ignition math namespaces +s/( |^|<|\()ignition::math::Vector3d/\1math::Vector3/g +s/( |^|<|\()ignition::math::Pose3d/\1math::Pose/g +s/( |^|<|\()ignition::math::clamp/\1math::clamp/g +s/( |^|<|\()ignition::math::Quaterniond/\1math::Quaternion/g +s/( |^|<|\()octoignition::math::Vector3d/\octomath::Vector3/g +s/( |^|<|\()ignition::ignition::math::Vector3d/\1ignition::math::Vector3d/g + +#removals +s/event::Events::DisconnectWorldUpdateBegin\(.*\)\;//g + +# includes +s/(#include +)"ignition\/math\/Vector3.hh"/\1"gazebo\/math\/Vector3.hh"/g + +# pointer casting +#Gazebo pre 7 s/std::dynamic_pointer_cast|\.))X(\(\))/\1x/g +s/([^q](->|\.))Y(\(\))/\1y/g +s/([^q](->|\.))Z(\(\))/\1z/g +s/([^q](->|\.))W(\(\))/\1w/g +s/([^q](->|\.))Rot(\(\))/\1rot/g +s/([^q](->|\.))Pos(\(\))/\1pos/g + +#[^q] is for the one time we modify a quaternion that we don't want to change + +#special function calls (manually added) +s/(->|\.)Physics(\()/\1GetPhysicsEngine\2/g +s/(->|\.)Gravity(\()/\1GetPhysicsEngine\(\)->GetGravity\2/g +s/(->|\.)EntityByName(\(.*name)/\1GetByName\2/g +s/(->|\.)EntityByName(\(.*id)/\1GetEntity\2/g +s/(->|\.)Position(\(0)/\1GetAngle(0).Radian(/g + +#function calls! (automatically generated) +#s/(->|\.)Altitude(\()/\1GetAltitude\2/g +s/(->|\.)AvgFPS(\()/\1GetAvgFPS\2/g +s/(->|\.)Camera(\()/\1GetCamera\2/g +s/(->|\.)CoG(\()/\1GetCoG\2/g +s/(->|\.)GlobalAxis(\()/\1GetGlobalAxis\2/g +s/(->|\.)HFOV(\()/\1GetHFOV\2/g +s/(->|\.)ImageData(\()/\1GetImageData\2/g +s/(->|\.)ImageDepth(\()/\1GetImageDepth\2/g +s/(->|\.)ImageFormat(\()/\1GetImageFormat\2/g +s/(->|\.)ImageHeight(\()/\1GetImageHeight\2/g +s/(->|\.)ImageWidth(\()/\1GetImageWidth\2/g +s/(->|\.)Inverse(\()/\1GetInverse\2/g +s/(->|\.)LaserShape(\()/\1GetLaserShape\2/g +s/(->|\.)LastMeasurementTime(\()/\1GetLastMeasurementTime\2/g +s/(->|\.)LastRenderWallTime(\()/\1GetLastRenderWallTime\2/g +#s/(->|\.)Latitude(\()/\1GetLatitude\2/g +s/(->|\.)Length(\()/\1GetLength\2/g +s/(->|\.)Link(\()/\1GetLink\2/g +#s/(->|\.)Longitude(\()/\1GetLongitude\2/g +s/(->|\.)ParentName(\()/\1GetParentName\2/g +s/(->|\.)Range(\()/\1GetRange\2/g +s/(->|\.)RangeMax(\()/\1GetRangeMax\2/g +s/(->|\.)RangeMin(\()/\1GetRangeMin\2/g +s/(->|\.)RelativeAngularVel(\()/\1GetRelativeAngularVel\2/g +s/(->|\.)RelativeLinearAccel(\()/\1GetRelativeLinearAccel\2/g +s/(->|\.)RelativeLinearVel(\()/\1GetRelativeLinearVel\2/g +s/(->|\.)RenderRate(\()/\1GetRenderRate\2/g +s/(->|\.)SimTime(\()/\1GetSimTime\2/g +#s/(->|\.)SetPosition(\()/\1SetAngle\2/g +s/(->|\.)WorldAngularVel(\()/\1GetWorldAngularVel\2/g +s/(->|\.)WorldCoGPose(\()/\1GetWorldCoGPose\2/g +s/(->|\.)WorldLinearVel(\()/\1GetWorldLinearVel\2/g +s/(->|\.)WorldName(\()/\1GetWorldName\2/g +s/(->|\.)WorldPose(\()/\1GetWorldPose\2/g diff --git a/rotors_gazebo_plugins/autobackport/seds_pre7.txt b/rotors_gazebo_plugins/autobackport/seds_pre7.txt new file mode 100644 index 0000000..49a3577 --- /dev/null +++ b/rotors_gazebo_plugins/autobackport/seds_pre7.txt @@ -0,0 +1,69 @@ +# ignition math namespaces +s/( |^|<|\()ignition::math::Vector3d/\1math::Vector3/g +s/( |^|<|\()ignition::math::Pose3d/\1math::Pose/g +s/( |^|<|\()ignition::math::clamp/\1math::clamp/g +s/( |^|<|\()ignition::math::Quaterniond/\1math::Quaternion/g +s/( |^|<|\()octoignition::math::Vector3d/\octomath::Vector3/g +s/( |^|<|\()ignition::ignition::math::Vector3d/\1ignition::math::Vector3d/g + +#removals +s/event::Events::DisconnectWorldUpdateBegin\(.*\)\;//g + +# includes +s/(#include +)"ignition\/math\/Vector3.hh"/\1"gazebo\/math\/Vector3.hh"/g + +# pointer casting +s/std::dynamic_pointer_cast|\.))X(\(\))/\1x/g +s/([^q](->|\.))Y(\(\))/\1y/g +s/([^q](->|\.))Z(\(\))/\1z/g +s/([^q](->|\.))W(\(\))/\1w/g +s/([^q](->|\.))Rot(\(\))/\1rot/g +s/([^q](->|\.))Pos(\(\))/\1pos/g + +#[^q] is for the one time we modify a quaternion that we don't want to change + +#special function calls (manually added) +s/(->|\.)Physics(\()/\1GetPhysicsEngine\2/g +s/(->|\.)Gravity(\()/\1GetPhysicsEngine\(\)->GetGravity\2/g +s/(->|\.)EntityByName(\(.*name)/\1GetByName\2/g +s/(->|\.)EntityByName(\(.*id)/\1GetEntity\2/g +s/(->|\.)Position(\(0)/\1GetAngle(0).Radian(/g + +#function calls! (automatically generated) +s/(->|\.)Altitude(\()/\1GetAltitude\2/g +s/(->|\.)AvgFPS(\()/\1GetAvgFPS\2/g +s/(->|\.)Camera(\()/\1GetCamera\2/g +s/(->|\.)CoG(\()/\1GetCoG\2/g +s/(->|\.)GlobalAxis(\()/\1GetGlobalAxis\2/g +s/(->|\.)HFOV(\()/\1GetHFOV\2/g +s/(->|\.)ImageData(\()/\1GetImageData\2/g +s/(->|\.)ImageDepth(\()/\1GetImageDepth\2/g +s/(->|\.)ImageFormat(\()/\1GetImageFormat\2/g +s/(->|\.)ImageHeight(\()/\1GetImageHeight\2/g +s/(->|\.)ImageWidth(\()/\1GetImageWidth\2/g +s/(->|\.)Inverse(\()/\1GetInverse\2/g +s/(->|\.)LaserShape(\()/\1GetLaserShape\2/g +s/(->|\.)LastMeasurementTime(\()/\1GetLastMeasurementTime\2/g +s/(->|\.)LastRenderWallTime(\()/\1GetLastRenderWallTime\2/g +s/(->|\.)Latitude(\()/\1GetLatitude\2/g +s/(->|\.)Length(\()/\1GetLength\2/g +s/(->|\.)Link(\()/\1GetLink\2/g +s/(->|\.)Longitude(\()/\1GetLongitude\2/g +s/(->|\.)ParentName(\()/\1GetParentName\2/g +s/(->|\.)Range(\()/\1GetRange\2/g +s/(->|\.)RangeMax(\()/\1GetRangeMax\2/g +s/(->|\.)RangeMin(\()/\1GetRangeMin\2/g +s/(->|\.)RelativeAngularVel(\()/\1GetRelativeAngularVel\2/g +s/(->|\.)RelativeLinearAccel(\()/\1GetRelativeLinearAccel\2/g +s/(->|\.)RelativeLinearVel(\()/\1GetRelativeLinearVel\2/g +s/(->|\.)RenderRate(\()/\1GetRenderRate\2/g +s/(->|\.)SimTime(\()/\1GetSimTime\2/g +s/(->|\.)SetPosition(\()/\1SetAngle\2/g +s/(->|\.)WorldAngularVel(\()/\1GetWorldAngularVel\2/g +s/(->|\.)WorldCoGPose(\()/\1GetWorldCoGPose\2/g +s/(->|\.)WorldLinearVel(\()/\1GetWorldLinearVel\2/g +s/(->|\.)WorldName(\()/\1GetWorldName\2/g +s/(->|\.)WorldPose(\()/\1GetWorldPose\2/g diff --git a/rotors_gazebo_plugins/cmake/FindEigen.cmake b/rotors_gazebo_plugins/cmake/FindEigen.cmake new file mode 100644 index 0000000..8587367 --- /dev/null +++ b/rotors_gazebo_plugins/cmake/FindEigen.cmake @@ -0,0 +1,81 @@ +############################################################################### +# +# CMake script for finding the Eigen library. +# +# http://eigen.tuxfamily.org/index.php?title=Main_Page +# +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD +# license. +# +# +# Input variables: +# +# - Eigen_ROOT_DIR (optional): When specified, header files and libraries +# will be searched for in `${Eigen_ROOT_DIR}/include` and +# `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order +# will be ignored. When unspecified, the default CMake search order is used. +# This variable can be specified either as a CMake or environment variable. +# If both are set, preference is given to the CMake variable. +# Use this variable for finding packages installed in a nonstandard location, +# or for enforcing that one of multiple package installations is picked up. +# +# Cache variables (not intended to be used in CMakeLists.txt files) +# +# - Eigen_INCLUDE_DIR: Absolute path to package headers. +# +# +# Output variables: +# +# - Eigen_FOUND: Boolean that indicates if the package was found +# - Eigen_INCLUDE_DIRS: Paths to the necessary header files +# - Eigen_VERSION: Version of Eigen library found +# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen +# +# +# Example usage: +# +# # Passing the version means Eigen_FOUND will only be TRUE if a +# # version >= the provided version is found. +# find_package(Eigen 3.1.2) +# if(NOT Eigen_FOUND) +# # Error handling +# endif() +# ... +# add_definitions(${Eigen_DEFINITIONS}) +# ... +# include_directories(${Eigen_INCLUDE_DIRS} ...) +# +############################################################################### + +find_package(PkgConfig) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + + +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" + "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" + PATH_SUFFIXES eigen3 include/eigen3 include) + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") +endif(EIGEN_FOUND) + + +set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +set(Eigen_FOUND ${EIGEN_FOUND}) +set(Eigen_VERSION ${EIGEN_VERSION}) +set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) diff --git a/rotors_gazebo_plugins/cmake/FindGlog.cmake b/rotors_gazebo_plugins/cmake/FindGlog.cmake new file mode 100644 index 0000000..fc559bf --- /dev/null +++ b/rotors_gazebo_plugins/cmake/FindGlog.cmake @@ -0,0 +1,155 @@ +# Ceres Solver - A fast non-linear least squares minimizer +# Copyright 2013 Google Inc. All rights reserved. +# http://code.google.com/p/ceres-solver/ +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# * Neither the name of Google Inc. nor the names of its contributors may be +# used to endorse or promote products derived from this software without +# specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Author: alexs.mac@gmail.com (Alex Stewart) +# +# FindGlog.cmake - Find Google glog logging library. +# +# This module defines the following variables: +# +# GLOG_FOUND: TRUE iff glog is found. +# GLOG_INCLUDE_DIRS: Include directories for glog. +# GLOG_LIBRARIES: Libraries required to link glog. +# +# The following variables control the behaviour of this module: +# +# GLOG_INCLUDE_DIRS_HINTS: List of additional directories in which to +# search for glog includes, e.g: /timbuktu/include. +# GLOG_LIBRARY_DIRS_HINTS: List of additional directories in which to +# search for glog libraries, e.g: /timbuktu/lib. +# +# The following variables are also defined by this module, but in line with +# CMake recommended FindPackage() module style should NOT be referenced directly +# by callers (use the plural variables detailed above instead). These variables +# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which +# are NOT re-called (i.e. search for library is not repeated) if these variables +# are set with valid values _in the CMake cache_. This means that if these +# variables are set directly in the cache, either by the user in the CMake GUI, +# or by the user passing -DVAR=VALUE directives to CMake when called (which +# explicitly defines a cache variable), then they will be used verbatim, +# bypassing the HINTS variables and other hard-coded search locations. +# +# GLOG_INCLUDE_DIR: Include directory for glog, not including the +# include directory of any dependencies. +# GLOG_LIBRARY: glog library, not including the libraries of any +# dependencies. +# Called if we failed to find glog or any of it's required dependencies, +# unsets all public (designed to be used externally) variables and reports +# error message at priority depending upon [REQUIRED/QUIET/] argument. +MACRO(GLOG_REPORT_NOT_FOUND REASON_MSG) + UNSET(GLOG_FOUND) + UNSET(GLOG_INCLUDE_DIRS) + UNSET(GLOG_LIBRARIES) + # Make results of search visible in the CMake GUI if glog has not + # been found so that user does not have to toggle to advanced view. + MARK_AS_ADVANCED(CLEAR GLOG_INCLUDE_DIR + GLOG_LIBRARY) + # Note _FIND_[REQUIRED/QUIETLY] variables defined by FindPackage() + # use the camelcase library name, not uppercase. + IF (Glog_FIND_QUIETLY) + MESSAGE(STATUS "Failed to find glog - " ${REASON_MSG} ${ARGN}) + ELSEIF (Glog_FIND_REQUIRED) + MESSAGE(FATAL_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) + ELSE() + # Neither QUIETLY nor REQUIRED, use SEND_ERROR which emits an error + # that prevents generation, but continues configuration. + MESSAGE(SEND_ERROR "Failed to find glog - " ${REASON_MSG} ${ARGN}) + ENDIF () +ENDMACRO(GLOG_REPORT_NOT_FOUND) +# TODO: Add standard Windows search locations for glog. +LIST(APPEND GLOG_CHECK_INCLUDE_DIRS + /usr/include + /usr/local/include + /usr/local/homebrew/include # Mac OS X + /opt/local/var/macports/software # Mac OS X. + /opt/local/include) +LIST(APPEND GLOG_CHECK_LIBRARY_DIRS + /usr/lib + /usr/local/lib + /usr/local/homebrew/lib # Mac OS X. + /opt/local/lib) +# Search supplied hint directories first if supplied. +FIND_PATH(GLOG_INCLUDE_DIR + NAMES glog/logging.h + PATHS ${GLOG_INCLUDE_DIR_HINTS} + ${GLOG_CHECK_INCLUDE_DIRS}) +IF (NOT GLOG_INCLUDE_DIR OR + NOT EXISTS ${GLOG_INCLUDE_DIR}) + GLOG_REPORT_NOT_FOUND( + "Could not find glog include directory, set GLOG_INCLUDE_DIR " + "to directory containing glog/logging.h") +ENDIF (NOT GLOG_INCLUDE_DIR OR + NOT EXISTS ${GLOG_INCLUDE_DIR}) +FIND_LIBRARY(GLOG_LIBRARY NAMES glog + PATHS ${GLOG_LIBRARY_DIR_HINTS} + ${GLOG_CHECK_LIBRARY_DIRS}) +IF (NOT GLOG_LIBRARY OR + NOT EXISTS ${GLOG_LIBRARY}) + GLOG_REPORT_NOT_FOUND( + "Could not find glog library, set GLOG_LIBRARY " + "to full path to libglog.") +ENDIF (NOT GLOG_LIBRARY OR + NOT EXISTS ${GLOG_LIBRARY}) +# Mark internally as found, then verify. GLOG_REPORT_NOT_FOUND() unsets +# if called. +SET(GLOG_FOUND TRUE) +# Glog does not seem to provide any record of the version in its +# source tree, thus cannot extract version. +# Catch case when caller has set GLOG_INCLUDE_DIR in the cache / GUI and +# thus FIND_[PATH/LIBRARY] are not called, but specified locations are +# invalid, otherwise we would report the library as found. +IF (GLOG_INCLUDE_DIR AND + NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) + GLOG_REPORT_NOT_FOUND( + "Caller defined GLOG_INCLUDE_DIR:" + " ${GLOG_INCLUDE_DIR} does not contain glog/logging.h header.") +ENDIF (GLOG_INCLUDE_DIR AND + NOT EXISTS ${GLOG_INCLUDE_DIR}/glog/logging.h) +# TODO: This regex for glog library is pretty primitive, could it be better? +IF (GLOG_LIBRARY AND + NOT ${GLOG_LIBRARY} MATCHES ".*glog[^/]*") + GLOG_REPORT_NOT_FOUND( + "Caller defined GLOG_LIBRARY: " + "${GLOG_LIBRARY} does not match glog.") +ENDIF (GLOG_LIBRARY AND + NOT ${GLOG_LIBRARY} MATCHES ".*glog[^/]*") +# Set standard CMake FindPackage variables if found. +IF (GLOG_FOUND) + SET(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) + SET(GLOG_LIBRARIES ${GLOG_LIBRARY}) +ENDIF (GLOG_FOUND) +# Handle REQUIRED / QUIET optional arguments. +INCLUDE(FindPackageHandleStandardArgs) +FIND_PACKAGE_HANDLE_STANDARD_ARGS(Glog DEFAULT_MSG + GLOG_INCLUDE_DIRS GLOG_LIBRARIES) +# Only mark internal variables as advanced if we found glog, otherwise +# leave them visible in the standard GUI for the user to set manually. +IF (GLOG_FOUND) + MARK_AS_ADVANCED(FORCE GLOG_INCLUDE_DIR + GLOG_LIBRARY) +ENDIF (GLOG_FOUND) diff --git a/rotors_gazebo_plugins/include/liftdrag_plugin/liftdrag_plugin.h b/rotors_gazebo_plugins/include/liftdrag_plugin/liftdrag_plugin.h new file mode 100644 index 0000000..b35925f --- /dev/null +++ b/rotors_gazebo_plugins/include/liftdrag_plugin/liftdrag_plugin.h @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2014-2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef _GAZEBO_LIFT_DRAG_PLUGIN_HH_ +#define _GAZEBO_LIFT_DRAG_PLUGIN_HH_ + +#include +#include + +#include "gazebo/common/Plugin.hh" +#include "gazebo/physics/physics.hh" +#include "gazebo/transport/TransportTypes.hh" + +namespace gazebo +{ + /// \brief A plugin that simulates lift and drag. + class GAZEBO_VISIBLE LiftDragPlugin : public ModelPlugin + { + /// \brief Constructor. + public: LiftDragPlugin(); + + /// \brief Destructor. + public: ~LiftDragPlugin(); + + // Documentation Inherited. + public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + /// \brief Callback for World Update events. + protected: virtual void OnUpdate(); + + /// \brief Connection to World Update events. + protected: event::ConnectionPtr updateConnection; + + /// \brief Pointer to world. + protected: physics::WorldPtr world; + + /// \brief Pointer to physics engine. + protected: physics::PhysicsEnginePtr physics; + + /// \brief Pointer to model containing plugin. + protected: physics::ModelPtr model; + + /// \brief Coefficient of Lift / alpha slope. + /// Lift = C_L * q * S + /// where q (dynamic pressure) = 0.5 * rho * v^2 + protected: double cla; + + /// \brief Coefficient of Drag / alpha slope. + /// Drag = C_D * q * S + /// where q (dynamic pressure) = 0.5 * rho * v^2 + protected: double cda; + + /// \brief Coefficient of Moment / alpha slope. + /// Moment = C_M * q * S + /// where q (dynamic pressure) = 0.5 * rho * v^2 + protected: double cma; + + /// \brief angle of attach when airfoil stalls + protected: double alphaStall; + + /// \brief Cl-alpha rate after stall + protected: double claStall; + + /// \brief Cd-alpha rate after stall + protected: double cdaStall; + + /// \brief Cm-alpha rate after stall + protected: double cmaStall; + + /// \brief: \TODO: make a stall velocity curve + protected: double velocityStall; + + /// \brief air density + /// at 25 deg C it's about 1.1839 kg/m^3 + /// At 20 °C and 101.325 kPa, dry air has a density of 1.2041 kg/m3. + protected: double rho; + + /// \brief if the shape is aerodynamically radially symmetric about + /// the forward direction. Defaults to false for wing shapes. + /// If set to true, the upward direction is determined by the + /// angle of attack. + protected: bool radialSymmetry; + + /// \brief effective planeform surface area + protected: double area; + + /// \brief angle of sweep + protected: double sweep; + + /// \brief initial angle of attack + protected: double alpha0; + + /// \brief angle of attack + protected: double alpha; + + /// \brief center of pressure in link local coordinates + protected: ignition::math::Vector3d cp; + + /// \brief Normally, this is taken as a direction parallel to the chord + /// of the airfoil in zero angle of attack forward flight. + protected: ignition::math::Vector3d forward; + + /// \brief A vector in the lift/drag plane, perpendicular to the forward + /// vector. Inflow velocity orthogonal to forward and upward vectors + /// is considered flow in the wing sweep direction. + protected: ignition::math::Vector3d upward; + + /// \brief Smoothed velocity + protected: ignition::math::Vector3d velSmooth; + + /// \brief Pointer to link currently targeted by mud joint. + protected: physics::LinkPtr link; + + /// \brief Pointer to a joint that actuates a control surface for + /// this lifting body + protected: physics::JointPtr controlJoint; + + /// \brief how much to change CL per radian of control surface joint + /// value. + protected: double controlJointRadToCL; + + /// \brief SDF for this plugin; + protected: sdf::ElementPtr sdf; + }; +} +#endif diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/common.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/common.h new file mode 100644 index 0000000..800c917 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/common.h @@ -0,0 +1,210 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ROTORS_GAZEBO_PLUGINS_COMMON_H_ +#define ROTORS_GAZEBO_PLUGINS_COMMON_H_ + +#include +#include +#include + +namespace gazebo { + +//===============================================================================================// +//========================================= DEBUGGING ===========================================// +//===============================================================================================// + +/// \addtogroup Debug Print Switches +/// @{ + +// The following boolean constants enable/disable debug printing when certain plugin methods are called. +// Suitable for debugging purposes. Left on permanently can swamp std::out and can crash Gazebo. + +static const bool kPrintOnPluginLoad = false; +static const bool kPrintOnUpdates = false; +static const bool kPrintOnMsgCallback = false; + +/// @} + +// Default values +static const std::string kDefaultNamespace = ""; +static constexpr double kDefaultRotorVelocitySlowdownSim = 10.0; + +//===============================================================================================// +//================================== TOPICS FOR ROS INTERFACE ===================================// +//===============================================================================================// + +// These should perhaps be defined in an .sdf/.xacro file instead? +static const std::string kConnectGazeboToRosSubtopic = "connect_gazebo_to_ros_subtopic"; +static const std::string kConnectRosToGazeboSubtopic = "connect_ros_to_gazebo_subtopic"; + +/// \brief Special-case topic for ROS interface plugin to listen to (if present) +/// and broadcast transforms to the ROS system. +static const std::string kBroadcastTransformSubtopic = "broadcast_transform"; + + +/// \brief Obtains a parameter from sdf. +/// \param[in] sdf Pointer to the sdf object. +/// \param[in] name Name of the parameter. +/// \param[out] param Param Variable to write the parameter to. +/// \param[in] default_value Default value, if the parameter not available. +/// \param[in] verbose If true, gzerror if the parameter is not available. +template +bool getSdfParam(sdf::ElementPtr sdf, const std::string& name, T& param, const T& default_value, const bool& verbose = + false) { + if (sdf->HasElement(name)) { + param = sdf->GetElement(name)->Get(); + return true; + } + else { + param = default_value; + if (verbose) + gzerr << "[rotors_gazebo_plugins] Please specify a value for parameter \"" << name << "\".\n"; + } + return false; +} + +template +void model_param(const std::string& world_name, const std::string& model_name, const std::string& param, T& param_value) +{ + TiXmlElement* e_param = nullptr; + TiXmlElement* e_param_tmp = nullptr; + std::string dbg_param; + + TiXmlDocument doc(world_name + ".xml"); + if (doc.LoadFile()) + { + TiXmlHandle h_root(doc.RootElement()); + + TiXmlElement* e_model = h_root.FirstChild("model").Element(); + + for( e_model; e_model; e_model=e_model->NextSiblingElement("model") ) + { + const char* attr_name = e_model->Attribute("name"); + if (attr_name) + { + //specific + if (model_name.compare(attr_name) == 0) + { + e_param_tmp = e_model->FirstChildElement(param); + if (e_param_tmp) + { + e_param = e_param_tmp; + dbg_param = ""; + } + break; + } + } + else + { + //common + e_param = e_model->FirstChildElement(param); + dbg_param = "common "; + } + } + + if (e_param) + { + std::istringstream iss(e_param->GetText()); + iss >> param_value; + + gzdbg << model_name << " model: " << dbg_param << "parameter " << param << " = " << param_value << " from " << doc.Value() << "\n"; + } + } + +} + +} + +/// \brief This class can be used to apply a first order filter on a signal. +/// It allows different acceleration and deceleration time constants. +/// \details +/// Short reveiw of discrete time implementation of first order system: +/// Laplace: +/// X(s)/U(s) = 1/(tau*s + 1) +/// continous time system: +/// dx(t) = (-1/tau)*x(t) + (1/tau)*u(t) +/// discretized system (ZoH): +/// x(k+1) = exp(samplingTime*(-1/tau))*x(k) + (1 - exp(samplingTime*(-1/tau))) * u(k) +template +class FirstOrderFilter { + + public: + FirstOrderFilter(double timeConstantUp, double timeConstantDown, T initialState): + timeConstantUp_(timeConstantUp), + timeConstantDown_(timeConstantDown), + previousState_(initialState) {} + + /// \brief This method will apply a first order filter on the inputState. + T updateFilter(T inputState, double samplingTime) { + + T outputState; + if (inputState > previousState_) { + // Calcuate the outputState if accelerating. + double alphaUp = exp(-samplingTime / timeConstantUp_); + // x(k+1) = Ad*x(k) + Bd*u(k) + outputState = alphaUp * previousState_ + (1 - alphaUp) * inputState; + + } + else { + // Calculate the outputState if decelerating. + double alphaDown = exp(-samplingTime / timeConstantDown_); + outputState = alphaDown * previousState_ + (1 - alphaDown) * inputState; + } + previousState_ = outputState; + return outputState; + + } + + ~FirstOrderFilter() {} + + protected: + double timeConstantUp_; + double timeConstantDown_; + T previousState_; +}; + +/// \brief Computes a quaternion from the 3-element small angle approximation theta. +template +Eigen::Quaternion QuaternionFromSmallAngle(const Eigen::MatrixBase & theta) { + typedef typename Derived::Scalar Scalar; + EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived); + EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 3); + const Scalar q_squared = theta.squaredNorm() / 4.0; + + if (q_squared < 1) { + return Eigen::Quaternion(sqrt(1 - q_squared), theta[0] * 0.5, theta[1] * 0.5, theta[2] * 0.5); + } + else { + const Scalar w = 1.0 / sqrt(1 + q_squared); + const Scalar f = w * 0.5; + return Eigen::Quaternion(w, theta[0] * f, theta[1] * f, theta[2] * f); + } +} + +template +void copyPosition(const In& in, Out* out) { + out->x = in.x; + out->y = in.y; + out->z = in.z; +} + +#endif /* ROTORS_GAZEBO_PLUGINS_COMMON_H_ */ diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/depth_noise_model.hpp b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/depth_noise_model.hpp new file mode 100644 index 0000000..2f07ef2 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/depth_noise_model.hpp @@ -0,0 +1,68 @@ +/* + * Copyright 2018 Michael Pantic, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef ROTORS_GAZEBO_PLUGINS_DEPTH_NOISE_MODEL_H +#define ROTORS_GAZEBO_PLUGINS_DEPTH_NOISE_MODEL_H + +#include +#include + +class DepthNoiseModel { + public: + DepthNoiseModel() + : max_depth(1000.0f), min_depth(0.2f), gen(std::random_device{}()) {} + + virtual void ApplyNoise(uint32_t width, uint32_t height, float *data) = 0; + + float max_depth; // [m] + float min_depth; // [m] Values smaller/larger than these two are replaced + // by NaN + + protected: + bool InRange(float depth) const; + + const float bad_point = std::numeric_limits::quiet_NaN(); + std::normal_distribution dist; + std::mt19937 gen; +}; + +class KinectDepthNoiseModel : public DepthNoiseModel { + public: + KinectDepthNoiseModel() : DepthNoiseModel() {} + + void ApplyNoise(uint32_t width, uint32_t height, float *data); +}; + +class D435DepthNoiseModel : public DepthNoiseModel { + public: + D435DepthNoiseModel() + : h_fov(M_PI_2), // Default 90deg for D435 + baseline(0.05f), // Default 50 mm for D435 + subpixel_err(0.1f), // Default subpixel calibration error + max_stdev(3.0f), + DepthNoiseModel() {} + + void ApplyNoise(uint32_t width, uint32_t height, float *data); + + // public params... + float h_fov; // [rad] + float baseline; // [m] + float subpixel_err; // [pixel] Calibration error + float max_stdev; // [m] cutoff for distance standard deviation: + // If modeled standard deviation becomes bigger, it is replaced with this + // value. +}; + +#endif // ROTORS_GAZEBO_PLUGINS_DEPTH_NOISE_MODEL_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_geotagged_images_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_geotagged_images_plugin.h new file mode 100644 index 0000000..0fe56e6 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_geotagged_images_plugin.h @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#pragma once + +#include + +#include +#include +#include +#include +#include +//#include +#include +#include +#include +#include + +namespace gazebo { + +/// \brief Gazebo plugin that saves geotagged camera images to disk. +class GAZEBO_VISIBLE GeotaggedImagesPlugin : public SensorPlugin { + public: GeotaggedImagesPlugin(); + + public: virtual ~GeotaggedImagesPlugin(); + + public: virtual void Load(sensors::SensorPtr sensor, sdf::ElementPtr sdf); + + public: void OnNewFrame(const unsigned char *image); + public: void OnNewGpsPosition(ConstVector3dPtr& v); + + protected: float storeIntervalSec_; + private: int imageCounter_; + common::Time lastImageTime_; + + protected: sensors::CameraSensorPtr parentSensor_; + protected: rendering::CameraPtr camera_; + protected: rendering::ScenePtr scene_; + private: event::ConnectionPtr newFrameConnection_; + private: std::string storageDir_; + private: msgs::Vector3d lastGpsPosition_; + + private: transport::NodePtr node_handle_; + private: std::string namespace_; + private: transport::SubscriberPtr gpsSub_; + + protected: unsigned int width_, height_, depth_; + protected: unsigned int destWidth_, destHeight_; ///< output size + protected: std::string format_; +}; + +} // namespace gazebo diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_gimbal_controller_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_gimbal_controller_plugin.h new file mode 100644 index 0000000..2473fe2 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_gimbal_controller_plugin.h @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2012-2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +/* Desc: A basic gimbal controller + * Author: John Hsu + */ + +#ifndef _GAZEBO_GIMBAL_CONTROLLER_PLUGIN_HH_ +#define _GAZEBO_GIMBAL_CONTROLLER_PLUGIN_HH_ + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + class GAZEBO_VISIBLE GimbalControllerPlugin : public ModelPlugin + { + /// \brief Constructor + public: GimbalControllerPlugin(); + + public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + public: virtual void Init(); + + private: void OnUpdate(); + + private: void OnPitchStringMsg(ConstAnyPtr &_msg); + private: void OnRollStringMsg(ConstAnyPtr &_msg); + private: void OnYawStringMsg(ConstAnyPtr &_msg); + + /// \TODO something to move into Angle class + /// \brief returns _angle1 normalized about + /// (_reference - M_PI, _reference + M_PI] + /// \param[in] _angle1 input angle + /// \param[in] _reference reference input angle for normalization + /// \return normalized _angle1 about _reference + private: double NormalizeAbout(double _angle, double _reference); + + /// \TODO something to move into Angle class + /// \brief returns shortest angular distance from _from to _to + /// \param[in] _from starting anglular position + /// \param[in] _to end angular position + /// \return distance traveled from starting to end angular positions + private: double ShortestAngularDistance(double _from, double _to); + + private: sdf::ElementPtr sdf; + + private: std::vector connections; + + private: transport::SubscriberPtr pitchSub; + private: transport::SubscriberPtr rollSub; + private: transport::SubscriberPtr yawSub; + + private: transport::PublisherPtr pitchPub; + private: transport::PublisherPtr rollPub; + private: transport::PublisherPtr yawPub; + + private: physics::ModelPtr model; + + /// \brief yaw camera + private: physics::JointPtr yawJoint; + + /// \brief camera roll joint + private: physics::JointPtr rollJoint; + + /// \brief camera pitch joint + private: physics::JointPtr pitchJoint; + + private: sensors::ImuSensorPtr imuSensor; + + private: std::string status; + + private: double pitchCommand; + private: double yawCommand; + private: double rollCommand; + + private: transport::NodePtr node; + + private: common::PID pitchPid; + private: common::PID rollPid; + private: common::PID yawPid; + private: common::Time lastUpdateTime; + + private: ignition::ignition::math::Vector3d ThreeAxisRot( + double r11, double r12, double r21, double r31, double r32); + private: ignition::ignition::math::Vector3d QtoZXY( + const ignition::math::Quaterniond &_q); + }; +} +#endif diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_gst_camera_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_gst_camera_plugin.h new file mode 100644 index 0000000..9a9b73d --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_gst_camera_plugin.h @@ -0,0 +1,81 @@ +/* + * Copyright (C) 2012-2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#pragma once + +#include +#include + +#include "gazebo/common/Plugin.hh" +#include "gazebo/sensors/CameraSensor.hh" +#include "gazebo/gazebo.hh" +#include "gazebo/common/common.hh" +#include "gazebo/rendering/Camera.hh" +#include "gazebo/util/system.hh" +#include "gazebo/transport/transport.hh" +#include "gazebo/msgs/msgs.hh" + +#include + +namespace gazebo +{ + + +/// \class GstCameraPlugin +/// \brief A Gazebo plugin that can be attached to a camera and then streams the video data using gstreamer. +/// It streams to a configurable UDP port, default is 5600. +/// +/// Connect to the stream via command line with: +/// gst-launch-1.0 -v udpsrc port=5600 caps='application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H264' \ +/// ! rtph264depay ! avdec_h264 ! videoconvert ! autovideosink fps-update-interval=1000 sync=false +class GAZEBO_VISIBLE GstCameraPlugin : public SensorPlugin +{ + public: GstCameraPlugin(); + + public: virtual ~GstCameraPlugin(); + + public: virtual void Load(sensors::SensorPtr sensor, sdf::ElementPtr sdf); + + public: virtual void OnNewFrame(const unsigned char *image, + unsigned int width, unsigned int height, + unsigned int depth, const std::string &format); + + public: void startGstThread(); + public: void gstCallback(GstElement *appsrc); + + protected: unsigned int width, height, depth; + float rate; + protected: std::string format; + + protected: int udpPort; + + protected: sensors::CameraSensorPtr parentSensor; + protected: rendering::CameraPtr camera; + + private: event::ConnectionPtr newFrameConnection; + + private: transport::NodePtr node_handle_; + private: std::string namespace_; + private: const std::string topicName = "gst_video"; + + GstBuffer *frameBuffer; + std::mutex frameBufferMutex; + GMainLoop *mainLoop; + GstClockTime gstTimestamp; + +}; + +} /* namespace gazebo */ diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_lidar_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_lidar_plugin.h new file mode 100644 index 0000000..da7973e --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_lidar_plugin.h @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2012-2014 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +/* + * Desc: Ray Plugin + * Author: Nate Koenig mod by John Hsu + */ + +#ifndef _GAZEBO_RAY_PLUGIN_HH_ +#define _GAZEBO_RAY_PLUGIN_HH_ + +#include "gazebo/common/Plugin.hh" +#include "gazebo/sensors/SensorTypes.hh" +#include "gazebo/sensors/RaySensor.hh" +#include "gazebo/util/system.hh" + +#include "Lidar.pb.h" + +namespace gazebo +{ + /// \brief A Gazebo LIDAR plugin. + class GAZEBO_VISIBLE GazeboLidarPlugin : public SensorPlugin + { + /// \brief Constructor + public: GazeboLidarPlugin(); + + /// \brief Destructor + public: virtual ~GazeboLidarPlugin(); + + /// \brief Update callback + public: virtual void OnNewLaserScans(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Pointer to parent + protected: physics::WorldPtr world; + + /// \brief The parent sensor + private: + sensors::RaySensorPtr parentSensor; + transport::NodePtr node_handle_; + transport::PublisherPtr lidar_pub_; + std::string namespace_; + + + /// \brief The connection tied to RayPlugin::OnNewLaserScans() + private: + event::ConnectionPtr newLaserScansConnection; + lidar_msgs::msgs::lidar lidar_message; + }; +} +#endif diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_optical_flow_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_optical_flow_plugin.h new file mode 100644 index 0000000..bdd9f19 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/external/gazebo_optical_flow_plugin.h @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2012-2015 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef _GAZEBO_OPTICAL_FLOW_PLUGIN_HH_ +#define _GAZEBO_OPTICAL_FLOW_PLUGIN_HH_ + +#include + +#include "gazebo/common/Plugin.hh" +#include "gazebo/sensors/CameraSensor.hh" +#include "gazebo/gazebo.hh" +#include "gazebo/common/common.hh" +#include "gazebo/rendering/Camera.hh" +#include "gazebo/util/system.hh" +#include "gazebo/transport/transport.hh" +#include "gazebo/msgs/msgs.hh" + +#include "OpticalFlow.pb.h" + +#include +#include +#include + +#include "flow_opencv.hpp" +#include "flow_px4.hpp" + +using namespace cv; +using namespace std; + +namespace gazebo +{ + class GAZEBO_VISIBLE OpticalFlowPlugin : public SensorPlugin + { + public: + OpticalFlowPlugin(); + virtual ~OpticalFlowPlugin(); + virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); + virtual void OnNewFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + protected: + unsigned int width, height, depth; + std::string format; + sensors::CameraSensorPtr parentSensor; + rendering::CameraPtr camera; + + private: + event::ConnectionPtr newFrameConnection; + Mat old_gray; + Mat frame_gray; + transport::PublisherPtr opticalFlow_pub_; + transport::NodePtr node_handle_; + opticalFlow_msgs::msgs::opticalFlow opticalFlow_message; + std::string namespace_; + boost::timer::cpu_timer timer_; + OpticalFlowOpenCV *_optical_flow; + // OpticalFlowPX4 *_optical_flow; + + float hfov; + float rate; + int dt_us; + float focal_length; + double first_frame_time; + double frame_time; + double old_frame_time; + uint32_t frame_time_us; + }; +} +#endif diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/fw_parameters.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/fw_parameters.h new file mode 100644 index 0000000..717a8c3 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/fw_parameters.h @@ -0,0 +1,305 @@ +/* + * Copyright 2017 Pavel Vechersky, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ROTORS_GAZEBO_PLUGINS_FW_PARAMETERS_H_ +#define ROTORS_GAZEBO_PLUGINS_FW_PARAMETERS_H_ + +#include +#include + +namespace gazebo { + +// Forward declaration. +struct ControlSurface; + +// Default vehicle parameters (Techpod model) +static constexpr double kDefaultWingSpan = 2.59; +static constexpr double kDefaultWingSurface = 0.47; +static constexpr double kDefaultChordLength = 0.18; +static constexpr double kDefaultThrustInclination = 0.0; + +// Default aerodynamic parameter values (Techpod model) +static constexpr double kDefaultAlphaMax = 0.27; +static constexpr double kDefaultAlphaMin = -0.27; + +static const Eigen::Vector3d kDefaultCDragAlpha = + Eigen::Vector3d(0.1360, -0.6737, 5.4546); +static const Eigen::Vector3d kDefaultCDragBeta = + Eigen::Vector3d(0.0195, 0.0, -0.3842); +static const Eigen::Vector3d kDefaultCDragDeltaAil = + Eigen::Vector3d(0.0195, 1.4205e-4, 7.5037e-6); +static const Eigen::Vector3d kDefaultCDragDeltaFlp = + Eigen::Vector3d(0.0195, 2.7395e-4, 1.23e-5); + +static const Eigen::Vector2d kDefaultCSideForceBeta = + Eigen::Vector2d(0.0, -0.3073); + +static const Eigen::Vector4d kDefaultCLiftAlpha = + Eigen::Vector4d(0.2127, 10.8060, -46.8324, 60.6017); +static const Eigen::Vector2d kDefaultCLiftDeltaAil = + Eigen::Vector2d(0.3304, 0.0048); +static const Eigen::Vector2d kDefaultCLiftDeltaFlp = + Eigen::Vector2d(0.3304, 0.0073); + +static const Eigen::Vector2d kDefaultCRollMomentBeta = + Eigen::Vector2d(0.0, -0.0154); +static const Eigen::Vector2d kDefaultCRollMomentP = + Eigen::Vector2d(0.0, -0.1647); +static const Eigen::Vector2d kDefaultCRollMomentR = + Eigen::Vector2d(0.0, 0.0117); +static const Eigen::Vector2d kDefaultCRollMomentDeltaAil = + Eigen::Vector2d(0.0, 0.0570); +static const Eigen::Vector2d kDefaultCRollMomentDeltaFlp = + Eigen::Vector2d(0.0, 0.001); + +static const Eigen::Vector2d kDefaultCPitchMomentAlpha = + Eigen::Vector2d(0.0435, -2.9690); +static const Eigen::Vector2d kDefaultCPitchMomentQ = + Eigen::Vector2d(-0.1173, -106.1541); +static const Eigen::Vector2d kDefaultCPitchMomentDeltaElv = + Eigen::Vector2d(-0.1173, -6.1308); + +static const Eigen::Vector2d kDefaultCYawMomentBeta = + Eigen::Vector2d(0.0, 0.0430); +static const Eigen::Vector2d kDefaultCYawMomentR = + Eigen::Vector2d(0.0, -0.0827); +static const Eigen::Vector2d kDefaultCYawMomentDeltaRud = + Eigen::Vector2d(0.0, 0.06); + +static const Eigen::Vector3d kDefaultCThrust = + Eigen::Vector3d(0.0, 14.7217, 0.0); + +// Default values for fixed-wing controls (Techpod model) +static constexpr double kDefaultControlSurfaceDeflectionMin = + -20.0 * M_PI / 180.0; +static constexpr double kDefaultControlSurfaceDeflectionMax = + 20.0 * M_PI / 180.0; + +static constexpr int kDefaultAileronLeftChannel = 4; +static constexpr int kDefaultAileronRightChannel = 0; +static constexpr int kDefaultElevatorChannel = 1; +static constexpr int kDefaultFlapChannel = 2; +static constexpr int kDefaultRudderChannel = 3; +static constexpr int kDefaultThrottleChannel = 5; + +/// \brief Wrapper function for extracting control surface parameters from a +/// YAML node. +inline void YAMLReadControlSurface(const YAML::Node& node, + const std::string& name, + ControlSurface& surface); + +/// \brief This function reads a vector from a YAML node and converts it into +/// a vector of type Eigen. +template +inline void YAMLReadEigenVector(const YAML::Node& node, + const std::string& name, + Eigen::MatrixBase& value); + +/// \brief This function reads a parameter from a YAML node. +template +inline void YAMLReadParam(const YAML::Node& node, + const std::string& name, + T& value); + +/// \brief Macros to reduce copies of names. +#define READ_CONTROL_SURFACE(node, item) \ + YAMLReadControlSurface(node, #item, item); +#define READ_EIGEN_VECTOR(node, item) YAMLReadEigenVector(node, #item, item); +#define READ_PARAM(node, item) YAMLReadParam(node, #item, item); + +struct FWAerodynamicParameters { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + FWAerodynamicParameters() + : alpha_max(kDefaultAlphaMax), + alpha_min(kDefaultAlphaMin), + c_drag_alpha(kDefaultCDragAlpha), + c_drag_beta(kDefaultCDragBeta), + c_drag_delta_ail(kDefaultCDragDeltaAil), + c_drag_delta_flp(kDefaultCDragDeltaFlp), + c_side_force_beta(kDefaultCSideForceBeta), + c_lift_alpha(kDefaultCLiftAlpha), + c_lift_delta_ail(kDefaultCLiftDeltaAil), + c_lift_delta_flp(kDefaultCLiftDeltaFlp), + c_roll_moment_beta(kDefaultCRollMomentBeta), + c_roll_moment_p(kDefaultCRollMomentP), + c_roll_moment_r(kDefaultCRollMomentR), + c_roll_moment_delta_ail(kDefaultCRollMomentDeltaAil), + c_roll_moment_delta_flp(kDefaultCRollMomentDeltaFlp), + c_pitch_moment_alpha(kDefaultCPitchMomentAlpha), + c_pitch_moment_q(kDefaultCPitchMomentQ), + c_pitch_moment_delta_elv(kDefaultCPitchMomentDeltaElv), + c_yaw_moment_beta(kDefaultCYawMomentBeta), + c_yaw_moment_r(kDefaultCYawMomentR), + c_yaw_moment_delta_rud(kDefaultCYawMomentDeltaRud), + c_thrust(kDefaultCThrust) {} + + double alpha_max; + double alpha_min; + + Eigen::Vector3d c_drag_alpha; + Eigen::Vector3d c_drag_beta; + Eigen::Vector3d c_drag_delta_ail; + Eigen::Vector3d c_drag_delta_flp; + + Eigen::Vector2d c_side_force_beta; + + Eigen::Vector4d c_lift_alpha; + Eigen::Vector2d c_lift_delta_ail; + Eigen::Vector2d c_lift_delta_flp; + + Eigen::Vector2d c_roll_moment_beta; + Eigen::Vector2d c_roll_moment_p; + Eigen::Vector2d c_roll_moment_r; + Eigen::Vector2d c_roll_moment_delta_ail; + Eigen::Vector2d c_roll_moment_delta_flp; + + Eigen::Vector2d c_pitch_moment_alpha; + Eigen::Vector2d c_pitch_moment_q; + Eigen::Vector2d c_pitch_moment_delta_elv; + + Eigen::Vector2d c_yaw_moment_beta; + Eigen::Vector2d c_yaw_moment_r; + Eigen::Vector2d c_yaw_moment_delta_rud; + + Eigen::Vector3d c_thrust; + + void LoadAeroParamsYAML(const std::string& yaml_path) { + const YAML::Node node = YAML::LoadFile(yaml_path); + + READ_PARAM(node, alpha_max); + READ_PARAM(node, alpha_min); + + READ_EIGEN_VECTOR(node, c_drag_alpha); + READ_EIGEN_VECTOR(node, c_drag_beta); + READ_EIGEN_VECTOR(node, c_drag_delta_ail); + READ_EIGEN_VECTOR(node, c_drag_delta_flp); + + READ_EIGEN_VECTOR(node, c_side_force_beta); + + READ_EIGEN_VECTOR(node, c_lift_alpha); + READ_EIGEN_VECTOR(node, c_lift_delta_ail); + READ_EIGEN_VECTOR(node, c_lift_delta_flp); + + READ_EIGEN_VECTOR(node, c_roll_moment_beta); + READ_EIGEN_VECTOR(node, c_roll_moment_p); + READ_EIGEN_VECTOR(node, c_roll_moment_r); + READ_EIGEN_VECTOR(node, c_roll_moment_delta_ail); + READ_EIGEN_VECTOR(node, c_roll_moment_delta_flp); + + READ_EIGEN_VECTOR(node, c_pitch_moment_alpha); + READ_EIGEN_VECTOR(node, c_pitch_moment_q); + READ_EIGEN_VECTOR(node, c_pitch_moment_delta_elv); + + READ_EIGEN_VECTOR(node, c_yaw_moment_beta); + READ_EIGEN_VECTOR(node, c_yaw_moment_r); + READ_EIGEN_VECTOR(node, c_yaw_moment_delta_rud); + + READ_EIGEN_VECTOR(node, c_thrust); + } +}; + +struct ControlSurface { + ControlSurface(int cs_channel, + double defl_min = kDefaultControlSurfaceDeflectionMin, + double defl_max = kDefaultControlSurfaceDeflectionMax) + : channel(cs_channel), + deflection_min(defl_min), + deflection_max(defl_max) {} + + int channel; + + double deflection_min; + double deflection_max; + + void LoadControlSurfaceNode(const YAML::Node& node) { + READ_PARAM(node, channel); + READ_PARAM(node, deflection_min); + READ_PARAM(node, deflection_max); + } +}; + +struct FWVehicleParameters { + FWVehicleParameters() + : wing_span(kDefaultWingSpan), + wing_surface(kDefaultWingSurface), + chord_length(kDefaultChordLength), + thrust_inclination(kDefaultThrustInclination), + throttle_channel(kDefaultThrottleChannel), + aileron_left(kDefaultAileronLeftChannel), + aileron_right(kDefaultAileronRightChannel), + elevator(kDefaultElevatorChannel), + flap(kDefaultFlapChannel), + rudder(kDefaultRudderChannel) {} + + double wing_span; + double wing_surface; + double chord_length; + double thrust_inclination; + + int throttle_channel; + + ControlSurface aileron_left; + ControlSurface aileron_right; + ControlSurface elevator; + ControlSurface flap; + ControlSurface rudder; + + void LoadVehicleParamsYAML(const std::string& yaml_path) { + const YAML::Node node = YAML::LoadFile(yaml_path); + + READ_PARAM(node, wing_span); + READ_PARAM(node, wing_surface); + READ_PARAM(node, chord_length); + READ_PARAM(node, thrust_inclination); + + READ_PARAM(node, throttle_channel); + + READ_CONTROL_SURFACE(node, aileron_left); + READ_CONTROL_SURFACE(node, aileron_right); + READ_CONTROL_SURFACE(node, elevator); + READ_CONTROL_SURFACE(node, flap); + READ_CONTROL_SURFACE(node, rudder); + } +}; + +inline void YAMLReadControlSurface(const YAML::Node& node, + const std::string& name, + ControlSurface& surface) { + const YAML::Node surface_node = node[name]; + surface.LoadControlSurfaceNode(surface_node); +} + +template +inline void YAMLReadEigenVector(const YAML::Node& node, + const std::string& name, + Eigen::MatrixBase& value) { + std::vector vec = + node[name].as>(); + assert(vec.size() == Derived::SizeAtCompileTime); + value = Eigen::Map(&vec[0], vec.size()); +} + +template +inline void YAMLReadParam(const YAML::Node& node, + const std::string& name, + T& value) { + value = node[name].as(); +} + +} + +#endif /* ROTORS_GAZEBO_PLUGINS_FW_PARAMETERS_H_ */ diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_bag_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_bag_plugin.h new file mode 100644 index 0000000..ec1150b --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_bag_plugin.h @@ -0,0 +1,267 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rotors_comm/RecordRosbag.h" +#include "rotors_comm/WindSpeed.h" +#include "rotors_gazebo_plugins/common.h" + + +namespace gazebo { + +// Default values, the rest from common.h +static const std::string kDefaultFrameId = "ground_truth_pose"; +static const std::string kDefaultLinkName = "base_link"; +static const std::string kDefaultBagFilename_ = "simulator.bag"; +static const std::string kDefaultRecordingServiceName = "record_rosbag"; +static constexpr bool kDefaultWaitToRecord = false; +static constexpr bool kDefaultIsRecording = false; + +/// \brief This plugin is used to create rosbag files from within gazebo. +/// \details This plugin is ROS dependent, and is not built if NO_ROS=TRUE is provided to +/// CMakeLists.txt (as in the case of a PX4/Firmware build). +class GazeboBagPlugin : public ModelPlugin { + typedef std::map MotorNumberToJointMap; + typedef std::pair MotorNumberToJointPair; + public: + GazeboBagPlugin() + : ModelPlugin(), + namespace_(kDefaultNamespace), + // DEFAULT TOPICS + ground_truth_pose_topic_(mav_msgs::default_topics::GROUND_TRUTH_POSE), + ground_truth_twist_topic_(mav_msgs::default_topics::GROUND_TRUTH_TWIST), + imu_topic_(mav_msgs::default_topics::IMU), + control_attitude_thrust_topic_(mav_msgs::default_topics::COMMAND_ATTITUDE_THRUST), + control_motor_speed_topic_(mav_msgs::default_topics::COMMAND_ACTUATORS), + control_rate_thrust_topic_(mav_msgs::default_topics::COMMAND_RATE_THRUST), + wind_speed_topic_(mav_msgs::default_topics::WIND_SPEED), + motor_topic_(mav_msgs::default_topics::MOTOR_MEASUREMENT), + wrench_topic_(mav_msgs::default_topics::WRENCH), + external_force_topic_(mav_msgs::default_topics::EXTERNAL_FORCE), + waypoint_topic_(mav_msgs::default_topics::COMMAND_TRAJECTORY), + command_pose_topic_(mav_msgs::default_topics::COMMAND_POSE), + //--------------- + frame_id_(kDefaultFrameId), + link_name_(kDefaultLinkName), + bag_filename_(kDefaultBagFilename_), + recording_service_name_(kDefaultRecordingServiceName), + rotor_velocity_slowdown_sim_(kDefaultRotorVelocitySlowdownSim), + wait_to_record_(kDefaultWaitToRecord), + is_recording_(kDefaultIsRecording), + node_handle_(nullptr), + contact_mgr_(nullptr) {} + + virtual ~GazeboBagPlugin(); + + protected: + /// \brief Load the plugin. + /// \param[in] _model Pointer to the model that loaded this plugin. + /// \param[in] _sdf SDF element that describes the plugin. + void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + /// \brief Called when the world is updated. + /// \param[in] _info Update timing information. + void OnUpdate(const common::UpdateInfo& /*_info*/); + + /// \brief Starting recording the rosbag + void StartRecording(); + + /// \brief Stop recording the rosbag + void StopRecording(); + + /// \brief Called when an IMU message is received. + /// \param[in] imu_msg A IMU message from sensor_msgs. + void ImuCallback(const sensor_msgs::ImuConstPtr& imu_msg); + + /// \brief Called when an WrenchStamped message is received. + /// \param[in] force_msg A WrenchStamped message from geometry_msgs. + void ExternalForceCallback(const geometry_msgs::WrenchStampedConstPtr& force_msg); + + /// \brief Called when a MultiDOFJointTrajectoryPoint message is received. + /// \param[in] trajectory_msg A MultiDOFJointTrajectoryPoint message from trajectory_msgs. + void WaypointCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_msg); + + /// \brief Called when a PoseStamped message is received. + /// \param[in] pose_msg A PoseStamped message from geometry_msgs. + void CommandPoseCallback(const geometry_msgs::PoseStampedConstPtr& pose_msg); + + /// \brief Called when a AttitudeThrust message is received. + /// \param[in] control_msg A AttitudeThrust message from mav_msgs. + void AttitudeThrustCallback(const mav_msgs::AttitudeThrustConstPtr& control_msg); + + /// \brief Called when a Actuators message is received. + /// \param[in] control_msg A Actuators message from mav_msgs. + void ActuatorsCallback(const mav_msgs::ActuatorsConstPtr& control_msg); + + /// \brief Called when a RateThrust message is received. + /// \param[in] control_msg A RateThrust message from mav_msgs. + void RateThrustCallback(const mav_msgs::RateThrustConstPtr& control_msg); + + /// \brief Called when a WindSpeed message is received. + /// \param[in] wind_speed_msg A WindSpeed message from rotors_comm. + void WindSpeedCallback(const rotors_comm::WindSpeedConstPtr& wind_speed_msg); + + /// \brief Log the ground truth pose and twist. + /// \param[in] now The current gazebo common::Time + void LogGroundTruth(const common::Time now); + + /// \brief Log all the motor velocities. + /// \param[in] now The current gazebo common::Time + void LogMotorVelocities(const common::Time now); + + /// \brief Log all the wrenches. + /// \param[in] now The current gazebo common::Time + void LogWrenches(const common::Time now); + + /// \brief Called when a request to start or stop recording is received. + /// \param[in] req The request to start or stop recording. + /// \param[out] res The response to be sent back to the client. + bool RecordingServiceCallback(rotors_comm::RecordRosbag::Request& req, + rotors_comm::RecordRosbag::Response& res); + + private: + /// \brief Pointer to the update event connection. + event::ConnectionPtr update_connection_; + + physics::WorldPtr world_; + physics::ModelPtr model_; + physics::LinkPtr link_; + + physics::Link_V child_links_; + + MotorNumberToJointMap motor_joints_; + + /// \brief Pointer to the ContactManager to get all collisions of this + /// link and its children. + physics::ContactManager *contact_mgr_; + + std::string namespace_; + std::string ground_truth_pose_topic_; + std::string ground_truth_twist_topic_; + std::string imu_topic_; + std::string external_force_topic_; + std::string waypoint_topic_; + std::string command_pose_topic_; + std::string control_attitude_thrust_topic_; + std::string control_motor_speed_topic_; + std::string control_rate_thrust_topic_; + std::string wind_speed_topic_; + std::string wrench_topic_; + std::string motor_topic_; + std::string frame_id_; + std::string link_name_; + std::string bag_filename_; + std::string recording_service_name_; + double rotor_velocity_slowdown_sim_; + + /// \brief Mutex lock for thread safety of writing bag files + boost::mutex mtx_; + + /// \brief Whether the plugin should wait for user command to start recording + bool wait_to_record_; + + /// \brief Whether the plugin is currenly recording a rosbag + bool is_recording_; + + rosbag::Bag bag_; + ros::NodeHandle *node_handle_; + + // Ros subscribers + ros::Subscriber imu_sub_; + ros::Subscriber external_force_sub_; + ros::Subscriber waypoint_sub_; + ros::Subscriber control_attitude_thrust_sub_; + ros::Subscriber control_motor_speed_sub_; + ros::Subscriber control_rate_thrust_sub_; + ros::Subscriber wind_speed_sub_; + ros::Subscriber command_pose_sub_; + + // Ros service server + ros::ServiceServer recording_service_; + + std::ofstream csvOut; + + template + void writeBag(const std::string& topic, const ros::Time& time, const T& msg) { + boost::mutex::scoped_lock lock(mtx_); + try { + bag_.write(topic, time, msg); + } + catch (rosbag::BagIOException& e) { + gzerr << "Error while writing to bag " << e.what() << std::endl; + } + catch (rosbag::BagException& e) { + if (time < ros::TIME_MIN) { + gzerr<<"Header stamp not set for msg published on topic: "<< topic << ". " << e.what() << std::endl; + } + else { + gzerr << "Error while writing to bag " << e.what() << std::endl; + } + } + } + + template + void writeBag(const std::string& topic, const ros::Time& time, boost::shared_ptr const& msg) { + boost::mutex::scoped_lock lock(mtx_); + try { + bag_.write(topic, time, msg); + } + catch (rosbag::BagIOException& e) { + gzerr << "Error while writing to bag " << e.what() << std::endl; + } + catch (rosbag::BagException& e) { + if (time < ros::TIME_MIN) { + gzerr<<"Header stamp not set for msg published on topic: "<< topic << ". " << e.what() << std::endl; + } + else { + gzerr << "Error while writing to bag " << e.what() << std::endl; + } + } + } + +}; + +} // namespace gazebo + +#endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_BAG_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_controller_interface.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_controller_interface.h new file mode 100644 index 0000000..26c11ba --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_controller_interface.h @@ -0,0 +1,116 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ROTORS_GAZEBO_PLUGINS_CONTROLLER_INTERFACE_H +#define ROTORS_GAZEBO_PLUGINS_CONTROLLER_INTERFACE_H + +#include +#include +#include + +#include +#include +#include + +#include // This comes from the mav_comm repo + +#include "Actuators.pb.h" + +#include "rotors_gazebo_plugins/common.h" + +namespace gazebo { + +/// \brief Motor speed topic name. +/// \details This just proxies the motor commands from command/motor_speed to the single motors via internal +/// ConsPtr passing, such that the original commands don't have to go n_motors-times over the wire. +static const std::string kDefaultMotorVelocityReferenceTopic = "gazebo/command/motor_speed"; + +typedef const boost::shared_ptr GzActuatorsMsgPtr; + +class GazeboControllerInterface : public ModelPlugin { + public: + GazeboControllerInterface() + : ModelPlugin(), + received_first_reference_(false), + pubs_and_subs_created_(false), + namespace_(kDefaultNamespace), + // DEFAULT TOPICS + motor_velocity_reference_pub_topic_(kDefaultMotorVelocityReferenceTopic), + command_motor_speed_sub_topic_(mav_msgs::default_topics::COMMAND_ACTUATORS), + //--------------- + node_handle_(NULL){} + ~GazeboControllerInterface(); + + void InitializeParams(); + void Publish(); + + protected: + void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + void OnUpdate(const common::UpdateInfo& /*_info*/); + + private: + + /// \brief Gets set to true the first time a motor command is received. + /// \details OnUpdate() will not do anything until this is true. + bool received_first_reference_; + + /// \brief Flag that is set to true once CreatePubsAndSubs() is called, used + /// to prevent CreatePubsAndSubs() from be called on every OnUpdate(). + bool pubs_and_subs_created_; + + /// \brief Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. + /// \details Call this once the first time OnUpdate() is called (can't + /// be called from Load() because there is no guarantee GazeboRosInterfacePlugin has + /// has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages). + void CreatePubsAndSubs(); + + /// \details This gets populated (including resizing if needed) when CommandMotorCallback() is + /// called. + Eigen::VectorXd input_reference_; + + //===== VARIABLES READ FROM SDF FILE =====// + std::string namespace_; + std::string motor_velocity_reference_pub_topic_; + std::string command_motor_speed_sub_topic_; + + + gazebo::transport::NodePtr node_handle_; + gazebo::transport::PublisherPtr motor_velocity_reference_pub_; + gazebo::transport::SubscriberPtr cmd_motor_sub_; + + physics::ModelPtr model_; + physics::WorldPtr world_; + + /// \brief Pointer to the update event connection. + event::ConnectionPtr updateConnection_; + + boost::thread callback_queue_thread_; + + void QueueThread(); + + void CommandMotorCallback(GzActuatorsMsgPtr& actuators_msg); + +}; + +} // namespace gazebo + +#endif // ROTORS_GAZEBO_PLUGINS_CONTROLLER_INTERFACE_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_fw_dynamics_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_fw_dynamics_plugin.h new file mode 100644 index 0000000..30aec65 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_fw_dynamics_plugin.h @@ -0,0 +1,163 @@ +/* + * Copyright 2017 Pavel Vechersky, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ROTORS_GAZEBO_PLUGINS_FW_DYNAMICS_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_FW_DYNAMICS_PLUGIN_H + +#include +#include +#include +#include +#include + +#include "Actuators.pb.h" +#include "RollPitchYawrateThrust.pb.h" +#include "WindSpeed.pb.h" + +#include "rotors_gazebo_plugins/common.h" +#include "rotors_gazebo_plugins/fw_parameters.h" + +namespace gazebo { + +typedef const boost::shared_ptr + GzActuatorsMsgPtr; +typedef const boost::shared_ptr + GzRollPitchYawrateThrustMsgPtr; +typedef const boost::shared_ptr + GzWindSpeedMsgPtr; + +// Default values. +static constexpr bool kDefaultIsInputJoystick = false; + +// Constants. +static constexpr double kAirDensity = 1.18; +static constexpr double kGravity = 9.81; +static constexpr double kMinAirSpeedThresh = 0.1; + +class GazeboFwDynamicsPlugin : public ModelPlugin { + public: + /// \brief Constructor. + GazeboFwDynamicsPlugin(); + + /// \brief Destructor. + virtual ~GazeboFwDynamicsPlugin(); + + protected: + /// \brief Called when the plugin is first created, and after the world + /// has been loaded. This function should not be blocking. + void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + /// \brief This gets called by the world update start event. + void OnUpdate(const common::UpdateInfo&); + + /// \brief Calculates the forces and moments to be applied to the + /// fixed-wing body. + void UpdateForcesAndMoments(); + + /// \brief Convert control surface input that is normalized in range + /// [-1, 1] to a physical deflection angle value. + double NormalizedInputToAngle(const ControlSurface& surface, double input); + + private: + /// \brief Are the input commands coming from a joystick (as opposed to + /// a remote control via HIL interface, for example)? + bool is_input_joystick_; + + /// \brief Flag that is set to true once CreatePubsAndSubs() is called, + /// used to prevent CreatePubsAndSubs() from be called on every + /// OnUpdate(). + bool pubs_and_subs_created_; + + /// \brief Creates all required publishers and subscribers, incl. routing + /// of messages to/from ROS if required. + /// \details Call this once the first time OnUpdate() is called (can't be + /// called from Load() because there is no guarantee + /// GazeboRosInterfacePlugin has loaded and listening to + /// ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages). + void CreatePubsAndSubs(); + + /// \brief Transport namespace. + std::string namespace_; + /// \brief Topic name for actuator commands. + std::string actuators_sub_topic_; + /// \brief Topic name for roll_pitch_yawrate_thrust commands. + std::string roll_pitch_yawrate_thrust_sub_topic_; + /// \brief Topic name for wind speed readings. + std::string wind_speed_sub_topic_; + + /// \brief Handle for the Gazebo node. + transport::NodePtr node_handle_; + + /// \brief Subscriber for receiving actuator commands. + gazebo::transport::SubscriberPtr actuators_sub_; + /// \brief Subscriber for receiving roll_pitch_yawrate_thrust commands. + gazebo::transport::SubscriberPtr roll_pitch_yawrate_thrust_sub_; + /// \brief Subscriber ror receiving wind speed readings. + gazebo::transport::SubscriberPtr wind_speed_sub_; + + /// \brief Pointer to the world. + physics::WorldPtr world_; + /// \brief Pointer to the model. + physics::ModelPtr model_; + /// \brief Pointer to the link. + physics::LinkPtr link_; + /// \brief Pointer to the update event connection. + event::ConnectionPtr updateConnection_; + + /// \brief Most current wind speed reading [m/s]. + ignition::math::Vector3d W_wind_speed_W_B_; + + /// \brief The aerodynamic properties of the aircraft. + FWAerodynamicParameters aero_params_; + + /// \brief The physical properties of the aircraft. + FWVehicleParameters vehicle_params_; + + /// \brief Left aileron deflection [rad]. + double delta_aileron_left_; + /// \brief Right aileron deflection [rad]. + double delta_aileron_right_; + /// \brief Elevator deflection [rad]. + double delta_elevator_; + /// \brief Flap deflection [rad]. + double delta_flap_; + /// \brief Rudder deflection [rad]. + double delta_rudder_; + /// \brief Throttle input, in range from 0 to 1. + double throttle_; + + /// \brief Processes the actuator commands. + /// \details Converts control surface actuator inputs into physical angles + /// before storing them and throttle values for later use in + /// calculation of forces and moments. + void ActuatorsCallback(GzActuatorsMsgPtr& actuators_msg); + + /// \brief Process the roll_pitch_yawrate_thrust commands. + /// \details Converts the inputs into physical angles before storing them + /// and thrust values for later use in calculation of forces and + /// moments. + void RollPitchYawrateThrustCallback(GzRollPitchYawrateThrustMsgPtr& + roll_pitch_yawrate_thrust_msg); + + /// \brief Processes the wind speed readings. + /// \details Stores the most current wind speed reading for later use in + /// calculation of forces and moments. + void WindSpeedCallback(GzWindSpeedMsgPtr& wind_speed_msg); +}; + +} // namespace gazebo + +#endif // ROTORS_GAZEBO_PLUGINS_FW_DYNAMICS_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_gps_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_gps_plugin.h new file mode 100644 index 0000000..57e88ea --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_gps_plugin.h @@ -0,0 +1,110 @@ +/* + * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ROTORS_GAZEBO_PLUGINS_GPS_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_GPS_PLUGIN_H + +// SYSTEM +#include + +// 3RD PARTY +#include +#include +#include + +// USER +#include "NavSatFix.pb.h" // GPS message type generated by protobuf .proto file +#include "TwistStamped.pb.h" // GPS ground speed message +#include "rotors_gazebo_plugins/common.h" + +namespace gazebo { + +// Default values +static constexpr double kDefaultHorPosStdDev = 3.0; +static constexpr double kDefaultVerPosStdDev = 6.0; +static constexpr double kDefaultHorVelStdDev = 0.1; +static constexpr double kDefaultVerVelStdDev = 0.1; + +class GazeboGpsPlugin : public SensorPlugin { + public: + typedef std::normal_distribution<> NormalDistribution; + + GazeboGpsPlugin(); + virtual ~GazeboGpsPlugin(); + + protected: + void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); + + /// \brief Publishes both a NavSatFix and Gazebo message. + void OnUpdate(); + + private: + + std::string namespace_; + + /// \brief Flag that is set to true once CreatePubsAndSubs() is called, used + /// to prevent CreatePubsAndSubs() from be called on every OnUpdate(). + bool pubs_and_subs_created_; + + /// \brief Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. + /// \details Call this once the first time OnUpdate() is called (can't + /// be called from Load() because there is no guarantee GazeboRosInterfacePlugin has + /// has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages). + void CreatePubsAndSubs(); + + gazebo::transport::NodePtr node_handle_; + + gazebo::transport::PublisherPtr gz_gps_pub_; + + gazebo::transport::PublisherPtr gz_ground_speed_pub_; + + /// \brief Name of topic for GPS messages, read from SDF file. + std::string gps_topic_; + + /// \brief Name of topic for ground speed messages, read from SDF file. + std::string ground_speed_topic_; + + /// \brief Pointer to the parent sensor + sensors::GpsSensorPtr parent_sensor_; + + /// \biref Pointer to the world. + physics::WorldPtr world_; + + /// \brief Pointer to the sensor link. + physics::LinkPtr link_; + + /// \brief Pointer to the update event connection. + event::ConnectionPtr updateConnection_; + + /// \brief GPS message to be published on sensor update. + gz_sensor_msgs::NavSatFix gz_gps_message_; + + /// \brief Ground speed message to be published on sensor update. + gz_geometry_msgs::TwistStamped gz_ground_speed_message_; + + /// \brief Normal distributions for ground speed noise in x, y, and z directions. + NormalDistribution ground_speed_n_[3]; + + /// \brief Random number generator. + std::random_device random_device_; + + std::mt19937 random_generator_; +}; + +} // namespace gazebo + +#endif // ROTORS_GAZEBO_PLUGINS_GPS_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_imu_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_imu_plugin.h new file mode 100644 index 0000000..51c7b4a --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_imu_plugin.h @@ -0,0 +1,180 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ROTORS_GAZEBO_PLUGINS_IMU_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_IMU_PLUGIN_H + +#include + +#include +#include +#include +#include +#include + +#include "Imu.pb.h" + +#include "rotors_gazebo_plugins/common.h" + +namespace gazebo { + +// Default values for use with ADIS16448 IMU +static constexpr double kDefaultAdisGyroscopeNoiseDensity = + 2.0 * 35.0 / 3600.0 / 180.0 * M_PI; +static constexpr double kDefaultAdisGyroscopeRandomWalk = + 2.0 * 4.0 / 3600.0 / 180.0 * M_PI; +static constexpr double kDefaultAdisGyroscopeBiasCorrelationTime = + 1.0e+3; +static constexpr double kDefaultAdisGyroscopeTurnOnBiasSigma = + 0.5 / 180.0 * M_PI; +static constexpr double kDefaultAdisAccelerometerNoiseDensity = + 2.0 * 2.0e-3; +static constexpr double kDefaultAdisAccelerometerRandomWalk = + 2.0 * 3.0e-3; +static constexpr double kDefaultAdisAccelerometerBiasCorrelationTime = + 300.0; +static constexpr double kDefaultAdisAccelerometerTurnOnBiasSigma = + 20.0e-3 * 9.8; +// Earth's gravity in Zurich (lat=+47.3667degN, lon=+8.5500degE, h=+500m, WGS84) +static constexpr double kDefaultGravityMagnitude = 9.8068; + +// A description of the parameters: +// https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model-and-Intrinsics +// TODO(burrimi): Should I have a minimalistic description of the params here? +struct ImuParameters { + /// Gyroscope noise density (two-sided spectrum) [rad/s/sqrt(Hz)] + double gyroscope_noise_density; + /// Gyroscope bias random walk [rad/s/s/sqrt(Hz)] + double gyroscope_random_walk; + /// Gyroscope bias correlation time constant [s] + double gyroscope_bias_correlation_time; + /// Gyroscope turn on bias standard deviation [rad/s] + double gyroscope_turn_on_bias_sigma; + /// Accelerometer noise density (two-sided spectrum) [m/s^2/sqrt(Hz)] + double accelerometer_noise_density; + /// Accelerometer bias random walk. [m/s^2/s/sqrt(Hz)] + double accelerometer_random_walk; + /// Accelerometer bias correlation time constant [s] + double accelerometer_bias_correlation_time; + /// Accelerometer turn on bias standard deviation [m/s^2] + double accelerometer_turn_on_bias_sigma; + /// Norm of the gravitational acceleration [m/s^2] + double gravity_magnitude; + + ImuParameters() + : gyroscope_noise_density(kDefaultAdisGyroscopeNoiseDensity), + gyroscope_random_walk(kDefaultAdisGyroscopeRandomWalk), + gyroscope_bias_correlation_time( + kDefaultAdisGyroscopeBiasCorrelationTime), + gyroscope_turn_on_bias_sigma(kDefaultAdisGyroscopeTurnOnBiasSigma), + accelerometer_noise_density(kDefaultAdisAccelerometerNoiseDensity), + accelerometer_random_walk(kDefaultAdisAccelerometerRandomWalk), + accelerometer_bias_correlation_time( + kDefaultAdisAccelerometerBiasCorrelationTime), + accelerometer_turn_on_bias_sigma( + kDefaultAdisAccelerometerTurnOnBiasSigma), + gravity_magnitude(kDefaultGravityMagnitude) {} +}; + +class GazeboImuPlugin : public ModelPlugin { + public: + + GazeboImuPlugin(); + ~GazeboImuPlugin(); + + void InitializeParams(); + void Publish(); + + protected: + void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + /// \brief This method adds noise to acceleration and angular rates for + /// accelerometer and gyroscope measurement simulation. + void AddNoise( + Eigen::Vector3d* linear_acceleration, + Eigen::Vector3d* angular_velocity, + const double dt); + + /// \brief This gets called by the world update start event. + /// \details Calculates IMU parameters and then publishes one IMU message. + void OnUpdate(const common::UpdateInfo&); + + private: + + /// \brief Flag that is set to true once CreatePubsAndSubs() is called, used + /// to prevent CreatePubsAndSubs() from be called on every OnUpdate(). + bool pubs_and_subs_created_; + + /// \brief Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. + /// \details Call this once the first time OnUpdate() is called (can't + /// be called from Load() because there is no guarantee GazeboRosInterfacePlugin has + /// has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages). + void CreatePubsAndSubs(); + + + std::string namespace_; + std::string imu_topic_; + + /// \brief Handle for the Gazebo node. + transport::NodePtr node_handle_; + + transport::PublisherPtr imu_pub_; + + std::string frame_id_; + std::string link_name_; + + std::default_random_engine random_generator_; + std::normal_distribution standard_normal_distribution_; + + /// \brief Pointer to the world. + physics::WorldPtr world_; + + /// \brief Pointer to the model. + physics::ModelPtr model_; + + /// \brief Pointer to the link. + physics::LinkPtr link_; + + /// \brief Pointer to the update event connection. + event::ConnectionPtr updateConnection_; + + common::Time last_time_; + + /// \brief IMU message. + /// \details This is modified everytime OnUpdate() is called, + // and then published onto a topic + gz_sensor_msgs::Imu imu_message_; + + ignition::math::Vector3d gravity_W_; + ignition::math::Vector3d velocity_prev_W_; + + Eigen::Vector3d gyroscope_bias_; + Eigen::Vector3d accelerometer_bias_; + + Eigen::Vector3d gyroscope_turn_on_bias_; + Eigen::Vector3d accelerometer_turn_on_bias_; + + ImuParameters imu_parameters_; +}; + +} // namespace gazebo + +#endif // ROTORS_GAZEBO_PLUGINS_IMU_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_magnetometer_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_magnetometer_plugin.h new file mode 100644 index 0000000..8a7d147 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_magnetometer_plugin.h @@ -0,0 +1,99 @@ +/* + * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ROTORS_GAZEBO_PLUGINS_MAGNETOMETER_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_MAGNETOMETER_PLUGIN_H + +#include + +#include +#include +#include +#include + +#include "MagneticField.pb.h" + +#include "rotors_gazebo_plugins/common.h" +#include "rotors_gazebo_plugins/sdf_api_wrapper.hpp" + +namespace gazebo { + +// Default magnetic field [Tesla] in NED frame, obtained from World Magnetic +// Model: (http://www.ngdc.noaa.gov/geomag-web/#igrfwmm) for Zurich: +// lat=+47.3667degN, lon=+8.5500degE, h=+500m, WGS84 +static constexpr double kDefaultRefMagNorth = 0.000021493; +static constexpr double kDefaultRefMagEast = 0.000000815; +static constexpr double kDefaultRefMagDown = 0.000042795; + +class GazeboMagnetometerPlugin : public ModelPlugin { + + public: + typedef std::normal_distribution<> NormalDistribution; + typedef std::uniform_real_distribution<> UniformDistribution; + + GazeboMagnetometerPlugin(); + virtual ~GazeboMagnetometerPlugin(); + + protected: + void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + void OnUpdate(const common::UpdateInfo&); + + private: + + /// \brief Flag that is set to true once CreatePubsAndSubs() is called, used + /// to prevent CreatePubsAndSubs() from be called on every OnUpdate(). + bool pubs_and_subs_created_; + + /// \brief Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. + /// \details Call this once the first time OnUpdate() is called (can't + /// be called from Load() because there is no guarantee GazeboRosInterfacePlugin has + /// has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages). + void CreatePubsAndSubs(); + + std::string namespace_; + std::string magnetometer_topic_; + gazebo::transport::NodePtr node_handle_; + gazebo::transport::PublisherPtr magnetometer_pub_; + std::string frame_id_; + + /// \brief Pointer to the world. + physics::WorldPtr world_; + + /// \brief Pointer to the model. + physics::ModelPtr model_; + + /// \brief Pointer to the link. + physics::LinkPtr link_; + + //// \brief Pointer to the update event connection. + event::ConnectionPtr updateConnection_; + + ignition::math::Vector3d mag_W_; + + /// \brief Magnetic field message. + /// \details Reused message object which is defined here to reduce + /// memory allocation. + gz_sensor_msgs::MagneticField mag_message_; + + NormalDistribution noise_n_[3]; + + std::random_device random_device_; + std::mt19937 random_generator_; +}; + +} // namespace gazebo + +#endif // ROTORS_GAZEBO_PLUGINS_MAGNETOMETER_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_mavlink_interface.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_mavlink_interface.h new file mode 100644 index 0000000..dd7abe9 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_mavlink_interface.h @@ -0,0 +1,297 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2015-2018 PX4 Pro Development Team + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include "common.h" +#include +#include +#include +#include + +#include +#include "msgbuffer.h" + +#include + +static const uint32_t kDefaultMavlinkUdpPort = 14560; +static const uint32_t kDefaultQGCUdpPort = 14550; + +using lock_guard = std::lock_guard; +static constexpr auto kDefaultDevice = "/dev/ttyACM0"; +static constexpr auto kDefaultBaudRate = 921600; + +//! Maximum buffer size with padding for CRC bytes (280 + padding) +static constexpr ssize_t MAX_SIZE = MAVLINK_MAX_PACKET_LEN + 16; +static constexpr size_t MAX_TXQ_SIZE = 1000; + +namespace gazebo { + +typedef const boost::shared_ptr CommandMotorSpeedPtr; +typedef const boost::shared_ptr ImuPtr; +typedef const boost::shared_ptr LidarPtr; +typedef const boost::shared_ptr OpticalFlowPtr; + +// Default values +// static const std::string kDefaultNamespace = ""; + +// This just proxies the motor commands from command/motor_speed to the single motors via internal +// ConsPtr passing, such that the original commands don't have to go n_motors-times over the wire. +static const std::string kDefaultMotorVelocityReferencePubTopic = "/gazebo/command/motor_speed"; + +static const std::string kDefaultImuTopic = "/imu"; +static const std::string kDefaultLidarTopic = "/link/lidar"; +static const std::string kDefaultOpticalFlowTopic = "/px4flow/link/opticalFlow"; + +//! Rx packer framing status. (same as @p mavlink::mavlink_framing_t) +enum class Framing : uint8_t { + incomplete = MAVLINK_FRAMING_INCOMPLETE, + ok = MAVLINK_FRAMING_OK, + bad_crc = MAVLINK_FRAMING_BAD_CRC, + bad_signature = MAVLINK_FRAMING_BAD_SIGNATURE, +}; + +class GazeboMavlinkInterface : public ModelPlugin { +public: + GazeboMavlinkInterface() : ModelPlugin(), + received_first_referenc_(false), + namespace_(kDefaultNamespace), + protocol_version_(2.0), + motor_velocity_reference_pub_topic_(kDefaultMotorVelocityReferencePubTopic), + use_propeller_pid_(false), + use_elevator_pid_(false), + use_left_elevon_pid_(false), + use_right_elevon_pid_(false), + imu_sub_topic_(kDefaultImuTopic), + opticalFlow_sub_topic_(kDefaultOpticalFlowTopic), + lidar_sub_topic_(kDefaultLidarTopic), + model_ {}, + world_(nullptr), + left_elevon_joint_(nullptr), + right_elevon_joint_(nullptr), + elevator_joint_(nullptr), + propeller_joint_(nullptr), + gimbal_yaw_joint_(nullptr), + gimbal_pitch_joint_(nullptr), + gimbal_roll_joint_(nullptr), + input_offset_ {}, + input_scaling_ {}, + zero_position_disarmed_ {}, + zero_position_armed_ {}, + input_index_ {}, + lat_rad_(0.0), + lon_rad_(0.0), + mavlink_udp_port_(kDefaultMavlinkUdpPort), + qgc_udp_port_(kDefaultMavlinkUdpPort), + serial_enabled_(false), + tx_q {}, + rx_buf {}, + m_status {}, + m_buffer {}, + io_service(), + serial_dev(io_service), + device_(kDefaultDevice), + baudrate_(kDefaultBaudRate), + hil_mode_(false), + hil_state_level_(false) + {} + + ~GazeboMavlinkInterface(); + + void Publish(); + +protected: + void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + void OnUpdate(const common::UpdateInfo& /*_info*/); + +private: + bool received_first_referenc_; + Eigen::VectorXd input_reference_; + + float protocol_version_; + + std::string namespace_; + std::string motor_velocity_reference_pub_topic_; + std::string mavlink_control_sub_topic_; + std::string link_name_; + + transport::NodePtr node_handle_; + transport::PublisherPtr motor_velocity_reference_pub_; + transport::SubscriberPtr mav_control_sub_; + + physics::ModelPtr model_; + physics::WorldPtr world_; + physics::JointPtr left_elevon_joint_; + physics::JointPtr right_elevon_joint_; + physics::JointPtr elevator_joint_; + physics::JointPtr propeller_joint_; + physics::JointPtr gimbal_yaw_joint_; + physics::JointPtr gimbal_pitch_joint_; + physics::JointPtr gimbal_roll_joint_; + common::PID propeller_pid_; + common::PID elevator_pid_; + common::PID left_elevon_pid_; + common::PID right_elevon_pid_; + bool use_propeller_pid_; + bool use_elevator_pid_; + bool use_left_elevon_pid_; + bool use_right_elevon_pid_; + + std::vector joints_; + std::vector pids_; + + /// \brief Pointer to the update event connection. + event::ConnectionPtr updateConnection_; + + boost::thread callback_queue_thread_; + void QueueThread(); + void ImuCallback(ImuPtr& imu_msg); + void LidarCallback(LidarPtr& lidar_msg); + void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg); + void send_mavlink_message(const mavlink_message_t *message, const int destination_port = 0); + void handle_message(mavlink_message_t *msg); + void pollForMAVLinkMessages(double _dt, uint32_t _timeoutMs); + + // Serial interface + void open(); + void close(); + void do_read(); + void parse_buffer(const boost::system::error_code& err, std::size_t bytes_t); + void do_write(bool check_tx_state); + inline bool is_open(){ + return serial_dev.is_open(); + } + + static const unsigned n_out_max = 18; + static const unsigned n_motors = 12; + static const unsigned n_servos = 6; + static const u_int32_t kMotorSpeedFlag = 1; + static const u_int32_t kServoPositionFlag = 2; + + double alt_home = 488.0; // meters + + unsigned _rotor_count; + + double input_offset_[n_out_max]; + double input_scaling_[n_out_max]; + std::string joint_control_type_[n_out_max]; + std::string gztopic_[n_out_max]; + double zero_position_disarmed_[n_out_max]; + double zero_position_armed_[n_out_max]; + int input_index_[n_out_max]; + transport::PublisherPtr joint_control_pub_[n_out_max]; + + transport::SubscriberPtr imu_sub_; + transport::SubscriberPtr lidar_sub_; + transport::SubscriberPtr sonar_sub_; + transport::SubscriberPtr opticalFlow_sub_; + + std::string imu_sub_topic_; + std::string lidar_sub_topic_; + std::string opticalFlow_sub_topic_; + + common::Time last_time_; + common::Time last_imu_time_; + common::Time last_actuator_time_; + double imu_update_interval_ = 0.004; + double lat_rad_; + double lon_rad_; + + void handle_control(double _dt); + + ignition::math::Vector3d gravity_W_; + ignition::math::Vector3d velocity_prev_W_; + ignition::math::Vector3d mag_d_; + + std::default_random_engine rand_; + std::normal_distribution randn_; + + int _fd; + struct sockaddr_in _myaddr; ///< The locally bound address + struct sockaddr_in _srcaddr; ///< SITL instance + socklen_t _addrlen; + unsigned char _buf[65535]; + struct pollfd fds[1]; + + struct sockaddr_in _srcaddr_2; ///< MAVROS + + //so we dont have to do extra callbacks + ignition::math::Vector3d optflow_gyro {}; + double optflow_distance; + double sonar_distance; + + in_addr_t mavlink_addr_; + int mavlink_udp_port_; + + in_addr_t qgc_addr_; + int qgc_udp_port_; + + // Serial interface + mavlink_status_t m_status; + mavlink_message_t m_buffer; + bool serial_enabled_; + std::thread io_thread; + std::string device_; + std::array rx_buf; + std::recursive_mutex mutex; + unsigned int baudrate_; + std::atomic tx_in_progress; + std::deque tx_q; + boost::asio::io_service io_service; + boost::asio::serial_port serial_dev; + + bool hil_mode_; + bool hil_state_level_; + + }; +} diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_motor_model.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_motor_model.h new file mode 100644 index 0000000..bf66010 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_motor_model.h @@ -0,0 +1,192 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ROTORS_GAZEBO_PLUGINS_MOTOR_MODELS_H +#define ROTORS_GAZEBO_PLUGINS_MOTOR_MODELS_H + +// SYSTEM +#include + +// 3RD PARTY +#include +#include +#include +#include +#include +#include +#include +#include // This comes from the mav_comm repo + +// USER +#include "rotors_gazebo_plugins/common.h" +#include "rotors_gazebo_plugins/motor_model.hpp" +#include "Float32.pb.h" +#include "CommandMotorSpeed.pb.h" +#include "WindSpeed.pb.h" + +namespace turning_direction { +const static int CCW = 1; +const static int CW = -1; +} // namespace turning_direction + +enum class MotorType { + kVelocity, + kPosition, + kForce +}; + +namespace gazebo { + +// Changed name from speed to input for more generality. TODO(kajabo): integrate general actuator command. +typedef const boost::shared_ptr GzCommandMotorInputMsgPtr; +typedef const boost::shared_ptr GzWindSpeedMsgPtr; + +// Set the max_force_ to the max double value. The limitations get handled by the FirstOrderFilter. +static constexpr double kDefaultMaxForce = std::numeric_limits::max(); +static constexpr double kDefaultMotorConstant = 8.54858e-06; +static constexpr double kDefaultMomentConstant = 0.016; +static constexpr double kDefaultTimeConstantUp = 1.0 / 80.0; +static constexpr double kDefaultTimeConstantDown = 1.0 / 40.0; +static constexpr double kDefaulMaxRotVelocity = 838.0; +static constexpr double kDefaultRotorDragCoefficient = 1.0e-4; +static constexpr double kDefaultRollingMomentCoefficient = 1.0e-6; + +class GazeboMotorModel : public MotorModel, public ModelPlugin { + + public: + GazeboMotorModel() + : ModelPlugin(), + MotorModel(), + command_sub_topic_(mav_msgs::default_topics::COMMAND_ACTUATORS), + wind_speed_sub_topic_(mav_msgs::default_topics::WIND_SPEED), + motor_speed_pub_topic_(mav_msgs::default_topics::MOTOR_MEASUREMENT), + motor_position_pub_topic_(mav_msgs::default_topics::MOTOR_POSITION_MEASUREMENT), + motor_force_pub_topic_(mav_msgs::default_topics::MOTOR_FORCE_MEASUREMENT), + publish_speed_(true), + publish_position_(false), + publish_force_(false), + motor_number_(0), + turning_direction_(turning_direction::CW), + motor_type_(MotorType::kVelocity), + max_force_(kDefaultMaxForce), + max_rot_velocity_(kDefaulMaxRotVelocity), + moment_constant_(kDefaultMomentConstant), + motor_constant_(kDefaultMotorConstant), + ref_motor_input_(0.0), + rolling_moment_coefficient_(kDefaultRollingMomentCoefficient), + rotor_drag_coefficient_(kDefaultRotorDragCoefficient), + rotor_velocity_slowdown_sim_(kDefaultRotorVelocitySlowdownSim), + time_constant_down_(kDefaultTimeConstantDown), + time_constant_up_(kDefaultTimeConstantUp), + node_handle_(nullptr), + wind_speed_W_(0, 0, 0), + pubs_and_subs_created_(false) {} + + virtual ~GazeboMotorModel(); + + virtual void InitializeParams(); + virtual void Publish(); + + protected: + virtual void UpdateForcesAndMoments(); + virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + virtual void OnUpdate(const common::UpdateInfo & /*_info*/); + + private: + + /// \brief Flag that is set to true once CreatePubsAndSubs() is called, used + /// to prevent CreatePubsAndSubs() from be called on every OnUpdate(). + bool pubs_and_subs_created_; + + /// \brief Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. + /// \details Call this once the first time OnUpdate() is called (can't + /// be called from Load() because there is no guarantee GazeboRosInterfacePlugin has + /// has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages). + void CreatePubsAndSubs(); + + std::string command_sub_topic_; + std::string wind_speed_sub_topic_; + std::string joint_name_; + std::string link_name_; + std::string motor_speed_pub_topic_; + std::string motor_position_pub_topic_; + std::string motor_force_pub_topic_; + std::string namespace_; + + bool publish_speed_; + bool publish_position_; + bool publish_force_; + + int motor_number_; + int turning_direction_; + MotorType motor_type_; + + double max_force_; + double max_rot_velocity_; + double moment_constant_; + double motor_constant_; + double ref_motor_input_; + double rolling_moment_coefficient_; + double rotor_drag_coefficient_; + double rotor_velocity_slowdown_sim_; + double time_constant_down_; + double time_constant_up_; + + common::PID pids_; + + gazebo::transport::NodePtr node_handle_; + + gazebo::transport::PublisherPtr motor_velocity_pub_; + + gazebo::transport::PublisherPtr motor_position_pub_; + + gazebo::transport::PublisherPtr motor_force_pub_; + + gazebo::transport::SubscriberPtr command_sub_; + + gazebo::transport::SubscriberPtr wind_speed_sub_; + + physics::ModelPtr model_; + physics::JointPtr joint_; + physics::LinkPtr link_; + + /// \brief Pointer to the update event connection. + event::ConnectionPtr updateConnection_; + + boost::thread callback_queue_thread_; + + void QueueThread(); + + gz_std_msgs::Float32 turning_velocity_msg_; + gz_std_msgs::Float32 position_msg_; + gz_std_msgs::Float32 force_msg_; + + void ControlCommandCallback(GzCommandMotorInputMsgPtr& command_motor_input_msg); + + void WindSpeedCallback(GzWindSpeedMsgPtr& wind_speed_msg); + + std::unique_ptr> rotor_velocity_filter_; + ignition::math::Vector3d wind_speed_W_; +}; + +} // namespace gazebo { + +#endif // ROTORS_GAZEBO_PLUGINS_MOTOR_MODELS_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_multirotor_base_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_multirotor_base_plugin.h new file mode 100644 index 0000000..ca1d406 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_multirotor_base_plugin.h @@ -0,0 +1,121 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_MULTIROTOR_BASE_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_GAZEBO_MULTIROTOR_BASE_PLUGIN_H + +#include + +#include +#include +#include +#include + +#include // This comes from the mav_comm repo + +#include "Actuators.pb.h" +#include "JointState.pb.h" + +#include "rotors_gazebo_plugins/common.h" + +namespace gazebo { + +// Default values +static const std::string kDefaultLinkName = "base_link"; +static const std::string kDefaultFrameId = "base_link"; +static const std::string kDefaultJointStatePubTopic = "joint_states"; + +/// \brief This plugin publishes the motor speeds of your multirotor model. +class GazeboMultirotorBasePlugin : public ModelPlugin { + typedef std::map MotorNumberToJointMap; + typedef std::pair MotorNumberToJointPair; + public: + GazeboMultirotorBasePlugin() + : ModelPlugin(), + namespace_(kDefaultNamespace), + joint_state_pub_topic_(kDefaultJointStatePubTopic), + actuators_pub_topic_(mav_msgs::default_topics::MOTOR_MEASUREMENT), + link_name_(kDefaultLinkName), + frame_id_(kDefaultFrameId), + rotor_velocity_slowdown_sim_(kDefaultRotorVelocitySlowdownSim), + node_handle_(NULL), + pubs_and_subs_created_(false) {} + + virtual ~GazeboMultirotorBasePlugin(); + + protected: + + /// \brief Load the plugin. + /// \param[in] _model Pointer to the model that loaded this plugin. + /// \param[in] _sdf SDF element that describes the plugin. + void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + /// \brief Called when the world is updated. + /// \param[in] _info Update timing information. + void OnUpdate(const common::UpdateInfo& /*_info*/); + + private: + + /// \brief Flag that is set to true once CreatePubsAndSubs() is called, used + /// to prevent CreatePubsAndSubs() from be called on every OnUpdate(). + bool pubs_and_subs_created_; + + /// \brief Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. + /// \details Call this once the first time OnUpdate() is called (can't + /// be called from Load() because there is no guarantee GazeboRosInterfacePlugin has + /// has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages). + void CreatePubsAndSubs(); + + /// \brief Pointer to the update event connection. + event::ConnectionPtr update_connection_; + + physics::WorldPtr world_; + physics::ModelPtr model_; + physics::LinkPtr link_; + + physics::Link_V child_links_; + + MotorNumberToJointMap motor_joints_; + + std::string namespace_; + std::string joint_state_pub_topic_; + std::string actuators_pub_topic_; + std::string link_name_; + std::string frame_id_; + double rotor_velocity_slowdown_sim_; + + gazebo::transport::PublisherPtr motor_pub_; + + /// \details Re-used message object, defined here to reduce dynamic memory allocation. + gz_sensor_msgs::Actuators actuators_msg_; + + gazebo::transport::PublisherPtr joint_state_pub_; + + /// \details Re-used message object, defined here to reduce dynamic memory allocation. + gz_sensor_msgs::JointState joint_state_msg_; + + gazebo::transport::NodePtr node_handle_; +}; + +} // namespace gazebo + +#endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_MULTIROTOR_BASE_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_noisydepth_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_noisydepth_plugin.h new file mode 100644 index 0000000..2d37ab5 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_noisydepth_plugin.h @@ -0,0 +1,112 @@ +/* + * Copyright 2018 Michael Pantic, ASL, ETH Zurich, Switzerland + * + * Forked from Openni/Kinect Depth Plugin, retaining original copyright header: + * + * Copyright (C) 2012-2014 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +/* + * Desc: A dynamic controller plugin that publishes ROS image_raw camera_info + * topic for generic camera sensor. + * Author: John Hsu + * Date: 24 Sept 2008 + */ + +#ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_NOISYDEPTH_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_GAZEBO_NOISYDEPTH_PLUGIN_H + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +namespace gazebo { +class GazeboNoisyDepth : public DepthCameraPlugin, GazeboRosCameraUtils { + public: + GazeboNoisyDepth(); + + ~GazeboNoisyDepth(); + + /// \brief Load the plugin + /// \param take in SDF root element + virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Advertise depth image + virtual void Advertise(); + + virtual void OnNewDepthFrame(const float *_image, unsigned int _width, + unsigned int _height, unsigned int _depth, + const std::string &_format); + + protected: + virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, + unsigned int _height, unsigned int _depth, + const std::string &_format); + + private: + void DepthImageConnect(); + void DepthImageDisconnect(); + void DepthInfoConnect(); + void DepthInfoDisconnect(); + void FillDepthImage(const float *_src); + bool FillDepthImageHelper(const uint32_t rows_arg, + const uint32_t cols_arg, + const uint32_t step_arg, + const float *data_arg, + sensor_msgs::Image *image_msg); + + virtual void PublishCameraInfo(); + + event::ConnectionPtr load_connection_; + std::unique_ptr noise_model; + + ros::Publisher depth_image_pub_; + ros::Publisher depth_image_camera_info_pub_; + + int depth_image_connect_count_; + int depth_info_connect_count_; + + std::string depth_image_topic_name_; + std::string depth_image_camera_info_topic_name_; + + common::Time depth_sensor_update_time_; + common::Time last_depth_image_camera_info_update_time_; + sensor_msgs::Image depth_image_msg_; +}; +} +#endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_NOISYDEPTH_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_octomap_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_octomap_plugin.h new file mode 100644 index 0000000..b2011b4 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_octomap_plugin.h @@ -0,0 +1,95 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_OCTOMAP_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_GAZEBO_OCTOMAP_PLUGIN_H + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo { + +/// \brief Octomap plugin for Gazebo. +/// \details This plugin is dependent on ROS, and is not built if NO_ROS=TRUE is provided to +/// CMakeLists.txt. The PX4/Firmware build does not build this file. +class OctomapFromGazeboWorld : public WorldPlugin { + public: + OctomapFromGazeboWorld() + : WorldPlugin(), node_handle_(kDefaultNamespace), octomap_(NULL) {} + virtual ~OctomapFromGazeboWorld(); + + protected: + + /// \brief Load the plugin. + /// \param[in] _parent Pointer to the world that loaded this plugin. + /// \param[in] _sdf SDF element that describes the plugin. + void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf); + + bool CheckIfInterest(const ignition::math::Vector3d & central_point, + gazebo::physics::RayShapePtr ray, + const double leaf_size); + + void FloodFill(const ignition::math::Vector3d & seed_point, + const ignition::math::Vector3d & bounding_box_origin, + const ignition::math::Vector3d & bounding_box_lengths, + const double leaf_size); + + /*! \brief Creates octomap by floodfilling freespace. + * + * Creates an octomap of the environment in 3 steps: + * -# Casts rays along the central X,Y and Z axis of each cell. Marks any + * cell where a ray intersects a mesh as occupied + * -# Floodfills the area from the top and bottom marking all connected + * space that has not been set to occupied as free. + * -# Labels all remaining unknown space as occupied. + * + * Can give incorrect results in the following situations: + * -# The top central cell or bottom central cell are either occupied or + * completely enclosed by occupied cells. + * -# A completely enclosed hollow space will be marked as occupied. + * -# Cells containing a mesh that does not intersect its central axes will + * be marked as unoccupied + */ + void CreateOctomap(const rotors_comm::Octomap::Request& msg); + + private: + physics::WorldPtr world_; + ros::NodeHandle node_handle_; + ros::ServiceServer srv_; + octomap::OcTree* octomap_; + ros::Publisher octomap_publisher_; + bool ServiceCallback(rotors_comm::Octomap::Request& req, + rotors_comm::Octomap::Response& res); +}; + +} // namespace gazebo + +#endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_OCTOMAP_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_odometry_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_odometry_plugin.h new file mode 100644 index 0000000..78461d6 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_odometry_plugin.h @@ -0,0 +1,173 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_ODOMETRY_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_GAZEBO_ODOMETRY_PLUGIN_H + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include // This comes from the mav_comm repo + +#include "rotors_gazebo_plugins/common.h" +#include "rotors_gazebo_plugins/sdf_api_wrapper.hpp" + +#include "Odometry.pb.h" + + +namespace gazebo { + +// Default values +static const std::string kDefaultParentFrameId = "world"; +static const std::string kDefaultChildFrameId = "odometry_sensor"; +static const std::string kDefaultLinkName = "odometry_sensor_link"; + +static constexpr int kDefaultMeasurementDelay = 0; +static constexpr int kDefaultMeasurementDivisor = 1; +static constexpr int kDefaultGazeboSequence = 0; +static constexpr int kDefaultOdometrySequence = 0; +static constexpr double kDefaultUnknownDelay = 0.0; +static constexpr double kDefaultCovarianceImageScale = 1.0; + +class GazeboOdometryPlugin : public ModelPlugin { + public: + typedef std::normal_distribution<> NormalDistribution; + typedef std::uniform_real_distribution<> UniformDistribution; + typedef std::deque > OdometryQueue; + typedef boost::array CovarianceMatrix; + + GazeboOdometryPlugin() + : ModelPlugin(), + random_generator_(random_device_()), + // DEFAULT TOPICS + pose_pub_topic_(mav_msgs::default_topics::POSE), + pose_with_covariance_stamped_pub_topic_(mav_msgs::default_topics::POSE_WITH_COVARIANCE), + position_stamped_pub_topic_(mav_msgs::default_topics::POSITION), + transform_stamped_pub_topic_(mav_msgs::default_topics::TRANSFORM), + odometry_pub_topic_(mav_msgs::default_topics::ODOMETRY), + //--------------- + parent_frame_id_(kDefaultParentFrameId), + child_frame_id_(kDefaultChildFrameId), + link_name_(kDefaultLinkName), + measurement_delay_(kDefaultMeasurementDelay), + measurement_divisor_(kDefaultMeasurementDivisor), + unknown_delay_(kDefaultUnknownDelay), + gazebo_sequence_(kDefaultGazeboSequence), + odometry_sequence_(kDefaultOdometrySequence), + covariance_image_scale_(kDefaultCovarianceImageScale), + pubs_and_subs_created_(false) {} + + ~GazeboOdometryPlugin(); + + void InitializeParams(); + void Publish(); + + protected: + void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + void OnUpdate(const common::UpdateInfo& /*_info*/); + + private: + + /// \brief Flag that is set to true once CreatePubsAndSubs() is called, used + /// to prevent CreatePubsAndSubs() from be called on every OnUpdate(). + bool pubs_and_subs_created_; + + /// \brief Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. + /// \details Call this once the first time OnUpdate() is called (can't + /// be called from Load() because there is no guarantee GazeboRosInterfacePlugin has + /// has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages). + void CreatePubsAndSubs(); + + OdometryQueue odometry_queue_; + + std::string namespace_; + std::string pose_pub_topic_; + std::string pose_with_covariance_stamped_pub_topic_; + std::string position_stamped_pub_topic_; + std::string transform_stamped_pub_topic_; + std::string odometry_pub_topic_; + std::string parent_frame_id_; + std::string child_frame_id_; + std::string link_name_; + + NormalDistribution position_n_[3]; + NormalDistribution attitude_n_[3]; + NormalDistribution linear_velocity_n_[3]; + NormalDistribution angular_velocity_n_[3]; + UniformDistribution position_u_[3]; + UniformDistribution attitude_u_[3]; + UniformDistribution linear_velocity_u_[3]; + UniformDistribution angular_velocity_u_[3]; + + CovarianceMatrix pose_covariance_matrix_; + CovarianceMatrix twist_covariance_matrix_; + + int measurement_delay_; + int measurement_divisor_; + int gazebo_sequence_; + int odometry_sequence_; + double unknown_delay_; + double covariance_image_scale_; + cv::Mat covariance_image_; + + std::random_device random_device_; + std::mt19937 random_generator_; + + gazebo::transport::NodePtr node_handle_; + + gazebo::transport::PublisherPtr pose_pub_; + gazebo::transport::PublisherPtr pose_with_covariance_stamped_pub_; + gazebo::transport::PublisherPtr position_stamped_pub_; + gazebo::transport::PublisherPtr transform_stamped_pub_; + gazebo::transport::PublisherPtr odometry_pub_; + + /// \brief Special-case publisher to publish stamped transforms with + /// frame IDs. The ROS interface plugin (if present) will + /// listen to this publisher and broadcast the transform + /// using transform_broadcast(). + gazebo::transport::PublisherPtr broadcast_transform_pub_; + + physics::WorldPtr world_; + physics::ModelPtr model_; + physics::LinkPtr link_; + physics::EntityPtr parent_link_; + + /// \brief Pointer to the update event connection. + event::ConnectionPtr updateConnection_; + + boost::thread callback_queue_thread_; + void QueueThread(); +}; + +} // namespace gazebo + +#endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_ODOMETRY_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_pressure_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_pressure_plugin.h new file mode 100644 index 0000000..3dda35c --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_pressure_plugin.h @@ -0,0 +1,125 @@ +/* + * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ROTORS_GAZEBO_PLUGINS_PRESSURE_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_PRESSURE_PLUGIN_H + +#include + +#include + +#include +#include +#include +#include + +#include "FluidPressure.pb.h" + +#include "rotors_gazebo_plugins/common.h" + +namespace gazebo { +// Constants +static constexpr double kGasConstantNmPerKmolKelvin = 8314.32; +static constexpr double kMeanMolecularAirWeightKgPerKmol = 28.9644; +static constexpr double kGravityMagnitude = 9.80665; +static constexpr double kEarthRadiusMeters = 6356766.0; +static constexpr double kPressureOneAtmospherePascals = 101325.0; +static constexpr double kSeaLevelTempKelvin = 288.15; +static constexpr double kTempLapseKelvinPerMeter = 0.0065; +static constexpr double kAirConstantDimensionless = kGravityMagnitude * + kMeanMolecularAirWeightKgPerKmol / + (kGasConstantNmPerKmolKelvin * -kTempLapseKelvinPerMeter); + +// Default values +static const std::string kDefaultPressurePubTopic = "air_pressure"; +static constexpr double kDefaultRefAlt = 500.0; /* m, Zurich: h=+500m, WGS84) */ +static constexpr double kDefaultPressureVar = 0.0; /* Pa^2, pressure variance */ + +class GazeboPressurePlugin : public ModelPlugin { + public: + /// \brief Constructor. + GazeboPressurePlugin(); + + /// \brief Destructor. + virtual ~GazeboPressurePlugin(); + + typedef std::normal_distribution<> NormalDistribution; + + protected: + /// \brief Called when the plugin is first created, and after the world + /// has been loaded. This function should not be blocking. + void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + /// \brief This gets called by the world update start event. + void OnUpdate(const common::UpdateInfo&); + + private: + /// \brief Flag that is set to true once CreatePubsAndSubs() is called, used + /// to prevent CreatePubsAndSubs() from be called on every OnUpdate(). + bool pubs_and_subs_created_; + + /// \brief Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. + /// \details Call this once the first time OnUpdate() is called (can't + /// be called from Load() because there is no guarantee GazeboRosInterfacePlugin has + /// has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages). + void CreatePubsAndSubs(); + + /// \brief Handle for the Gazebo node. + gazebo::transport::NodePtr node_handle_; + + /// \brief Pressure message publisher. + gazebo::transport::PublisherPtr pressure_pub_; + + /// \brief Transport namespace. + std::string namespace_; + + /// \brief Topic name for pressure messages. + std::string pressure_topic_; + + /// \brief Frame ID for pressure messages. + std::string frame_id_; + + /// \brief Pointer to the world. + physics::WorldPtr world_; + + /// \brief Pointer to the model. + physics::ModelPtr model_; + + /// \brief Pointer to the link. + physics::LinkPtr link_; + + /// \brief Pointer to the update event connection. + event::ConnectionPtr updateConnection_; + + /// \brief Reference altitude (meters). + double ref_alt_; + + /// \brief Pressure measurement variance (Pa^2). + double pressure_var_; + + /// \brief Normal distribution for pressure noise. + NormalDistribution pressure_n_[1]; + + /// \brief Fluid pressure message. + /// \details This is modified everytime OnUpdate() is called, + // and then published onto a topic + gz_sensor_msgs::FluidPressure pressure_message_; + + std::mt19937 random_generator_; +}; +} + +#endif // ROTORS_GAZEBO_PLUGINS_PRESSURE_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_ros_interface_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_ros_interface_plugin.h new file mode 100644 index 0000000..29e292f --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_ros_interface_plugin.h @@ -0,0 +1,336 @@ +/* + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H + +// SYSTEM INCLUDES +#include + +#include +#include +#include +#include +#include +#include "gazebo/msgs/msgs.hh" + +//=================== ROS =====================// +#include +#include +#include +#include + +//============= GAZEBO MSG TYPES ==============// +#include "ConnectGazeboToRosTopic.pb.h" +#include "ConnectRosToGazeboTopic.pb.h" + +#include "Actuators.pb.h" +#include "CommandMotorSpeed.pb.h" +#include "Float32.pb.h" +#include "FluidPressure.pb.h" +#include "Imu.pb.h" +#include "JointState.pb.h" +#include "MagneticField.pb.h" +#include "NavSatFix.pb.h" +#include "Odometry.pb.h" +#include "PoseWithCovarianceStamped.pb.h" +#include "RollPitchYawrateThrust.pb.h" +#include "TransformStamped.pb.h" +#include "TransformStampedWithFrameIds.pb.h" +#include "TwistStamped.pb.h" +#include "Vector3dStamped.pb.h" +#include "WindSpeed.pb.h" +#include "WrenchStamped.pb.h" + +//=============== ROS MSG TYPES ===============// +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rotors_gazebo_plugins/common.h" + +namespace gazebo { + +// typedef's to make life easier +typedef const boost::shared_ptr + GzConnectGazeboToRosTopicMsgPtr; +typedef const boost::shared_ptr + GzConnectRosToGazeboTopicMsgPtr; +typedef const boost::shared_ptr GzFloat32MsgPtr; +typedef const boost::shared_ptr + GzOdometryMsgPtr; +typedef const boost::shared_ptr GzPoseMsgPtr; +typedef const boost::shared_ptr< + const gz_geometry_msgs::PoseWithCovarianceStamped> + GzPoseWithCovarianceStampedMsgPtr; +typedef const boost::shared_ptr + GzTransformStampedMsgPtr; +typedef const boost::shared_ptr< + const gz_geometry_msgs::TransformStampedWithFrameIds> + GzTransformStampedWithFrameIdsMsgPtr; +typedef const boost::shared_ptr + GzTwistStampedMsgPtr; +typedef const boost::shared_ptr + GzVector3dStampedMsgPtr; +typedef const boost::shared_ptr + GzWrenchStampedMsgPtr; +typedef const boost::shared_ptr + GzRollPitchYawrateThrustPtr; +typedef const boost::shared_ptr GzWindSpeedMsgPtr; +typedef const boost::shared_ptr + GzActuatorsMsgPtr; +typedef const boost::shared_ptr + GzFluidPressureMsgPtr; +typedef const boost::shared_ptr GzImuPtr; +typedef const boost::shared_ptr + GzJointStateMsgPtr; +typedef const boost::shared_ptr + GzMagneticFieldMsgPtr; +typedef const boost::shared_ptr GzNavSatFixPtr; + +/// \brief ROS interface plugin for Gazebo. +/// \details This routes messages to/from Gazebo and ROS. This is used +/// so that individual plugins are not ROS dependent. +/// This is a WorldPlugin, only one of these is designed to be enabled +/// per Gazebo world. +class GazeboRosInterfacePlugin : public WorldPlugin { + public: + GazeboRosInterfacePlugin(); + ~GazeboRosInterfacePlugin(); + + void InitializeParams(); + void Publish(); + + protected: + void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf); + + /// \brief This gets called by the world update start event. + /// \details Calculates IMU parameters and then publishes one IMU message. + void OnUpdate(const common::UpdateInfo&); + + private: + /// \brief Provides a way for GzConnectGazeboToRosTopicMsgCallback() to + /// connect a Gazebo subscriber to a ROS publisher. + /// \details + /// GazeboMsgT The type of the message that will be subscribed to the + /// Gazebo framework. + /// RosMsgT The type of the message published to the ROS framework. + template + void ConnectHelper(void (GazeboRosInterfacePlugin::*fp)( + const boost::shared_ptr&, + ros::Publisher), + GazeboRosInterfacePlugin* ptr, std::string gazeboNamespace, + std::string gazeboTopicName, std::string rosTopicName, + transport::NodePtr gz_node_handle); + + std::vector nodePtrs_; + std::vector subscriberPtrs_; + + // std::string namespace_; + + /// \brief Handle for the Gazebo node. + transport::NodePtr gz_node_handle_; + + /// \brief Handle for the ROS node. + ros::NodeHandle* ros_node_handle_; + + /// \brief Pointer to the world. + physics::WorldPtr world_; + + /// \brief Pointer to the update event connection. + event::ConnectionPtr updateConnection_; + + // ============================================ // + // ====== CONNECT GAZEBO TO ROS MESSAGES ====== // + // ============================================ // + + transport::SubscriberPtr gz_connect_gazebo_to_ros_topic_sub_; + void GzConnectGazeboToRosTopicMsgCallback( + GzConnectGazeboToRosTopicMsgPtr& gz_connect_gazebo_to_ros_topic_msg); + + // ============================================ // + // ====== CONNECT ROS TO GAZEBO MESSAGES ====== // + // ============================================ // + + transport::SubscriberPtr gz_connect_ros_to_gazebo_topic_sub_; + + /// @brief Subscribes to the provided ROS topic and publishes on the + /// provided Gazebo topic (all info contained within the message). + /// @details Will create a Gazebo publisher if one doesn't already exist. + void GzConnectRosToGazeboTopicMsgCallback( + GzConnectRosToGazeboTopicMsgPtr& gz_connect_ros_to_gazebo_topic_msg); + + /// \brief Looks if a publisher on the provided topic already exists, and + /// returns it. + /// If no publisher exists, this method creates one and returns + /// that instead. + /// \warning Finding an already created publisher is not supported yet! + template + transport::PublisherPtr FindOrMakeGazeboPublisher(std::string topic); + + // ============================================ // + // ===== HELPER METHODS FOR MSG CONVERSION ==== // + // ============================================ // + + void ConvertHeaderGzToRos( + const gz_std_msgs::Header& gz_header, + std_msgs::Header_ >* ros_header); + + void ConvertHeaderRosToGz( + const std_msgs::Header_ >& ros_header, + gz_std_msgs::Header* gz_header); + + // ============================================ // + // ===== GAZEBO->ROS CALLBACKS/CONVERTERS ===== // + // ============================================ // + + // ACTUATORS + void GzActuatorsMsgCallback(GzActuatorsMsgPtr& gz_actuators_msg, + ros::Publisher ros_publisher); + mav_msgs::Actuators ros_actuators_msg_; + + // FLOAT32 + void GzFloat32MsgCallback(GzFloat32MsgPtr& gz_float_32_msg, + ros::Publisher ros_publisher); + std_msgs::Float32 ros_float_32_msg_; + + // FLUID PRESSURE + void GzFluidPressureMsgCallback(GzFluidPressureMsgPtr& gz_fluid_pressure_msg, + ros::Publisher ros_publisher); + sensor_msgs::FluidPressure ros_fluid_pressure_msg_; + + // IMU + void GzImuMsgCallback(GzImuPtr& gz_imu_msg, ros::Publisher ros_publisher); + sensor_msgs::Imu ros_imu_msg_; + + // JOINT STATE + void GzJointStateMsgCallback(GzJointStateMsgPtr& gz_joint_state_msg, + ros::Publisher ros_publisher); + sensor_msgs::JointState ros_joint_state_msg_; + + // MAGNETIC FIELD + void GzMagneticFieldMsgCallback(GzMagneticFieldMsgPtr& gz_magnetic_field_msg, + ros::Publisher ros_publisher); + sensor_msgs::MagneticField ros_magnetic_field_msg_; + + // NAT SAT FIX (GPS) + void GzNavSatFixCallback(GzNavSatFixPtr& gz_nav_sat_fix_msg, + ros::Publisher ros_publisher); + sensor_msgs::NavSatFix ros_nav_sat_fix_msg_; + + // ODOMETRY + void GzOdometryMsgCallback(GzOdometryMsgPtr& gz_odometry_msg, + ros::Publisher ros_publisher); + nav_msgs::Odometry ros_odometry_msg_; + + // POSE + void GzPoseMsgCallback(GzPoseMsgPtr& gz_pose_msg, + ros::Publisher ros_publisher); + geometry_msgs::Pose ros_pose_msg_; + + // POSE WITH COVARIANCE STAMPED + void GzPoseWithCovarianceStampedMsgCallback( + GzPoseWithCovarianceStampedMsgPtr& gz_pose_with_covariance_stamped_msg, + ros::Publisher ros_publisher); + geometry_msgs::PoseWithCovarianceStamped + ros_pose_with_covariance_stamped_msg_; + + // POSITION STAMPED + void GzVector3dStampedMsgCallback( + GzVector3dStampedMsgPtr& gz_vector_3d_stamped_msg, + ros::Publisher ros_publisher); + geometry_msgs::PointStamped ros_position_stamped_msg_; + + // TRANSFORM STAMPED + void GzTransformStampedMsgCallback( + GzTransformStampedMsgPtr& gz_transform_stamped_msg, + ros::Publisher ros_publisher); + geometry_msgs::TransformStamped ros_transform_stamped_msg_; + + // TWIST STAMPED + void GzTwistStampedMsgCallback(GzTwistStampedMsgPtr& gz_twist_stamped_msg, + ros::Publisher ros_publisher); + geometry_msgs::TwistStamped ros_twist_stamped_msg_; + + // WIND SPEED + void GzWindSpeedMsgCallback(GzWindSpeedMsgPtr& gz_wind_speed_msg, + ros::Publisher ros_publisher); + rotors_comm::WindSpeed ros_wind_speed_msg_; + + // WRENCH STAMPED + void GzWrenchStampedMsgCallback(GzWrenchStampedMsgPtr& gz_wrench_stamped_msg, + ros::Publisher ros_publisher); + geometry_msgs::WrenchStamped ros_wrench_stamped_msg_; + + // ============================================ // + // ===== ROS->GAZEBO CALLBACKS/CONVERTERS ===== // + // ============================================ // + + // ACTUATORS (change name??? motor control? motor speed?) + void RosActuatorsMsgCallback( + const mav_msgs::ActuatorsConstPtr& ros_actuators_msg_ptr, + gazebo::transport::PublisherPtr gz_publisher_ptr); + + // COMMAND MOTOR SPEED (this is the same as ACTUATORS!, merge???) + void RosCommandMotorSpeedMsgCallback( + const mav_msgs::ActuatorsConstPtr& ros_command_motor_speed_msg_ptr, + gazebo::transport::PublisherPtr gz_publisher_ptr); + + // ROLL PITCH YAWRATE THRUST + void RosRollPitchYawrateThrustMsgCallback( + const mav_msgs::RollPitchYawrateThrustConstPtr& + ros_roll_pitch_yawrate_thrust_msg_ptr, + gazebo::transport::PublisherPtr gz_publisher_ptr); + + // WIND SPEED + void RosWindSpeedMsgCallback( + const rotors_comm::WindSpeedConstPtr& ros_wind_speed_msg_ptr, + gazebo::transport::PublisherPtr gz_publisher_ptr); + + // ============================================ // + // ====== TRANSFORM BROADCASTER RELATED ======= // + // ============================================ // + + transport::SubscriberPtr gz_broadcast_transform_sub_; + + /// \brief This is a special-case callback which listens for Gazebo + /// "Transform" messages. Upon receiving one it + /// broadcasts the transform on the ROS system (using + /// transform_broadcast()). + void GzBroadcastTransformMsgCallback( + GzTransformStampedWithFrameIdsMsgPtr& broadcast_transform_msg); + + tf::Transform tf_; + tf::TransformBroadcaster transform_broadcaster_; +}; + +} // namespace gazebo + +#endif // #ifndef ROTORS_GAZEBO_PLUGINS_MSG_INTERFACE_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_plugin.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_plugin.h new file mode 100644 index 0000000..85bd8a5 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/gazebo_wind_plugin.h @@ -0,0 +1,209 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_PLUGIN_H +#define ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_PLUGIN_H + +#include + +#include +#include +#include +#include + +#include // This comes from the mav_comm repo + +#include "rotors_gazebo_plugins/common.h" + +#include "WindSpeed.pb.h" // Wind speed message +#include "WrenchStamped.pb.h" // Wind force message + +namespace gazebo { +// Default values +static const std::string kDefaultFrameId = "world"; +static const std::string kDefaultLinkName = "base_link"; +static const std::string kDefaultWindSpeedPubTopic = "wind_speed"; + +static constexpr double kDefaultWindForceMean = 0.0; +static constexpr double kDefaultWindForceVariance = 0.0; +static constexpr double kDefaultWindGustForceMean = 0.0; +static constexpr double kDefaultWindGustForceVariance = 0.0; + +static constexpr double kDefaultWindGustStart = 10.0; +static constexpr double kDefaultWindGustDuration = 0.0; + +static constexpr double kDefaultWindSpeedMean = 0.0; +static constexpr double kDefaultWindSpeedVariance = 0.0; + +static const ignition::math::Vector3d kDefaultWindDirection = ignition::math::Vector3d (1, 0, 0); +static const ignition::math::Vector3d kDefaultWindGustDirection = ignition::math::Vector3d (0, 1, 0); + +static constexpr bool kDefaultUseCustomStaticWindField = false; + + + +/// \brief This gazebo plugin simulates wind acting on a model. +/// \details This plugin publishes on a Gazebo topic and instructs the ROS interface plugin to +/// forward the message onto ROS. +class GazeboWindPlugin : public ModelPlugin { + public: + GazeboWindPlugin() + : ModelPlugin(), + namespace_(kDefaultNamespace), + wind_force_pub_topic_(mav_msgs::default_topics::EXTERNAL_FORCE), + wind_speed_pub_topic_(mav_msgs::default_topics::WIND_SPEED), + wind_force_mean_(kDefaultWindForceMean), + wind_force_variance_(kDefaultWindForceVariance), + wind_gust_force_mean_(kDefaultWindGustForceMean), + wind_gust_force_variance_(kDefaultWindGustForceVariance), + wind_speed_mean_(kDefaultWindSpeedMean), + wind_speed_variance_(kDefaultWindSpeedVariance), + wind_direction_(kDefaultWindDirection), + wind_gust_direction_(kDefaultWindGustDirection), + use_custom_static_wind_field_(kDefaultUseCustomStaticWindField), + frame_id_(kDefaultFrameId), + link_name_(kDefaultLinkName), + node_handle_(nullptr), + pubs_and_subs_created_(false) {} + + virtual ~GazeboWindPlugin(); + + protected: + + /// \brief Load the plugin. + /// \param[in] _model Pointer to the model that loaded this plugin. + /// \param[in] _sdf SDF element that describes the plugin. + void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + /// \brief Called when the world is updated. + /// \param[in] _info Update timing information. + void OnUpdate(const common::UpdateInfo& /*_info*/); + + private: + + /// \brief Flag that is set to true once CreatePubsAndSubs() is called, used + /// to prevent CreatePubsAndSubs() from be called on every OnUpdate(). + bool pubs_and_subs_created_; + + /// \brief Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. + /// \details Call this once the first time OnUpdate() is called (can't + /// be called from Load() because there is no guarantee GazeboRosInterfacePlugin has + /// has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages). + void CreatePubsAndSubs(); + + /// \brief Pointer to the update event connection. + event::ConnectionPtr update_connection_; + + physics::WorldPtr world_; + physics::ModelPtr model_; + physics::LinkPtr link_; + + std::string namespace_; + + std::string frame_id_; + std::string link_name_; + std::string wind_force_pub_topic_; + std::string wind_speed_pub_topic_; + + double wind_force_mean_; + double wind_force_variance_; + double wind_gust_force_mean_; + double wind_gust_force_variance_; + double wind_speed_mean_; + double wind_speed_variance_; + + ignition::math::Vector3d xyz_offset_; + ignition::math::Vector3d wind_direction_; + ignition::math::Vector3d wind_gust_direction_; + + common::Time wind_gust_end_; + common::Time wind_gust_start_; + + /// \brief Variables for custom wind field generation. + bool use_custom_static_wind_field_; + float min_x_; + float min_y_; + int n_x_; + int n_y_; + float res_x_; + float res_y_; + std::vector vertical_spacing_factors_; + std::vector bottom_z_; + std::vector top_z_; + std::vector u_; + std::vector v_; + std::vector w_; + + /// \brief Reads wind data from a text file and saves it. + /// \param[in] custom_wind_field_path Path to the wind field from ~/.ros. + void ReadCustomWindField(std::string& custom_wind_field_path); + + /// \brief Functions for trilinear interpolation of wind field at aircraft position. + + /// \brief Linear interpolation + /// \param[in] position y-coordinate of the target point. + /// values Pointer to an array of size 2 containing the wind values + /// of the two points to interpolate from (12 and 13). + /// points Pointer to an array of size 2 containing the y-coordinate + /// of the two points to interpolate from. + ignition::math::Vector3d LinearInterpolation(double position, ignition::math::Vector3d * values, double* points) const; + + /// \brief Bilinear interpolation + /// \param[in] position Pointer to an array of size 2 containing the x- and + /// y-coordinates of the target point. + /// values Pointer to an array of size 4 containing the wind values + /// of the four points to interpolate from (8, 9, 10 and 11). + /// points Pointer to an array of size 14 containing the z-coordinate + /// of the eight points to interpolate from, the x-coordinate + /// of the four intermediate points (8, 9, 10 and 11), and the + /// y-coordinate of the last two intermediate points (12 and 13). + ignition::math::Vector3d BilinearInterpolation(double* position, ignition::math::Vector3d * values, double* points) const; + + /// \brief Trilinear interpolation + /// \param[in] link_position Vector3 containing the x, y and z-coordinates + /// of the target point. + /// values Pointer to an array of size 8 containing the wind values of the + /// eight points to interpolate from (0, 1, 2, 3, 4, 5, 6 and 7). + /// points Pointer to an array of size 14 containing the z-coordinate + /// of the eight points to interpolate from, the x-coordinate + /// of the four intermediate points (8, 9, 10 and 11), and the + /// y-coordinate of the last two intermediate points (12 and 13). + ignition::math::Vector3d TrilinearInterpolation(ignition::math::Vector3d link_position, ignition::math::Vector3d * values, double* points) const; + + gazebo::transport::PublisherPtr wind_force_pub_; + gazebo::transport::PublisherPtr wind_speed_pub_; + + gazebo::transport::NodePtr node_handle_; + + /// \brief Gazebo message for sending wind data. + /// \details This is defined at the class scope so that it is re-created + /// everytime a wind message needs to be sent, increasing performance. + gz_geometry_msgs::WrenchStamped wrench_stamped_msg_; + + /// \brief Gazebo message for sending wind speed data. + /// \details This is defined at the class scope so that it is re-created + /// everytime a wind speed message needs to be sent, increasing performance. + gz_mav_msgs::WindSpeed wind_speed_msg_; +}; +} + +#endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_PLUGIN_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/geo_mag_declination.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/geo_mag_declination.h new file mode 100644 index 0000000..c799e05 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/geo_mag_declination.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name MAVGEO nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file geo_mag_declination.h +* +* Calculation / lookup table for earth magnetic field declination. +* +*/ + +#pragma once + +__BEGIN_DECLS + +float get_mag_declination(float lat, float lon); + +__END_DECLS diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/motor_controller.hpp b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/motor_controller.hpp new file mode 100644 index 0000000..49b6557 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/motor_controller.hpp @@ -0,0 +1,53 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ROTORS_GAZEBO_PLUGIN_MOTOR_CONTROLLER_H +#define ROTORS_GAZEBO_PLUGIN_MOTOR_CONTROLLER_H + +#include + +class MotorController +{ + public: + MotorController(int amount_motors) : + ref_rotor_rot_vels_(Eigen::VectorXd::Zero(amount_motors)){}; + virtual ~MotorController(); + void getMotorVelocities() { + calculateRefMotorVelocities(); + return ref_rotor_rot_vels_; + } + + virtual void calculateRefMotorVelocities() = 0; + virtual void initializeParams() = 0; + virtual void publish() = 0; + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + protected: + // imu_ + // odom_ + + Eigen::Vector3d position_; + Eigen::Vector3d velocity_; + Eigen::Quaternion attitude_; + Eigen::VectorXd ref_rotor_rot_vels_; +}; + +#endif // ROTORS_GAZEBO_PLUGIN_MOTOR_CONTROLLER_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/motor_model.hpp b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/motor_model.hpp new file mode 100644 index 0000000..7da9b80 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/motor_model.hpp @@ -0,0 +1,57 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ROTORS_GAZEBO_PLUGINS_MOTOR_MODEL_H +#define ROTORS_GAZEBO_PLUGINS_MOTOR_MODEL_H + +#include + +class MotorModel +{ + public: + MotorModel() + : motor_rot_vel_(0.0), + ref_motor_rot_vel_(0.0), + prev_sim_time_(0.0), + sampling_time_(0.01) {} + virtual ~MotorModel() {} + void GetMotorVelocity(double &result) const { + result = motor_rot_vel_; + } + void SetReferenceMotorVelocity(double ref_motor_rot_vel) { + ref_motor_rot_vel_ = ref_motor_rot_vel; + } + + virtual void InitializeParams() = 0; + virtual void Publish() = 0; + + protected: + double motor_rot_vel_; + double ref_motor_rot_vel_; + double prev_ref_motor_rot_vel_; + double prev_sim_time_; + double sampling_time_; + + + virtual void UpdateForcesAndMoments() = 0; +}; + +#endif // ROTORS_GAZEBO_PLUGINS_MOTOR_MODEL_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/msgbuffer.h b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/msgbuffer.h new file mode 100644 index 0000000..5d6e646 --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/msgbuffer.h @@ -0,0 +1,80 @@ +/** + * @brief MAVConn message buffer class (internal) + * @file msgbuffer.h + * @author Vladimir Ermakov + * @author Amy Wagoner (adaptation to Gazebo) + */ +/** + * Copyright 2017 MAVROS dev team. All rights reserved. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#pragma once + +#include +#include "mavlink/v2.0/common/mavlink.h" + +namespace gazebo { +/** + * @brief Message buffer for internal use in libmavconn + */ +struct MsgBuffer { + //! Maximum buffer size with padding for CRC bytes (280 + padding) + static constexpr ssize_t MAX_SIZE = MAVLINK_MAX_PACKET_LEN + 16; + uint8_t data[MAX_SIZE]; + ssize_t len; + ssize_t pos; + + MsgBuffer() : + pos(0), + len(0) + { } + + /** + * @brief Buffer constructor from mavlink_message_t + */ + explicit MsgBuffer(const mavlink_message_t *msg) : + pos(0) + { + len = mavlink_msg_to_send_buffer(data, msg); + // paranoic check, it must be less than MAVLINK_MAX_PACKET_LEN + assert(len < MAX_SIZE); + } + + /** + * @brief Buffer constructor for send_bytes() + * @param[in] nbytes should be less than MAX_SIZE + */ + MsgBuffer(const uint8_t *bytes, ssize_t nbytes) : + pos(0), + len(nbytes) + { + assert(0 < nbytes && nbytes < MAX_SIZE); + memcpy(data, bytes, nbytes); + } + + virtual ~MsgBuffer() { + pos = 0; + len = 0; + } + + uint8_t *dpos() { + return data + pos; + } + + ssize_t nbytes() { + return len - pos; + } +}; +} // namespace gazebo diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/multi_copter.hpp b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/multi_copter.hpp new file mode 100644 index 0000000..feb514f --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/multi_copter.hpp @@ -0,0 +1,82 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ROTORS_GAZEBO_PLUGINS_MULTI_COPTER_H +#define ROTORS_GAZEBO_PLUGINS_MULTI_COPTER_H + +#include + +#include "rotors_gazebo_plugins/motor_controller.hpp" +#include "rotors_gazebo_plugins/motor_model.hpp" + +class MultiCopter +{ + public: + MultiCopter(int amount_rotors) : + position_(Eigen::Vector3d::Zero()), + velocity_(Eigen::Vector3d::Zero()), + attitude_(Eigen::Quaterniond::Identity()), + rotor_rot_vels_(Eigen::VectorXd::Zero(amount_rotors)) + {} + MultiCopter(int amount_rotors, + MotorController& motor_controller) : + MultiCopter(amount_rotors) + { + setMotorController(motor_controller); + } + ~MultiCopter(); + + void setMotorController( + MotorController& motor_controller) { + motor_controller_ = motor_controller; + } + + Eigen::VectorXd getRefMotorVelocities(double dt) { + return motor_controller_.getMotorVelocities(dt); + } + + Eigen::VectorXd getMotorVelocities() { + return rotor_rot_vels_; + } + + virtual Eigen::Vector4d simulateMAV(double dt, + Eigen::VectorXd ref_rotor_rot_vels) = 0; + virtual void initializeParams() = 0; + virtual void publish() = 0; + + const MotorController motorController() { + return motor_controller_; + } + const Eigen::Vector3d position() {return position_;} + const Eigen::Vector3d velocity() {return velocity_;} + const Eigen::Quaterniond attitude() {return attitude_;} + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + protected: + MotorController motor_controller_; + Eigen::Vector3d position_; + Eigen::Vector3d velocity_; + Eigen::Quaterniond attitude_; + Eigen::Vector3d angular_rate_; + Eigen::VectorXd rotor_rot_vels_; +}; + +#endif // ROTORS_GAZEBO_PLUGINS_MULTI_COPTER_H diff --git a/rotors_gazebo_plugins/include/rotors_gazebo_plugins/sdf_api_wrapper.hpp b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/sdf_api_wrapper.hpp new file mode 100644 index 0000000..d87ae0b --- /dev/null +++ b/rotors_gazebo_plugins/include/rotors_gazebo_plugins/sdf_api_wrapper.hpp @@ -0,0 +1,57 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + + +#ifndef ROTORS_GAZEBO_PLUGINS_SDF_API_WRAPPER_H +#define ROTORS_GAZEBO_PLUGINS_SDF_API_WRAPPER_H + +#include + +namespace gazebo { + +#if SDF_MAJOR_VERSION >= 3 + typedef ignition::math::Vector3d SdfVector3; +#else + class SdfVector3 : public sdf::Vector3 { + /* + A wrapper class for deprecated sdf::Vector3 class to provide the same accessor + functions as in the newer ignition::math::Vector3 class. + */ + + public: + using sdf::Vector3::Vector3; + virtual ~SdfVector3() {} + + /// \brief Get the x value + /// \return the x value + double X() { return this->x; } + + /// \brief Get the y value + /// \return the y value + double Y() { return this->y; } + + /// \brief Get the z value + /// \return the z value + double Z() { return this->z; } + }; +#endif +} + +#endif // ROTORS_GAZEBO_PLUGINS_SDF_API_WRAPPER_H diff --git a/rotors_gazebo_plugins/msgs/Actuators.proto b/rotors_gazebo_plugins/msgs/Actuators.proto new file mode 100644 index 0000000..6288e65 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/Actuators.proto @@ -0,0 +1,17 @@ +syntax = "proto2"; +package gz_sensor_msgs; + +import "Header.proto"; + +// Actuators message type which is emitted by +// GazeboMultirotorBasePlugin +message Actuators +{ + required gz_std_msgs.Header header = 1; + + repeated double angles = 2 [packed=true]; + + repeated double angular_velocities = 3 [packed=true]; + + repeated double normalized = 4 [packed=true]; +} diff --git a/rotors_gazebo_plugins/msgs/CommandMotorSpeed.proto b/rotors_gazebo_plugins/msgs/CommandMotorSpeed.proto new file mode 100644 index 0000000..ead2eba --- /dev/null +++ b/rotors_gazebo_plugins/msgs/CommandMotorSpeed.proto @@ -0,0 +1,6 @@ +syntax = "proto2"; +package gz_mav_msgs; + +message CommandMotorSpeed { + repeated float motor_speed = 1 [packed=true]; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/ConnectGazeboToRosTopic.proto b/rotors_gazebo_plugins/msgs/ConnectGazeboToRosTopic.proto new file mode 100644 index 0000000..d4dbbfd --- /dev/null +++ b/rotors_gazebo_plugins/msgs/ConnectGazeboToRosTopic.proto @@ -0,0 +1,35 @@ +syntax = "proto2"; +package gz_std_msgs; + +// Message designed to be sent to the ROS interface plugin by other +// Gazebo plugins, to tell it to forward messages on a particular Gazebo topic onto +// a ROS topic +message ConnectGazeboToRosTopic +{ + + //required string gazebo_namespace = 1; + required string gazebo_topic = 2; + required string ros_topic = 3; + + // The supported messages types that the ROS interface plugin knows to convert + // from a Gazebo to a ROS message. + // Provided to gz_std_msgs::ConnectGazeboToRosTopic::set_msgtype() + enum MsgType { + ACTUATORS = 0; + FLOAT_32 = 1; + FLUID_PRESSURE = 2; + IMU = 3; + JOINT_STATE = 4; + MAGNETIC_FIELD = 5; + NAV_SAT_FIX = 6; + ODOMETRY = 7; + POSE = 8; + POSE_WITH_COVARIANCE_STAMPED = 9; + TRANSFORM_STAMPED = 10; + TWIST_STAMPED = 11; + VECTOR_3D_STAMPED = 12; + WIND_SPEED = 13; + WRENCH_STAMPED = 14; + } + required MsgType msgType = 4; +} diff --git a/rotors_gazebo_plugins/msgs/ConnectRosToGazeboTopic.proto b/rotors_gazebo_plugins/msgs/ConnectRosToGazeboTopic.proto new file mode 100644 index 0000000..16f2da1 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/ConnectRosToGazeboTopic.proto @@ -0,0 +1,22 @@ +syntax = "proto2"; +package gz_std_msgs; + +// Message designed to be sent to the ROS interface plugin by other +// Gazebo plugins, to tell it to forward messages on a particular ROS topic onto +// a Gazebo topic +message ConnectRosToGazeboTopic +{ + required string ros_topic = 1; + //required string gazebo_namespace = 2; + required string gazebo_topic = 3; + + // The supported messages types that the ROS interface plugin knows to convert + // from a ROS to a Gazebo message + enum MsgType { + ACTUATORS = 0; + COMMAND_MOTOR_SPEED = 1; + ROLL_PITCH_YAWRATE_THRUST = 2; + WIND_SPEED = 3; + } + required MsgType msgType = 4; +} diff --git a/rotors_gazebo_plugins/msgs/Float32.proto b/rotors_gazebo_plugins/msgs/Float32.proto new file mode 100644 index 0000000..0d4e195 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/Float32.proto @@ -0,0 +1,7 @@ +syntax = "proto2"; +package gz_std_msgs; + +// Holds a single 32-bit float +message Float32 { + required float data = 1; +} diff --git a/rotors_gazebo_plugins/msgs/FluidPressure.proto b/rotors_gazebo_plugins/msgs/FluidPressure.proto new file mode 100644 index 0000000..2971860 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/FluidPressure.proto @@ -0,0 +1,16 @@ +syntax = "proto2"; +package gz_sensor_msgs; + +import "Header.proto"; + +// FluidPressure message type which is emitted by the +// Pressure Gazebo plugin +// Designed to imitate ROS sensor_msgs::FluidPressure +message FluidPressure +{ + required gz_std_msgs.Header header = 1; + + required double fluid_pressure = 2; + + required double variance = 3; +} diff --git a/rotors_gazebo_plugins/msgs/Header.proto b/rotors_gazebo_plugins/msgs/Header.proto new file mode 100644 index 0000000..9a6f830 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/Header.proto @@ -0,0 +1,13 @@ +syntax = "proto2"; +package gz_std_msgs; + +// Header type designed to be added to other +// Gazebo message types. +message Header { + message Stamp { + required uint32 sec = 1; + required uint32 nsec = 2; + } + required string frame_id = 1; + required Stamp stamp = 2; +} diff --git a/rotors_gazebo_plugins/msgs/Imu.proto b/rotors_gazebo_plugins/msgs/Imu.proto new file mode 100644 index 0000000..e9c3e76 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/Imu.proto @@ -0,0 +1,22 @@ +syntax = "proto2"; +package gz_sensor_msgs; + +import "quaternion.proto"; +import "vector3d.proto"; +import "Header.proto"; + +// Message published by GazeboImuPlugin. +message Imu +{ + + required gz_std_msgs.Header header = 1; + + required gazebo.msgs.Quaternion orientation = 2; + repeated float orientation_covariance = 3 [packed=true]; + + required gazebo.msgs.Vector3d angular_velocity = 4; + repeated float angular_velocity_covariance = 5 [packed=true]; + + required gazebo.msgs.Vector3d linear_acceleration = 6; + repeated float linear_acceleration_covariance = 7 [packed=true]; +} diff --git a/rotors_gazebo_plugins/msgs/JointState.proto b/rotors_gazebo_plugins/msgs/JointState.proto new file mode 100644 index 0000000..515e58e --- /dev/null +++ b/rotors_gazebo_plugins/msgs/JointState.proto @@ -0,0 +1,15 @@ +syntax = "proto2"; +package gz_sensor_msgs; + +import "Header.proto"; + +// JointState message type which is emitted by +// GazeboMultirotorBasePlugin +message JointState +{ + required gz_std_msgs.Header header = 1; + + repeated string name = 2; + repeated double position = 3 [packed=true]; + +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/Lidar.proto b/rotors_gazebo_plugins/msgs/Lidar.proto new file mode 100644 index 0000000..b9739c3 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/Lidar.proto @@ -0,0 +1,10 @@ +syntax = "proto2"; +package lidar_msgs.msgs; + +message lidar +{ + required float time_msec = 1; + required float min_distance = 2; + required float max_distance = 3; + required float current_distance = 4; +} diff --git a/rotors_gazebo_plugins/msgs/MagneticField.proto b/rotors_gazebo_plugins/msgs/MagneticField.proto new file mode 100644 index 0000000..90d2fff --- /dev/null +++ b/rotors_gazebo_plugins/msgs/MagneticField.proto @@ -0,0 +1,18 @@ +syntax = "proto2"; +package gz_sensor_msgs; + +import "quaternion.proto"; +import "vector3d.proto"; +import "Header.proto"; + +// Magnetic field message type which is emitted by the +// Magnetometer Gazebo plugin +message MagneticField +{ + + required gz_std_msgs.Header header = 1; + + required gazebo.msgs.Vector3d magnetic_field = 2; + + repeated float magnetic_field_covariance = 3 [packed=true]; +} diff --git a/rotors_gazebo_plugins/msgs/NavSatFix.proto b/rotors_gazebo_plugins/msgs/NavSatFix.proto new file mode 100644 index 0000000..2daf68c --- /dev/null +++ b/rotors_gazebo_plugins/msgs/NavSatFix.proto @@ -0,0 +1,43 @@ +syntax = "proto2"; +package gz_sensor_msgs; + +import "Header.proto"; + +// NavSatFix (GPS) message type which is emitted by the +// GPS Gazebo plugin +// Designed to imitate ROS sensor_msgs::NavSatStatus +message NavSatFix +{ + required gz_std_msgs.Header header = 1; + + enum Service { + SERVICE_GPS = 0; + SERVICE_GLONASS = 1; + SERVICE_COMPASS = 2; + SERVICE_GALILEO = 3; + } + required Service service = 2; + + enum Status { + STATUS_NO_FIX = 0; + STATUS_FIX = 1; + STATUS_SBAS_FIX = 2; + STATUS_GBAS_FIX = 3; + } + required Status status = 3; + + required double latitude = 4; + required double longitude = 5; + required double altitude = 6; + + enum PositionCovarianceType { + COVARIANCE_TYPE_UNKNOWN = 0; + COVARIANCE_TYPE_APPROXIMATED = 1; + COVARIANCE_TYPE_DIAGONAL_KNOWN = 2; + COVARIANCE_TYPE_KNOWN = 3; + } + required PositionCovarianceType position_covariance_type = 7; + + repeated float position_covariance = 8 [packed=true]; + +} diff --git a/rotors_gazebo_plugins/msgs/Odometry.proto b/rotors_gazebo_plugins/msgs/Odometry.proto new file mode 100644 index 0000000..fa1a860 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/Odometry.proto @@ -0,0 +1,17 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "Header.proto"; +import "PoseWithCovariance.proto"; +import "TwistWithCovariance.proto"; + +// Odometry message type which is emitted by +// GazeboMultirotorBasePlugin +message Odometry +{ + required gz_std_msgs.Header header = 1; + + required string child_frame_id = 2; + required PoseWithCovariance pose = 3; + required TwistWithCovariance twist = 4; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/OpticalFlow.proto b/rotors_gazebo_plugins/msgs/OpticalFlow.proto new file mode 100644 index 0000000..8931461 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/OpticalFlow.proto @@ -0,0 +1,18 @@ +syntax = "proto2"; +package opticalFlow_msgs.msgs; + +message opticalFlow +{ + required float time_usec = 1; + required float sensor_id = 2; + required float integration_time_us = 3; + required float integrated_x = 4; + required float integrated_y = 5; + required float integrated_xgyro = 6; + required float integrated_ygyro = 7; + required float integrated_zgyro = 8; + required float temperature = 9; + required float quality = 10; + required float time_delta_distance_us = 11; + required float distance = 12; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/Point.proto b/rotors_gazebo_plugins/msgs/Point.proto new file mode 100644 index 0000000..af5332e --- /dev/null +++ b/rotors_gazebo_plugins/msgs/Point.proto @@ -0,0 +1,11 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +// Point (x, y, z) message type which designed to be used +// in other message types +message Point +{ + required double x = 1; + required double y = 2; + required double z = 3; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/PointStamped.proto b/rotors_gazebo_plugins/msgs/PointStamped.proto new file mode 100644 index 0000000..4404ad3 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/PointStamped.proto @@ -0,0 +1,12 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "Header.proto"; +import "Point.proto"; + +// PointStamped message type +message PointStamped +{ + required gz_std_msgs.Header header = 1; + required Point point = 2; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/PoseStamped.proto b/rotors_gazebo_plugins/msgs/PoseStamped.proto new file mode 100644 index 0000000..f025966 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/PoseStamped.proto @@ -0,0 +1,13 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "Header.proto"; +import "pose.proto"; + +// PoseStamped message type which is emitted by +// GazeboOdometryPlugin +message PoseStamped +{ + required gz_std_msgs.Header header = 1; + required gazebo.msgs.Pose pose = 2; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/PoseWithCovariance.proto b/rotors_gazebo_plugins/msgs/PoseWithCovariance.proto new file mode 100644 index 0000000..81556ad --- /dev/null +++ b/rotors_gazebo_plugins/msgs/PoseWithCovariance.proto @@ -0,0 +1,11 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "pose.proto"; + +// Pose with covariance message type which designed to be used +// in other message types +message PoseWithCovariance { + required gazebo.msgs.Pose pose = 1; + repeated double covariance = 2 [packed=true]; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/PoseWithCovarianceStamped.proto b/rotors_gazebo_plugins/msgs/PoseWithCovarianceStamped.proto new file mode 100644 index 0000000..520bc29 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/PoseWithCovarianceStamped.proto @@ -0,0 +1,12 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "Header.proto"; +import "PoseWithCovariance.proto"; + +// Pose with covariance stamped message type, +// used to emulate a ROS message in the Gazebo framework. +message PoseWithCovarianceStamped { + required gz_std_msgs.Header header = 1; + required PoseWithCovariance pose_with_covariance = 2; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/RollPitchYawrateThrust.proto b/rotors_gazebo_plugins/msgs/RollPitchYawrateThrust.proto new file mode 100644 index 0000000..a7fa780 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/RollPitchYawrateThrust.proto @@ -0,0 +1,29 @@ +syntax = "proto2"; +package gz_mav_msgs; + +import "Header.proto"; +import "vector3d.proto"; + +message RollPitchYawrateThrust { + required gz_std_msgs.Header header = 1; + + // We use the coordinate frames with the following convention: + // x: forward + // y: left + // z: up + + // rotation convention (z-y'-x''): + // yaw rotates around fixed frame's z axis + // pitch rotates around new y-axis (y') + // roll rotates around new x-axis (x'') + required double roll = 2; + required double pitch = 3; + required double yaw_rate = 4; + + // Thrust [N] expressed in the body frame. + // For a fixed-wing, usually the x-component is used. + // For a multi-rotor, usually the z-component is used. + // Set all un-used components to 0. + required gazebo.msgs.Vector3d thrust = 5; +} + diff --git a/rotors_gazebo_plugins/msgs/Transform.proto b/rotors_gazebo_plugins/msgs/Transform.proto new file mode 100644 index 0000000..483db5b --- /dev/null +++ b/rotors_gazebo_plugins/msgs/Transform.proto @@ -0,0 +1,10 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "vector3d.proto"; +import "quaternion.proto"; + +message Transform { + required gazebo.msgs.Vector3d translation = 1; + required gazebo.msgs.Quaternion rotation = 2; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/TransformStamped.proto b/rotors_gazebo_plugins/msgs/TransformStamped.proto new file mode 100644 index 0000000..a1d748a --- /dev/null +++ b/rotors_gazebo_plugins/msgs/TransformStamped.proto @@ -0,0 +1,10 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "Header.proto"; +import "Transform.proto"; + +message TransformStamped { + required gz_std_msgs.Header header = 1; + required Transform transform = 2; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/TransformStampedWithFrameIds.proto b/rotors_gazebo_plugins/msgs/TransformStampedWithFrameIds.proto new file mode 100644 index 0000000..b88efdc --- /dev/null +++ b/rotors_gazebo_plugins/msgs/TransformStampedWithFrameIds.proto @@ -0,0 +1,18 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "Header.proto"; +import "Transform.proto"; + +// A stamped transform message, but also with the +// parent and child frame ids. Note that the parent frame ID +// is likely to be the same as the frame_id provided in the +// header. Used by the topic to send transforms from the odometry +// plugin to the ROS interface plugin, so it can then call +// transform_broadcast(). +message TransformStampedWithFrameIds { + required gz_std_msgs.Header header = 1; + required Transform transform = 2; + required string parent_frame_id = 3; + required string child_frame_id = 4; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/Twist.proto b/rotors_gazebo_plugins/msgs/Twist.proto new file mode 100644 index 0000000..0d68cda --- /dev/null +++ b/rotors_gazebo_plugins/msgs/Twist.proto @@ -0,0 +1,12 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "vector3d.proto"; + +// Twist message type incorporates linear +// and angular components. +message Twist +{ + required gazebo.msgs.Vector3d linear = 1; + required gazebo.msgs.Vector3d angular = 2; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/TwistStamped.proto b/rotors_gazebo_plugins/msgs/TwistStamped.proto new file mode 100644 index 0000000..a77c0a3 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/TwistStamped.proto @@ -0,0 +1,13 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "Header.proto"; +import "Twist.proto"; + +// TwistStamped (e.g. ground speed) message type which is emitted by the +// GPS Gazebo plugin +message TwistStamped +{ + required gz_std_msgs.Header header = 1; + required Twist twist = 2; +} diff --git a/rotors_gazebo_plugins/msgs/TwistWithCovariance.proto b/rotors_gazebo_plugins/msgs/TwistWithCovariance.proto new file mode 100644 index 0000000..361871e --- /dev/null +++ b/rotors_gazebo_plugins/msgs/TwistWithCovariance.proto @@ -0,0 +1,11 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "Twist.proto"; + +// Twist with covariance message type which is designed to be used +// in other message types +message TwistWithCovariance { + required Twist twist = 1; + repeated double covariance = 2 [packed=true]; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/Vector3dStamped.proto b/rotors_gazebo_plugins/msgs/Vector3dStamped.proto new file mode 100644 index 0000000..5fc37f1 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/Vector3dStamped.proto @@ -0,0 +1,13 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "Header.proto"; +import "vector3d.proto"; + +// Extends the Gazebo Vector3d message type to +// include a header. +message Vector3dStamped +{ + required gz_std_msgs.Header header = 1; + required gazebo.msgs.Vector3d position = 2; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/WindSpeed.proto b/rotors_gazebo_plugins/msgs/WindSpeed.proto new file mode 100644 index 0000000..9a2da2f --- /dev/null +++ b/rotors_gazebo_plugins/msgs/WindSpeed.proto @@ -0,0 +1,10 @@ +syntax = "proto2"; +package gz_mav_msgs; + +import "Header.proto"; +import "vector3d.proto"; + +message WindSpeed { + required gz_std_msgs.Header header = 1; + required gazebo.msgs.Vector3d velocity = 2; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/Wrench.proto b/rotors_gazebo_plugins/msgs/Wrench.proto new file mode 100644 index 0000000..734ce73 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/Wrench.proto @@ -0,0 +1,11 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "vector3d.proto"; + +// Wrench includes both force and torque information about a joint. +message Wrench +{ + required gazebo.msgs.Vector3d force = 1; + required gazebo.msgs.Vector3d torque = 2; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/msgs/WrenchStamped.proto b/rotors_gazebo_plugins/msgs/WrenchStamped.proto new file mode 100644 index 0000000..1a7c603 --- /dev/null +++ b/rotors_gazebo_plugins/msgs/WrenchStamped.proto @@ -0,0 +1,11 @@ +syntax = "proto2"; +package gz_geometry_msgs; + +import "Header.proto"; +import "Wrench.proto"; + +// A message with both a header (with time info) and wrench info (forces and torques). +message WrenchStamped { + required gz_std_msgs.Header header = 1; + required Wrench wrench = 2; +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/package.xml b/rotors_gazebo_plugins/package.xml new file mode 100644 index 0000000..2b74c9c --- /dev/null +++ b/rotors_gazebo_plugins/package.xml @@ -0,0 +1,41 @@ + + rotors_gazebo_plugins + 2.1.1 + The rotors_gazebo_plugins package + + Fadri Furrer + Mina Kamel + + Fadri Furrer + Michael Burri + Mina Kamel + Janosch Nikolic + Markus Achtelik + + ASL 2.0 + + https://github.com/ethz-asl/rotors_simulator + https://github.com/ethz-asl/rotors_simulator/issues + + catkin + + + cmake_modules + cv_bridge + gazebo_dev + gazebo_ros + geometry_msgs + libgoogle-glog-dev + mav_msgs + octomap_msgs + octomap_ros + octomap + rosbag + roscpp + rotors_comm + rotors_control + std_srvs + tf + yaml-cpp + + diff --git a/rotors_gazebo_plugins/src/depth_noise_model.cpp b/rotors_gazebo_plugins/src/depth_noise_model.cpp new file mode 100644 index 0000000..36cc651 --- /dev/null +++ b/rotors_gazebo_plugins/src/depth_noise_model.cpp @@ -0,0 +1,73 @@ +/* + * Copyright 2018 Michael Pantic, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include + +#include + +inline bool DepthNoiseModel::InRange(const float depth) const { + return depth > this->min_depth && depth < this->max_depth; +} + +void D435DepthNoiseModel::ApplyNoise(const uint32_t width, + const uint32_t height, float *data) { + if (data == nullptr) { + return; + } + + float f = 0.5f * (width / tanf(h_fov / 2.0f)); + float multiplier = (subpixel_err) / (f * baseline * 1e6f); + Eigen::Map data_vector_map(data, width * height); + + // Formula taken from the Intel Whitepaper: + // "Best-Known-Methods for Tuning Intel RealSense™ D400 Depth Cameras for Best Performance". + // We are using the theoretical RMS model formula. + Eigen::VectorXf rms_noise = (data_vector_map * 1000.0).array().square() * multiplier; + Eigen::VectorXf noise = rms_noise.array().square(); + + // Sample noise for each pixel and transform variance according to error at this depth. + for (int i = 0; i < width * height; ++i) { + if (InRange(data_vector_map[i])) { + data_vector_map[i] += + this->dist(this->gen) * std::min(((float)noise(i)), max_stdev); + } else { + data_vector_map[i] = this->bad_point; + } + } +} + +void KinectDepthNoiseModel::ApplyNoise(const uint32_t width, + const uint32_t height, float *data) { + if (data == nullptr) { + return; + } + + // Axial noise model from + // https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=6375037, + // Nguyen, Izadi & Lovell: "Modeling Kinect Sensor Noise for Improved 3D Reconstrucion and Tracking", 3DIM/3DPVT, 2012. + // We are using the 10-60 Degree model as an approximation. + Eigen::Map data_vector_map(data, width * height); + Eigen::VectorXf var_noise = 0.0012f + 0.0019f * (data_vector_map.array() - 0.4f).array().square(); + + // Sample noise for each pixel and transform variance according to error at this depth. + for (int i = 0; i < width * height; ++i) { + if (InRange(data_vector_map[i])) { + data_vector_map[i] += this->dist(this->gen) * var_noise(i); + } else { + data_vector_map[i] = this->bad_point; + } + } +} diff --git a/rotors_gazebo_plugins/src/external/gazebo_geotagged_images_plugin.cpp b/rotors_gazebo_plugins/src/external/gazebo_geotagged_images_plugin.cpp new file mode 100644 index 0000000..9b39967 --- /dev/null +++ b/rotors_gazebo_plugins/src/external/gazebo_geotagged_images_plugin.cpp @@ -0,0 +1,181 @@ +/* + * Copyright (C) 2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include "rotors_gazebo_plugins/external/gazebo_geotagged_images_plugin.h" + +#include +#include +#include + +#include +#include +#include +#include + +#include "rotors_gazebo_plugins/common.h" + +using namespace std; +using namespace gazebo; +using namespace cv; + +GZ_REGISTER_SENSOR_PLUGIN(GeotaggedImagesPlugin) + + +GeotaggedImagesPlugin::GeotaggedImagesPlugin() : + SensorPlugin(), width_(0), height_(0), depth_(0), imageCounter_(0) {} + +GeotaggedImagesPlugin::~GeotaggedImagesPlugin() +{ + this->parentSensor_.reset(); + this->camera_.reset(); +} + +void GeotaggedImagesPlugin::Load(sensors::SensorPtr sensor, sdf::ElementPtr sdf) +{ + if(kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!sensor) + gzerr << "Invalid sensor pointer.\n"; + + this->parentSensor_ = + std::dynamic_pointer_cast(sensor); + + if (!this->parentSensor_) + { + gzerr << "GeotaggedImagesPlugin requires a CameraSensor.\n"; + } + + this->camera_ = this->parentSensor_->Camera(); + + if (!this->parentSensor_) + { + gzerr << "GeotaggedImagesPlugin not attached to a camera sensor\n"; + return; + } + scene_ = camera_->GetScene(); + lastImageTime_ = scene_->SimTime(); + + this->width_ = this->camera_->ImageWidth(); + this->height_ = this->camera_->ImageHeight(); + this->depth_ = this->camera_->ImageDepth(); + this->format_ = this->camera_->ImageFormat(); + + if (sdf->HasElement("robotNamespace")) { + namespace_ = sdf->GetElement("robotNamespace")->Get(); + } else { + gzwarn << "[gazebo_geotagging_images_camera_plugin] Please specify a robotNamespace.\n"; + } + + this->storeIntervalSec_ = 1; + if (sdf->HasElement("interval")) { + this->storeIntervalSec_ = sdf->GetElement("interval")->Get(); + } + + destWidth_ = width_; + if (sdf->HasElement("width")) { + destWidth_ = sdf->GetElement("width")->Get(); + } + destHeight_ = height_; + if (sdf->HasElement("height")) { + destHeight_ = sdf->GetElement("height")->Get(); + } + + + //check if exiftool exists + if (system("exiftool -ver &>/dev/null") != 0) { + gzerr << "exiftool not found. geotagging_images plugin will be disabled" << endl; + gzerr << "On Ubuntu, use 'sudo apt-get install libimage-exiftool-perl' to install" << endl; + return; + } + + node_handle_ = transport::NodePtr(new transport::Node()); + node_handle_->Init(namespace_); + + this->parentSensor_->SetActive(true); + + this->newFrameConnection_ = this->camera_->ConnectNewImageFrame( + boost::bind(&GeotaggedImagesPlugin::OnNewFrame, this, _1)); + + // This topic is published to by gazebo_mavlink_interface.cpp + /// \todo Should this be an absolute topic!?! + gpsSub_ = node_handle_->Subscribe("~/gps_position", &GeotaggedImagesPlugin::OnNewGpsPosition, this); + + storageDir_ = "frames"; + boost::filesystem::remove_all(storageDir_); //clear existing images + boost::filesystem::create_directory(storageDir_); +} + +void GeotaggedImagesPlugin::OnNewGpsPosition(ConstVector3dPtr& v) +{ + lastGpsPosition_ = *v; + //gzdbg << "got gps pos: "<camera_->ImageData(0); + + common::Time currentTime = scene_->SimTime(); + if (currentTime.Double() - lastImageTime_.Double() < storeIntervalSec_) { + return; + } + + Mat frame = Mat(height_, width_, CV_8UC3); + Mat frameBGR = Mat(height_, width_, CV_8UC3); + frame.data = (uchar*)image; //frame has not the right color format yet -> convert + cvtColor(frame, frameBGR, CV_RGB2BGR); + + char file_name[256]; + snprintf(file_name, sizeof(file_name), "%s/DSC%05i.jpg", storageDir_.c_str(), imageCounter_); + + if (destWidth_ != width_ || destHeight_ != height_) { + Mat frameResized; + cv::Size size(destWidth_, destHeight_); + cv::resize(frameBGR, frameResized, size); + imwrite(file_name, frameResized); + } else { + imwrite(file_name, frameBGR); + } + + char gps_tag_command[1024]; + double lat = lastGpsPosition_.x(); + char north_south = 'N', east_west = 'E'; + double lon = lastGpsPosition_.y(); + if (lat < 0.) { + lat = -lat; + north_south = 'S'; + } + if (lon < 0.) { + lon = -lon; + east_west = 'W'; + } + snprintf(gps_tag_command, sizeof(gps_tag_command), + "exiftool -gpslatituderef=%c -gpslongituderef=%c -gpsaltituderef=above -gpslatitude=%.9lf -gpslongitude=%.9lf" +// " -gpsdatetime=now -gpsmapdatum=WGS-84" + " -datetimeoriginal=now -gpsdop=0.8" + " -gpsmeasuremode=3-d -gpssatellites=13 -gpsaltitude=%.3lf -overwrite_original %s &>/dev/null", + north_south, east_west, lat, lon, lastGpsPosition_.z(), file_name); + + system(gps_tag_command); + + ++imageCounter_; + lastImageTime_ = currentTime; + +} diff --git a/rotors_gazebo_plugins/src/external/gazebo_gimbal_controller_plugin.cpp b/rotors_gazebo_plugins/src/external/gazebo_gimbal_controller_plugin.cpp new file mode 100644 index 0000000..d4e3d5f --- /dev/null +++ b/rotors_gazebo_plugins/src/external/gazebo_gimbal_controller_plugin.cpp @@ -0,0 +1,406 @@ +/* + * Copyright (C) 2012-2015 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include "rotors_gazebo_plugins/external/gazebo_gimbal_controller_plugin.h" + +using namespace gazebo; +using namespace std; + +GZ_REGISTER_MODEL_PLUGIN(GimbalControllerPlugin) + +///////////////////////////////////////////////// +GimbalControllerPlugin::GimbalControllerPlugin() + :status("closed") +{ + /// TODO: make these gains part of sdf xml + this->pitchPid.Init(5, 0, 0, 0, 0, 0.3, -0.3); + this->rollPid.Init(5, 0, 0, 0, 0, 0.3, -0.3); + this->yawPid.Init(1.0, 0, 0, 0, 0, 1.0, -1.0); + this->pitchCommand = 0.5* M_PI; + this->rollCommand = 0; + this->yawCommand = 0; +} + +///////////////////////////////////////////////// +void GimbalControllerPlugin::Load( + physics::ModelPtr _model, + sdf::ElementPtr _sdf) +{ + + if(kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + this->model = _model; + + this->sdf = _sdf; + + std::string yawJointName = "cgo3_vertical_arm_joint"; + this->yawJoint = this->model->GetJoint(yawJointName); + if (this->sdf->HasElement("joint_yaw")) + { + // Add names to map + yawJointName = sdf->Get("joint_yaw"); + if (this->model->GetJoint(yawJointName)) + { + this->yawJoint = this->model->GetJoint(yawJointName); + } + else + { + gzwarn << "joint_yaw [" << yawJointName << "] does not exist?\n"; + } + } + if (!this->yawJoint) + { + gzerr << "GimbalControllerPlugin::Load ERROR! Can't get yaw joint '" + << yawJointName << "' " << endl; + } + + std::string rollJointName = "cgo3_horizontal_arm_joint"; + this->rollJoint = this->model->GetJoint(rollJointName); + if (this->sdf->HasElement("joint_roll")) + { + // Add names to map + rollJointName = sdf->Get("joint_roll"); + if (this->model->GetJoint(rollJointName)) + { + this->rollJoint = this->model->GetJoint(rollJointName); + } + else + { + gzwarn << "joint_roll [" << rollJointName << "] does not exist?\n"; + } + } + if (!this->rollJoint) + { + gzerr << "GimbalControllerPlugin::Load ERROR! Can't get roll joint '" + << rollJointName << "' " << endl; + } + + + std::string pitchJointName = "cgo3_camera_joint"; + this->pitchJoint = this->model->GetJoint(pitchJointName); + if (this->sdf->HasElement("joint_pitch")) + { + // Add names to map + pitchJointName = sdf->Get("joint_pitch"); + if (this->model->GetJoint(pitchJointName)) + { + this->pitchJoint = this->model->GetJoint(pitchJointName); + } + else + { + gzwarn << "joint_pitch [" << pitchJointName << "] does not exist?\n"; + } + } + if (!this->pitchJoint) + { + gzerr << "GimbalControllerPlugin::Load ERROR! Can't get pitch joint '" + << pitchJointName << "' " << endl; + } + + + // get imu sensor + std::string imuSensorName = "camera_imu"; + if (this->sdf->HasElement("imu")) + { + // Add names to map + imuSensorName = sdf->Get("imu"); + } + + this->imuSensor = std::static_pointer_cast( + sensors::SensorManager::Instance()->GetSensor(imuSensorName)); + + if (!this->imuSensor) + { + gzerr << "GimbalControllerPlugin::Load ERROR! Can't get imu sensor '" + << imuSensorName << "' " << endl; + } +} + +///////////////////////////////////////////////// +void GimbalControllerPlugin::Init() +{ + this->node = transport::NodePtr(new transport::Node()); + this->node->Init(this->model->GetWorld()->GetName()); + + this->lastUpdateTime = this->model->GetWorld()->GetSimTime(); + + // receive pitch command via gz transport + std::string pitchTopic = std::string("~/") + this->model->GetName() + + "/gimbal_pitch_cmd"; + this->pitchSub = this->node->Subscribe(pitchTopic, + &GimbalControllerPlugin::OnPitchStringMsg, this); + // receive roll command via gz transport + std::string rollTopic = std::string("~/") + this->model->GetName() + + "/gimbal_roll_cmd"; + this->rollSub = this->node->Subscribe(rollTopic, + &GimbalControllerPlugin::OnRollStringMsg, this); + // receive yaw command via gz transport + std::string yawTopic = std::string("~/") + this->model->GetName() + + "/gimbal_yaw_cmd"; + this->yawSub = this->node->Subscribe(yawTopic, + &GimbalControllerPlugin::OnYawStringMsg, this); + + // plugin update + this->connections.push_back(event::Events::ConnectWorldUpdateBegin( + boost::bind(&GimbalControllerPlugin::OnUpdate, this))); + + // publish pitch status via gz transport + pitchTopic = std::string("~/") + this->model->GetName() + + "/gimbal_pitch_status"; + this->pitchPub = node->Advertise(pitchTopic); + + // publish roll status via gz transport + rollTopic = std::string("~/") + this->model->GetName() + + "/gimbal_roll_status"; + this->rollPub = node->Advertise(rollTopic); + + + // publish yaw status via gz transport + yawTopic = std::string("~/") + this->model->GetName() + + "/gimbal_yaw_status"; + this->yawPub = node->Advertise(yawTopic); + + gzmsg << "GimbalControllerPlugin::Init" << std::endl; +} + +///////////////////////////////////////////////// +void GimbalControllerPlugin::OnPitchStringMsg(ConstAnyPtr &_msg) +{ +// gzdbg << "pitch command received " << _msg->double_value() << std::endl; + this->pitchCommand = _msg->double_value(); +} + +///////////////////////////////////////////////// +void GimbalControllerPlugin::OnRollStringMsg(ConstAnyPtr &_msg) +{ +// gzdbg << "roll command received " << _msg->double_value() << std::endl; + this->rollCommand = _msg->double_value(); +} + +///////////////////////////////////////////////// +void GimbalControllerPlugin::OnYawStringMsg(ConstAnyPtr &_msg) +{ +// gzdbg << "yaw command received " << _msg->double_value() << std::endl; + this->yawCommand = _msg->double_value(); +} + +///////////////////////////////////////////////// +ignition::ignition::math::Vector3d GimbalControllerPlugin::ThreeAxisRot( + double r11, double r12, double r21, double r31, double r32) +{ + return ignition::ignition::math::Vector3d d( + atan2( r31, r32 ), + asin ( r21 ), + atan2( r11, r12 )); +} + +///////////////////////////////////////////////// +ignition::ignition::math::Vector3d GimbalControllerPlugin::QtoZXY( + const ignition::math::Quaterniond &_q) +{ + // taken from + // http://bediyap.com/programming/convert-quaternion-to-euler-rotations/ + // case zxy: + ignition::ignition::math::Vector3d result = this->ThreeAxisRot( + -2*(_q.X()*_q.Y() - _q.W()*_q.Z()), + _q.W()*_q.W() - _q.X()*_q.X() + _q.Y()*_q.Y() - _q.Z()*_q.Z(), + 2*(_q.Y()*_q.Z() + _q.W()*_q.X()), + -2*(_q.X()*_q.Z() - _q.W()*_q.Y()), + _q.W()*_q.W() - _q.X()*_q.X() - _q.Y()*_q.Y() + _q.Z()*_q.Z()); + return result; +} + +///////////////////////////////////////////////// +void GimbalControllerPlugin::OnUpdate() +{ + if (!this->pitchJoint || !this->rollJoint || !this->yawJoint) + return; + + common::Time time = this->model->GetWorld()->GetSimTime(); + if (time < this->lastUpdateTime) + { + gzerr << "time reset event\n"; + this->lastUpdateTime = time; + return; + } + else if (time > this->lastUpdateTime) + { + double dt = (this->lastUpdateTime - time).Double(); + + // anything to do with gazebo joint has + // hardcoded negative joint axis for pitch and roll + // TODO: make joint direction a parameter + const double rDir = -1; + const double pDir = -1; + const double yDir = 1; + + // truncate command inside joint angle limits + double rollLimited = ignition::math::clamp(this->rollCommand, + rDir*this->rollJoint->GetUpperLimit(0).Radian(), + rDir*this->rollJoint->GetLowerLimit(0).Radian()); + double pitchLimited = ignition::math::clamp(this->pitchCommand, + pDir*this->pitchJoint->GetUpperLimit(0).Radian(), + pDir*this->pitchJoint->GetLowerLimit(0).Radian()); + double yawLimited = ignition::math::clamp(this->yawCommand, + yDir*this->yawJoint->GetLowerLimit(0).Radian(), + yDir*this->yawJoint->GetUpperLimit(0).Radian()); + + ignition::math::Quaterniond commandRPY( + rollLimited, pitchLimited, yawLimited); + + + /// Get current joint angles (in sensor frame): + + /// currentAngleYPRVariable is defined in roll-pitch-yaw-fixed-axis + /// and gimbal is constructed using yaw-roll-pitch-variable-axis + ignition::ignition::math::Vector3d currentAngleYPRVariable( + this->imuSensor->Orientation().Euler()); + ignition::ignition::math::Vector3d currentAnglePRYVariable( + this->QtoZXY(currentAngleYPRVariable)); + + /// get joint limits (in sensor frame) + /// TODO: move to Load() if limits do not change + ignition::ignition::math::Vector3d lowerLimitsPRY + (pDir*this->pitchJoint->GetLowerLimit(0).Radian(), + rDir*this->rollJoint->GetLowerLimit(0).Radian(), + yDir*this->yawJoint->GetLowerLimit(0).Radian()); + ignition::ignition::math::Vector3d upperLimitsPRY + (pDir*this->pitchJoint->GetUpperLimit(0).Radian(), + rDir*this->rollJoint->GetUpperLimit(0).Radian(), + yDir*this->yawJoint->GetUpperLimit(0).Radian()); + + // normalize errors + double pitchError = this->ShortestAngularDistance( + pitchLimited, currentAnglePRYVariable.X()); + double rollError = this->ShortestAngularDistance( + rollLimited, currentAnglePRYVariable.Y()); + double yawError = this->ShortestAngularDistance( + yawLimited, currentAnglePRYVariable.Z()); + + // Clamp errors based on current angle and estimated errors from rotations: + // given error = current - target, then + // if target (current angle - error) is outside joint limit, truncate error + // so that current angle - error is within joint limit, i.e.: + // lower limit < current angle - error < upper limit + // or + // current angle - lower limit > error > current angle - upper limit + // re-expressed as clamps: + // hardcoded negative joint axis for pitch and roll + if (lowerLimitsPRY.X() < upperLimitsPRY.X()) + { + pitchError = ignition::math::clamp(pitchError, + currentAnglePRYVariable.X() - upperLimitsPRY.X(), + currentAnglePRYVariable.X() - lowerLimitsPRY.X()); + } + else + { + pitchError = ignition::math::clamp(pitchError, + currentAnglePRYVariable.X() - lowerLimitsPRY.X(), + currentAnglePRYVariable.X() - upperLimitsPRY.X()); + } + if (lowerLimitsPRY.Y() < upperLimitsPRY.Y()) + { + rollError = ignition::math::clamp(rollError, + currentAnglePRYVariable.Y() - upperLimitsPRY.Y(), + currentAnglePRYVariable.Y() - lowerLimitsPRY.Y()); + } + else + { + rollError = ignition::math::clamp(rollError, + currentAnglePRYVariable.Y() - lowerLimitsPRY.Y(), + currentAnglePRYVariable.Y() - upperLimitsPRY.Y()); + } + if (lowerLimitsPRY.Z() < upperLimitsPRY.Z()) + { + yawError = ignition::math::clamp(yawError, + currentAnglePRYVariable.Z() - upperLimitsPRY.Z(), + currentAnglePRYVariable.Z() - lowerLimitsPRY.Z()); + } + else + { + yawError = ignition::math::clamp(yawError, + currentAnglePRYVariable.Z() - lowerLimitsPRY.Z(), + currentAnglePRYVariable.Z() - upperLimitsPRY.Z()); + } + + // apply forces to move gimbal + double pitchForce = this->pitchPid.Update(pitchError, dt); + this->pitchJoint->SetForce(0, pDir*pitchForce); + + double rollForce = this->rollPid.Update(rollError, dt); + this->rollJoint->SetForce(0, rDir*rollForce); + + double yawForce = this->yawPid.Update(yawError, dt); + this->yawJoint->SetForce(0, yDir*yawForce); + + // ignition::ignition::math::Vector3d angles = this->imuSensor->Orientation().Euler(); + // gzerr << "ang[" << angles.X() << ", " << angles.Y() << ", " << angles.Z() + // << "] cmd[ " << this->rollCommand + // << ", " << this->pitchCommand << ", " << this->yawCommand + // << "] err[ " << rollError + // << ", " << pitchError << ", " << yawError + // << "] frc[ " << rollForce + // << ", " << pitchForce << ", " << yawForce << "]\n"; + + + this->lastUpdateTime = time; + } + + static int i =1000; + if (++i>100) + { + i = 0; + + gazebo::msgs::Any m; + m.set_type(gazebo::msgs::Any_ValueType_DOUBLE); + + m.set_double_value(this->pitchJoint->GetAngle(0).Radian()); + this->pitchPub->Publish(m); + + m.set_double_value(this->rollJoint->GetAngle(0).Radian()); + this->rollPub->Publish(m); + + m.set_double_value(this->yawJoint->GetAngle(0).Radian()); + this->yawPub->Publish(m); + } +} + +///////////////////////////////////////////////// +double GimbalControllerPlugin::NormalizeAbout(double _angle, double reference) +{ + double diff = _angle - reference; + // normalize diff about (-pi, pi], then add reference + while (diff <= -M_PI) + { + diff += 2.0*M_PI; + } + while (diff > M_PI) + { + diff -= 2.0*M_PI; + } + return diff + reference; +} + +///////////////////////////////////////////////// +double GimbalControllerPlugin::ShortestAngularDistance(double _from, double _to) +{ + return this->NormalizeAbout(_to, _from) - _from; +} diff --git a/rotors_gazebo_plugins/src/external/gazebo_gst_camera_plugin.cpp b/rotors_gazebo_plugins/src/external/gazebo_gst_camera_plugin.cpp new file mode 100644 index 0000000..c1aca5f --- /dev/null +++ b/rotors_gazebo_plugins/src/external/gazebo_gst_camera_plugin.cpp @@ -0,0 +1,272 @@ +/* + * Copyright (C) 2012-2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifdef _WIN32 + // Ensure that Winsock2.h is included before Windows.h, which can get + // pulled in by anybody (e.g., Boost). +#include +#endif + +#include "gazebo/sensors/DepthCameraSensor.hh" +#include "rotors_gazebo_plugins/external/gazebo_gst_camera_plugin.h" + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gazebo; + +GZ_REGISTER_SENSOR_PLUGIN(GstCameraPlugin) + + +static void cb_need_data(GstElement *appsrc, guint unused_size, gpointer user_data) { + GstCameraPlugin *plugin = (GstCameraPlugin*)user_data; + plugin->gstCallback(appsrc); +} + +void GstCameraPlugin::gstCallback(GstElement *appsrc) { + + frameBufferMutex.lock(); + + while (!frameBuffer) { + /* can happen if not initialized yet */ + frameBufferMutex.unlock(); + usleep(10000); + frameBufferMutex.lock(); + } + + GST_BUFFER_PTS(frameBuffer) = gstTimestamp; + GST_BUFFER_DURATION(frameBuffer) = gst_util_uint64_scale_int (1, GST_SECOND, (int)rate); + gstTimestamp += GST_BUFFER_DURATION(frameBuffer); + + GstFlowReturn ret; + g_signal_emit_by_name(appsrc, "push-buffer", frameBuffer, &ret); + + frameBufferMutex.unlock(); + + if (ret != GST_FLOW_OK) { + /* something wrong, stop pushing */ + gzerr << "g_signal_emit_by_name failed" << endl; + g_main_loop_quit(mainLoop); + } +} + +static void* start_thread(void* param) { + GstCameraPlugin* plugin = (GstCameraPlugin*)param; + plugin->startGstThread(); + return nullptr; +} + +void GstCameraPlugin::startGstThread() { + + gst_init(0, 0); + + mainLoop = g_main_loop_new(NULL, FALSE); + if (!mainLoop) { + gzerr << "Create loop failed. \n"; + return; + } + + GstElement* pipeline = gst_pipeline_new("sender"); + if (!pipeline) { + gzerr << "ERR: Create pipeline failed. \n"; + return; + } + + GstElement* dataSrc = gst_element_factory_make("appsrc", "AppSrc"); + GstElement* testSrc = gst_element_factory_make("videotestsrc", "FileSrc"); + GstElement* conv = gst_element_factory_make("videoconvert", "Convert"); + GstElement* encoder = gst_element_factory_make("x264enc", "AvcEncoder"); + GstElement* parser = gst_element_factory_make("h264parse", "Parser"); + GstElement* payload = gst_element_factory_make("rtph264pay", "PayLoad"); + GstElement* sink = gst_element_factory_make("udpsink", "UdpSink"); + if (!dataSrc || !testSrc || !conv || !encoder || !parser || !payload || !sink) { + gzerr << "ERR: Create elements failed. \n"; + return; + } + +// gzerr <<"width"<< this->width<<"\n"; +// gzerr <<"height"<< this->height<<"\n"; +// gzerr <<"rate"<< this->rate<<"\n"; + + // Config src + g_object_set(G_OBJECT(dataSrc), "caps", + gst_caps_new_simple ("video/x-raw", + "format", G_TYPE_STRING, "RGB", + "width", G_TYPE_INT, this->width, + "height", G_TYPE_INT, this->height, + "framerate", GST_TYPE_FRACTION, (unsigned int)this->rate, 1, + NULL), + "is-live", TRUE, + NULL); + + // Config encoder + g_object_set(G_OBJECT(encoder), "bitrate", 800, NULL); + g_object_set(G_OBJECT(encoder), "speed-preset", 2, NULL); //lower = faster, 6=medium + //g_object_set(G_OBJECT(encoder), "tune", "zerolatency", NULL); + //g_object_set(G_OBJECT(encoder), "low-latency", 1, NULL); + //g_object_set(G_OBJECT(encoder), "control-rate", 2, NULL); + + // Config payload + g_object_set(G_OBJECT(payload), "config-interval", 1, NULL); + + // Config udpsink + g_object_set(G_OBJECT(sink), "host", "127.0.0.1", NULL); + g_object_set(G_OBJECT(sink), "port", this->udpPort, NULL); + //g_object_set(G_OBJECT(sink), "sync", false, NULL); + //g_object_set(G_OBJECT(sink), "async", false, NULL); + + // Connect all elements to pipeline + gst_bin_add_many(GST_BIN(pipeline), dataSrc, conv, encoder, parser, payload, sink, NULL); + + // Link all elements + if (gst_element_link_many(dataSrc, conv, encoder, parser, payload, sink, NULL) != TRUE) { + gzerr << "ERR: Link all the elements failed. \n"; + return; + } + + // Set up appsrc + g_object_set(G_OBJECT(dataSrc), "stream-type", 0, "format", GST_FORMAT_TIME, NULL); + g_signal_connect(dataSrc, "need-data", G_CALLBACK(cb_need_data), this); + + // Start + gst_element_set_state(pipeline, GST_STATE_PLAYING); + g_main_loop_run(mainLoop); + + // Clean up + gst_element_set_state(pipeline, GST_STATE_NULL); + gst_object_unref(GST_OBJECT(pipeline)); + g_main_loop_unref(mainLoop); + mainLoop = nullptr; + +} + +///////////////////////////////////////////////// +GstCameraPlugin::GstCameraPlugin() +: SensorPlugin(), width(0), height(0), depth(0), frameBuffer(nullptr), mainLoop(nullptr), + gstTimestamp(0) +{ +} + +///////////////////////////////////////////////// +GstCameraPlugin::~GstCameraPlugin() +{ + this->parentSensor.reset(); + this->camera.reset(); + if (mainLoop) { + g_main_loop_quit(mainLoop); + } + std::lock_guard guard(frameBufferMutex); + if (frameBuffer) { + gst_buffer_unref(frameBuffer); + frameBuffer = nullptr; + } +} + +///////////////////////////////////////////////// +void GstCameraPlugin::Load(sensors::SensorPtr sensor, sdf::ElementPtr sdf) +{ + if(kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!sensor) + gzerr << "Invalid sensor pointer.\n"; + + this->parentSensor = + std::dynamic_pointer_cast(sensor); + + if (!this->parentSensor) + { + gzerr << "GstCameraPlugin requires a CameraSensor.\n"; + + if (std::dynamic_pointer_cast(sensor)) + gzmsg << "It is a depth camera sensor\n"; + } + + this->camera = this->parentSensor->Camera(); + + if (!this->parentSensor) + { + gzerr << "GstCameraPlugin not attached to a camera sensor\n"; + return; + } + + this->width = this->camera->ImageWidth(); + this->height = this->camera->ImageHeight(); + this->depth = this->camera->ImageDepth(); + this->format = this->camera->ImageFormat(); + this->rate = this->camera->RenderRate(); + + if (!isfinite(this->rate)) { + this->rate = 60.0; + } + + if (sdf->HasElement("robotNamespace")) + namespace_ = sdf->GetElement("robotNamespace")->Get(); + else + gzwarn << "[gazebo_gst_camera_plugin] Please specify a robotNamespace.\n"; + + this->udpPort = 5600; + if (sdf->HasElement("udpPort")) { + this->udpPort = sdf->GetElement("udpPort")->Get(); + } + + node_handle_ = transport::NodePtr(new transport::Node()); + node_handle_->Init(namespace_); + + + this->newFrameConnection = this->camera->ConnectNewImageFrame( + boost::bind(&GstCameraPlugin::OnNewFrame, this, _1, this->width, this->height, this->depth, this->format)); + + this->parentSensor->SetActive(true); + + /* start the gstreamer event loop */ + pthread_t threadId; + pthread_create(&threadId, NULL, start_thread, this); +} + +///////////////////////////////////////////////// +void GstCameraPlugin::OnNewFrame(const unsigned char * image, + unsigned int width, + unsigned int height, + unsigned int depth, + const std::string &format) +{ + + image = this->camera->ImageData(0); + + std::lock_guard guard(frameBufferMutex); + + if (frameBuffer) { + gst_buffer_unref(frameBuffer); + } + + // Alloc buffer + guint size = width * height * 3; + frameBuffer = gst_buffer_new_allocate(NULL, size, NULL); + + GstMapInfo mapInfo; + if (gst_buffer_map(frameBuffer, &mapInfo, GST_MAP_WRITE)) { + memcpy(mapInfo.data, image, size); + gst_buffer_unmap(frameBuffer, &mapInfo); + } else { + gzerr << "gst_buffer_map failed"< +#include +#include +#include +#include "gazebo/transport/transport.hh" +#include "gazebo/msgs/msgs.hh" + +#include +#include +#include +#include +#include +#include + +#include "rotors_gazebo_plugins/common.h" + +using namespace gazebo; +using namespace std; + +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboLidarPlugin) + + +GazeboLidarPlugin::GazeboLidarPlugin() +{ +} + + +GazeboLidarPlugin::~GazeboLidarPlugin() +{ + this->newLaserScansConnection.reset(); + + this->parentSensor.reset(); + this->world.reset(); +} + + +void GazeboLidarPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + if(kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + // Get then name of the parent sensor + this->parentSensor = + std::dynamic_pointer_cast(_parent); + + if (!this->parentSensor) + gzthrow("RayPlugin requires a Ray Sensor as its parent"); + + this->world = physics::get_world(this->parentSensor->WorldName()); + + this->newLaserScansConnection = + this->parentSensor->LaserShape()->ConnectNewLaserScans( + boost::bind(&GazeboLidarPlugin::OnNewLaserScans, this)); + + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzwarn << "[gazebo_lidar_plugin] Please specify a robotNamespace.\n"; + + node_handle_ = transport::NodePtr(new transport::Node()); + node_handle_->Init(namespace_); + + const string scopedName = _parent->ParentName(); + + string topicName = "~/" + scopedName + "/lidar"; + boost::replace_all(topicName, "::", "/"); + + lidar_pub_ = node_handle_->Advertise(topicName, 10); +} + + +void GazeboLidarPlugin::OnNewLaserScans() +{ + lidar_message.set_time_msec(0); + lidar_message.set_min_distance(parentSensor->RangeMin()); + lidar_message.set_max_distance(parentSensor->RangeMax()); + lidar_message.set_current_distance(parentSensor->Range(0)); + + lidar_pub_->Publish(lidar_message); +} diff --git a/rotors_gazebo_plugins/src/external/gazebo_optical_flow_plugin.cpp b/rotors_gazebo_plugins/src/external/gazebo_optical_flow_plugin.cpp new file mode 100644 index 0000000..8598551 --- /dev/null +++ b/rotors_gazebo_plugins/src/external/gazebo_optical_flow_plugin.cpp @@ -0,0 +1,168 @@ + +/* + * Copyright (C) 2012-2015 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifdef _WIN32 + // Ensure that Winsock2.h is included before Windows.h, which can get + // pulled in by anybody (e.g., Boost). +#include +#endif + +#include "gazebo/sensors/DepthCameraSensor.hh" + +#include "rotors_gazebo_plugins/common.h" +#include "rotors_gazebo_plugins/external/gazebo_optical_flow_plugin.h" + +#include +#include +#include +#include +#include + +using namespace cv; +using namespace std; + +using namespace gazebo; +GZ_REGISTER_SENSOR_PLUGIN(OpticalFlowPlugin) + +///////////////////////////////////////////////// +OpticalFlowPlugin::OpticalFlowPlugin() +: SensorPlugin(), width(0), height(0), depth(0), timer_() +{ + +} + +///////////////////////////////////////////////// +OpticalFlowPlugin::~OpticalFlowPlugin() +{ + this->parentSensor.reset(); + this->camera.reset(); +} + +///////////////////////////////////////////////// +void OpticalFlowPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) +{ + if(kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!_sensor) + gzerr << "Invalid sensor pointer.\n"; + + this->parentSensor = + std::dynamic_pointer_cast(_sensor); + + if (!this->parentSensor) + { + gzerr << "OpticalFlowPlugin requires a CameraSensor.\n"; + if (std::dynamic_pointer_cast(_sensor)) + gzmsg << "It is a depth camera sensor\n"; + } + + this->camera = this->parentSensor->Camera(); + + if (!this->parentSensor) + { + gzerr << "OpticalFlowPlugin not attached to a camera sensor\n"; + return; + } + + this->width = this->camera->ImageWidth(); + this->height = this->camera->ImageHeight(); + this->depth = this->camera->ImageDepth(); + this->format = this->camera->ImageFormat(); + + if (this->width != 64 || this->height != 64) { + gzerr << "[gazebo_optical_flow_plugin] Incorrect image size, must by 64 x 64.\n"; + } + + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzwarn << "[gazebo_optical_flow_plugin] Please specify a robotNamespace.\n"; + + node_handle_ = transport::NodePtr(new transport::Node()); + node_handle_->Init(namespace_); + + const string scopedName = _sensor->ParentName(); + + string topicName = "~/" + scopedName + "/opticalFlow"; + boost::replace_all(topicName, "::", "/"); + + opticalFlow_pub_ = node_handle_->Advertise(topicName, 10); + + hfov = float(this->camera->HFOV().Radian()); + first_frame_time = this->camera->LastRenderWallTime().Double(); + + old_frame_time = first_frame_time; + focal_length = (this->width/2)/tan(hfov/2); + + this->newFrameConnection = this->camera->ConnectNewImageFrame( + boost::bind(&OpticalFlowPlugin::OnNewFrame, this, _1, this->width, this->height, this->depth, this->format)); + + this->parentSensor->SetActive(true); + + //init flow + const int ouput_rate = 20; // -1 means use rate of camera + _optical_flow = new OpticalFlowOpenCV(focal_length, focal_length, ouput_rate); + // _optical_flow = new OpticalFlowPX4(focal_length, focal_length, ouput_rate, this->width); + +} + +///////////////////////////////////////////////// +void OpticalFlowPlugin::OnNewFrame(const unsigned char * _image, + unsigned int _width, + unsigned int _height, + unsigned int _depth, + const std::string &_format) +{ + + rate = this->camera->AvgFPS(); + _image = this->camera->ImageData(0); + frame_time = this->camera->LastRenderWallTime().Double(); + + frame_time_us = (frame_time - first_frame_time) * 1e6; //since start + + timer_.stop(); + + float flow_x_ang = 0; + float flow_y_ang = 0; + //calculate angular flow + int quality = _optical_flow->calcFlow((uint8_t *)_image, frame_time_us, dt_us, flow_x_ang, flow_y_ang); + + + + if (quality >= 0) { // calcFlow(...) returns -1 if data should not be published yet -> output_rate + //prepare optical flow message + opticalFlow_message.set_time_usec(0);//will be filled in simulator_mavlink.cpp + opticalFlow_message.set_sensor_id(2.0); + opticalFlow_message.set_integration_time_us(dt_us); + opticalFlow_message.set_integrated_x(flow_x_ang); + opticalFlow_message.set_integrated_y(flow_y_ang); + opticalFlow_message.set_integrated_xgyro(0.0); //get real values in gazebo_mavlink_interface.cpp + opticalFlow_message.set_integrated_ygyro(0.0); //get real values in gazebo_mavlink_interface.cpp + opticalFlow_message.set_integrated_zgyro(0.0); //get real values in gazebo_mavlink_interface.cpp + opticalFlow_message.set_temperature(20.0); + opticalFlow_message.set_quality(quality); + opticalFlow_message.set_time_delta_distance_us(0.0); + opticalFlow_message.set_distance(0.0); //get real values in gazebo_mavlink_interface.cpp + //send message + opticalFlow_pub_->Publish(opticalFlow_message); + timer_.start(); + } +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=2 ts=2 : */ diff --git a/rotors_gazebo_plugins/src/gazebo_bag_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_bag_plugin.cpp new file mode 100644 index 0000000..30dcb25 --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_bag_plugin.cpp @@ -0,0 +1,422 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "rotors_gazebo_plugins/gazebo_bag_plugin.h" + +#include + +#include + +namespace gazebo { + +GazeboBagPlugin::~GazeboBagPlugin() { + + if (node_handle_) { + node_handle_->shutdown(); + delete node_handle_; + } + bag_.close(); +} + +void GazeboBagPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + // Store the pointer to the model + model_ = _model; + // world_ = physics::get_world(model_->world.name); + world_ = model_->GetWorld(); + + if (_sdf->HasElement("robotNamespace")) { + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + } else { + gzerr << "[gazebo_bag_plugin] Please specify a robotNamespace.\n"; + } + node_handle_ = new ros::NodeHandle(namespace_); + + if (_sdf->HasElement("bagFileName")) { + bag_filename_ = _sdf->GetElement("bagFileName")->Get(); + } else { + gzerr << "[gazebo_bag_plugin] Please specify a bagFileName.\n"; + } + + if (_sdf->HasElement("linkName")) { + link_name_ = _sdf->GetElement("linkName")->Get(); + } else { + gzwarn << "[gazebo_bag_plugin] No linkName specified, using default " + << link_name_ << ".\n"; + } + + // Get the pointer to the link + link_ = model_->GetLink(link_name_); + if (link_ == NULL) { + gzthrow("[gazebo_bag_plugin] Couldn't find specified link \"" << link_name_ + << "\"."); + } + + getSdfParam(_sdf, "frameId", frame_id_, frame_id_); + getSdfParam(_sdf, "imuTopic", imu_topic_, imu_topic_); + getSdfParam(_sdf, "commandAttitudeThrustTopic", + control_attitude_thrust_topic_, + control_attitude_thrust_topic_); + getSdfParam(_sdf, "commandMotorSpeedTopic", + control_motor_speed_topic_, + control_motor_speed_topic_); + getSdfParam(_sdf, "commandRateThrustTopic", + control_rate_thrust_topic_, + control_rate_thrust_topic_); + getSdfParam(_sdf, "windSpeedTopic", + wind_speed_topic_, wind_speed_topic_); + getSdfParam(_sdf, "motorTopic", motor_topic_, motor_topic_); + getSdfParam(_sdf, "poseTopic", ground_truth_pose_topic_, + ground_truth_pose_topic_); + getSdfParam(_sdf, "twistTopic", ground_truth_twist_topic_, + ground_truth_twist_topic_); + getSdfParam(_sdf, "wrenchesTopic", wrench_topic_, + wrench_topic_); + getSdfParam(_sdf, "externalForceTopic", external_force_topic_, + external_force_topic_); + getSdfParam(_sdf, "waypointTopic", waypoint_topic_, + waypoint_topic_); + getSdfParam(_sdf, "commandPoseTopic", command_pose_topic_, + command_pose_topic_); + getSdfParam(_sdf, "recordingServiceName", + recording_service_name_, recording_service_name_); + + getSdfParam(_sdf, "rotorVelocitySlowdownSim", + rotor_velocity_slowdown_sim_, + rotor_velocity_slowdown_sim_); + + getSdfParam(_sdf, "waitToRecordBag", wait_to_record_, wait_to_record_); + + recording_service_ = node_handle_->advertiseService( + recording_service_name_, &GazeboBagPlugin::RecordingServiceCallback, + this); + + // Get the motor joints. + child_links_ = link_->GetChildJointsLinks(); + for (unsigned int i = 0; i < child_links_.size(); i++) { + std::string link_name = child_links_[i]->GetScopedName(); + + // Check if the link contains rotor_ in its name. + int pos = link_name.find("rotor_"); + if (pos != link_name.npos) { + std::string motor_number_str = link_name.substr(pos + 6); + unsigned int motor_number = std::stoi(motor_number_str); + std::string joint_name = child_links_[i]->GetName() + "_joint"; + physics::JointPtr joint = model_->GetJoint(joint_name); + motor_joints_.insert(MotorNumberToJointPair(motor_number, joint)); + } + } + + // Get the contact manager. + std::vector collisions; + contact_mgr_ = world_->Physics()->GetContactManager(); + for (unsigned int i = 0; i < link_->GetCollisions().size(); ++i) { + physics::CollisionPtr collision = link_->GetCollision(i); + collisions.push_back(collision->GetScopedName()); + } + for (unsigned int j = 0; j < child_links_.size(); ++j) { + unsigned int zero = 0; + for (unsigned int i = 0; i < child_links_[j]->GetCollisions().size(); ++i) { + collisions.push_back(child_links_[j]->GetCollision(i)->GetScopedName()); + } + } + + if (!collisions.empty()) { + contact_mgr_->CreateFilter(link_->GetName(), collisions); + } + + // If we do not need to wait for user command, we start recording right away + if (!wait_to_record_) StartRecording(); +} + +// This gets called by the world update start event. +void GazeboBagPlugin::OnUpdate(const common::UpdateInfo& _info) { + if (kPrintOnUpdates) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + // Get the current simulation time. + common::Time now = world_->SimTime(); + LogWrenches(now); + LogGroundTruth(now); + LogMotorVelocities(now); +} + +void GazeboBagPlugin::StartRecording() { + time_t rawtime; + struct tm* timeinfo; + char buffer[80]; + + time(&rawtime); + timeinfo = localtime(&rawtime); + + strftime(buffer, 80, "%Y-%m-%d-%H-%M-%S", timeinfo); + std::string date_time_str(buffer); + + std::string key(".bag"); + size_t pos = bag_filename_.rfind(key); + if (pos != std::string::npos) { + bag_filename_.erase(pos, key.length()); + } + std::string full_bag_filename = bag_filename_ + "_" + date_time_str + ".bag"; + + // Open a bag file and store it in ~/.ros/. + bag_.open(full_bag_filename, rosbag::bagmode::Write); + + // Subscriber to IMU sensor_msgs::Imu Message. + imu_sub_ = node_handle_->subscribe(imu_topic_, 10, + &GazeboBagPlugin::ImuCallback, this); + + // Subscriber to External Force WrenchStamped Message. + external_force_sub_ = node_handle_->subscribe(external_force_topic_, 10, + &GazeboBagPlugin::ExternalForceCallback, this); + + // Subscriber to Waypoint MultiDOFJointTrajectory Message. + waypoint_sub_ = node_handle_->subscribe( + waypoint_topic_, 10, &GazeboBagPlugin::WaypointCallback, this); + + // Subscriber to PoseStamped pose command message. + command_pose_sub_ = node_handle_->subscribe( + command_pose_topic_, 10, &GazeboBagPlugin::CommandPoseCallback, this); + + // Subscriber to Control Attitude Thrust Message. + control_attitude_thrust_sub_ = + node_handle_->subscribe(control_attitude_thrust_topic_, 10, + &GazeboBagPlugin::AttitudeThrustCallback, this); + + // Subscriber to Control Motor Speed Message. + control_motor_speed_sub_ = + node_handle_->subscribe(control_motor_speed_topic_, 10, + &GazeboBagPlugin::ActuatorsCallback, this); + + // Subscriber to Control Rate Thrust Message. + control_rate_thrust_sub_ = + node_handle_->subscribe(control_rate_thrust_topic_, 10, + &GazeboBagPlugin::RateThrustCallback, this); + + // Subscriber to Wind Speed Message. + wind_speed_sub_ = + node_handle_->subscribe(wind_speed_topic_, 10, + &GazeboBagPlugin::WindSpeedCallback, this); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboBagPlugin::OnUpdate, this, _1)); + + // Set the flag that we are actively recording. + is_recording_ = true; + + ROS_INFO("GazeboBagPlugin START recording bagfile %s", + full_bag_filename.c_str()); +} + +void GazeboBagPlugin::StopRecording() { + // Shutdown all the subscribers. + imu_sub_.shutdown(); + external_force_sub_.shutdown(); + waypoint_sub_.shutdown(); + command_pose_sub_.shutdown(); + control_attitude_thrust_sub_.shutdown(); + control_motor_speed_sub_.shutdown(); + control_rate_thrust_sub_.shutdown(); + wind_speed_sub_.shutdown(); + + // Disconnect the update event. + + + // Close the bag. + bag_.close(); + + // Clear the flag to show that we are not actively recording + is_recording_ = false; + + ROS_INFO("GazeboBagPlugin STOP recording bagfile"); +} + +void GazeboBagPlugin::ImuCallback(const sensor_msgs::ImuConstPtr& imu_msg) { + common::Time now = world_->SimTime(); + ros::Time ros_now = ros::Time(now.sec, now.nsec); + writeBag(namespace_ + "/" + imu_topic_, ros_now, imu_msg); +} + +void GazeboBagPlugin::ExternalForceCallback( + const geometry_msgs::WrenchStampedConstPtr& force_msg) { + common::Time now = world_->SimTime(); + ros::Time ros_now = ros::Time(now.sec, now.nsec); + writeBag(namespace_ + "/" + external_force_topic_, ros_now, force_msg); +} + +void GazeboBagPlugin::WaypointCallback( + const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_msg) { + common::Time now = world_->SimTime(); + ros::Time ros_now = ros::Time(now.sec, now.nsec); + writeBag(namespace_ + "/" + waypoint_topic_, ros_now, trajectory_msg); +} + +void GazeboBagPlugin::CommandPoseCallback( + const geometry_msgs::PoseStampedConstPtr& pose_msg) { + common::Time now = world_->SimTime(); + ros::Time ros_now = ros::Time(now.sec, now.nsec); + writeBag(namespace_ + "/" + command_pose_topic_, ros_now, pose_msg); +} + +void GazeboBagPlugin::AttitudeThrustCallback( + const mav_msgs::AttitudeThrustConstPtr& control_msg) { + common::Time now = world_->SimTime(); + ros::Time ros_now = ros::Time(now.sec, now.nsec); + writeBag(namespace_ + "/" + control_attitude_thrust_topic_, ros_now, + control_msg); +} + +void GazeboBagPlugin::ActuatorsCallback( + const mav_msgs::ActuatorsConstPtr& control_msg) { + common::Time now = world_->SimTime(); + ros::Time ros_now = ros::Time(now.sec, now.nsec); + writeBag(namespace_ + "/" + control_motor_speed_topic_, ros_now, control_msg); +} + +void GazeboBagPlugin::RateThrustCallback( + const mav_msgs::RateThrustConstPtr& control_msg) { + common::Time now = world_->SimTime(); + ros::Time ros_now = ros::Time(now.sec, now.nsec); + writeBag(namespace_ + "/" + control_rate_thrust_topic_, ros_now, control_msg); +} + +void GazeboBagPlugin::WindSpeedCallback( + const rotors_comm::WindSpeedConstPtr& wind_speed_msg) { + common::Time now = world_->SimTime(); + ros::Time ros_now = ros::Time(now.sec, now.nsec); + writeBag(namespace_ + "/" + wind_speed_topic_, ros_now, wind_speed_msg); +} + +void GazeboBagPlugin::LogMotorVelocities(const common::Time now) { + ros::Time ros_now = ros::Time(now.sec, now.nsec); + + mav_msgs::Actuators rot_velocities_msg; + rot_velocities_msg.angular_velocities.resize(motor_joints_.size()); + + MotorNumberToJointMap::iterator m; + for (m = motor_joints_.begin(); m != motor_joints_.end(); ++m) { + double motor_rot_vel = + m->second->GetVelocity(0) * rotor_velocity_slowdown_sim_; + rot_velocities_msg.angular_velocities[m->first] = motor_rot_vel; + } + rot_velocities_msg.header.stamp.sec = now.sec; + rot_velocities_msg.header.stamp.nsec = now.nsec; + + writeBag(namespace_ + "/" + motor_topic_, ros_now, rot_velocities_msg); +} + +void GazeboBagPlugin::LogGroundTruth(const common::Time now) { + ros::Time ros_now = ros::Time(now.sec, now.nsec); + + geometry_msgs::PoseStamped pose_msg; + geometry_msgs::TwistStamped twist_msg; + + // Get pose and update the message. + ignition::math::Pose3d pose = link_->WorldPose(); + pose_msg.header.frame_id = frame_id_; + pose_msg.header.stamp.sec = now.sec; + pose_msg.header.stamp.nsec = now.nsec; + pose_msg.pose.position.x = pose.Pos().X(); + pose_msg.pose.position.y = pose.Pos().Y(); + pose_msg.pose.position.z = pose.Pos().Z(); + pose_msg.pose.orientation.w = pose.Rot().W(); + pose_msg.pose.orientation.x = pose.Rot().X(); + pose_msg.pose.orientation.y = pose.Rot().Y(); + pose_msg.pose.orientation.z = pose.Rot().Z(); + + writeBag(namespace_ + "/" + ground_truth_pose_topic_, ros_now, pose_msg); + + // Get twist and update the message. + ignition::math::Vector3d linear_veloctiy = link_->WorldLinearVel(); + ignition::math::Vector3d angular_veloctiy = link_->WorldAngularVel(); + twist_msg.header.frame_id = frame_id_; + twist_msg.header.stamp.sec = now.sec; + twist_msg.header.stamp.nsec = now.nsec; + twist_msg.twist.linear.x = linear_veloctiy.X(); + twist_msg.twist.linear.y = linear_veloctiy.Y(); + twist_msg.twist.linear.z = linear_veloctiy.Z(); + twist_msg.twist.angular.x = angular_veloctiy.X(); + twist_msg.twist.angular.y = angular_veloctiy.Y(); + twist_msg.twist.angular.z = angular_veloctiy.Z(); + + writeBag(namespace_ + "/" + ground_truth_twist_topic_, ros_now, twist_msg); +} + +void GazeboBagPlugin::LogWrenches(const common::Time now) { + geometry_msgs::WrenchStamped wrench_msg; + std::vector contacts = contact_mgr_->GetContacts(); + for (int i = 0; i < contact_mgr_->GetContactCount(); ++i) { + std::string collision2_name = + contacts[i]->collision2->GetLink()->GetScopedName(); + double body1_force = contacts[i]->wrench->body1Force.Length(); + + // Exclude extremely small forces. + if (body1_force < 1e-10) continue; + // Do this, such that all the contacts are logged. + // (publishing on the same topic with the same time stamp is impossible) + ros::Time ros_now = ros::Time(now.sec, now.nsec + i * 1000); + std::string collision1_name = + contacts[i]->collision1->GetLink()->GetScopedName(); + wrench_msg.header.frame_id = collision1_name + "--" + collision2_name; + wrench_msg.header.stamp.sec = now.sec; + wrench_msg.header.stamp.nsec = now.nsec; + wrench_msg.wrench.force.x = contacts[i]->wrench->body1Force.X(); + wrench_msg.wrench.force.y = contacts[i]->wrench->body1Force.Y(); + wrench_msg.wrench.force.z = contacts[i]->wrench->body1Force.Z(); + wrench_msg.wrench.torque.x = contacts[i]->wrench->body1Torque.X(); + wrench_msg.wrench.torque.y = contacts[i]->wrench->body1Torque.Y(); + wrench_msg.wrench.torque.z = contacts[i]->wrench->body1Torque.Z(); + + writeBag(namespace_ + "/" + wrench_topic_, ros_now, wrench_msg); + } +} + +bool GazeboBagPlugin::RecordingServiceCallback( + rotors_comm::RecordRosbag::Request& req, + rotors_comm::RecordRosbag::Response& res) { + if (req.record && !is_recording_) { + StartRecording(); + res.success = true; + } else if (req.record && is_recording_) { + gzwarn << "[gazebo_bag_plugin] Already recording rosbag, ignoring start " + "command.\n"; + res.success = false; + } else if (!req.record && is_recording_) { + StopRecording(); + res.success = true; + } else if (!req.record && !is_recording_) { + gzwarn << "[gazebo_bag_plugin] Already not recording rosbag, ignoring stop " + "command.\n"; + res.success = false; + } + + return res.success; +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboBagPlugin); + +} // namespace gazebo diff --git a/rotors_gazebo_plugins/src/gazebo_controller_interface.cpp b/rotors_gazebo_plugins/src/gazebo_controller_interface.cpp new file mode 100644 index 0000000..c178840 --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_controller_interface.cpp @@ -0,0 +1,185 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "rotors_gazebo_plugins/gazebo_controller_interface.h" + +#include "ConnectGazeboToRosTopic.pb.h" +#include "ConnectRosToGazeboTopic.pb.h" + +namespace gazebo { + +GazeboControllerInterface::~GazeboControllerInterface() { +} + +void GazeboControllerInterface::Load(physics::ModelPtr _model, + sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + // Store the pointer to the model. + model_ = _model; + + world_ = model_->GetWorld(); + + namespace_.clear(); + + //==============================================// + //========== READ IN PARAMS FROM SDF ===========// + //==============================================// + + if (_sdf->HasElement("robotNamespace")) { + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + } else { + gzerr << "[gazebo_motor_model] Please specify a robotNamespace.\n"; + } + + node_handle_ = gazebo::transport::NodePtr(new transport::Node()); + + // Initialise with default namespace (typically /gazebo/default/) + node_handle_->Init(); + + getSdfParam(_sdf, "commandMotorSpeedSubTopic", + command_motor_speed_sub_topic_, + command_motor_speed_sub_topic_); + getSdfParam(_sdf, "motorSpeedCommandPubTopic", + motor_velocity_reference_pub_topic_, + motor_velocity_reference_pub_topic_); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + updateConnection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboControllerInterface::OnUpdate, this, _1)); +} + +void GazeboControllerInterface::OnUpdate(const common::UpdateInfo& /*_info*/) { + if (kPrintOnUpdates) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!pubs_and_subs_created_) { + CreatePubsAndSubs(); + pubs_and_subs_created_ = true; + } + + if (!received_first_reference_) { + return; + } + + common::Time now = world_->SimTime(); + + gz_sensor_msgs::Actuators turning_velocities_msg; + + for (int i = 0; i < input_reference_.size(); i++) { + turning_velocities_msg.add_angular_velocities((double)input_reference_[i]); + } + + turning_velocities_msg.mutable_header()->mutable_stamp()->set_sec(now.sec); + turning_velocities_msg.mutable_header()->mutable_stamp()->set_nsec(now.nsec); + + // Frame ID is not used for this particular message + turning_velocities_msg.mutable_header()->set_frame_id(""); + + motor_velocity_reference_pub_->Publish(turning_velocities_msg); +} + +void GazeboControllerInterface::CreatePubsAndSubs() { + gzdbg << __FUNCTION__ << "() called." << std::endl; + + // Create temporary "ConnectGazeboToRosTopic" publisher and message + gazebo::transport::PublisherPtr gz_connect_gazebo_to_ros_topic_pub = + node_handle_->Advertise( + "~/" + kConnectGazeboToRosSubtopic, 1); + + // Create temporary "ConnectRosToGazeboTopic" publisher and message + gazebo::transport::PublisherPtr gz_connect_ros_to_gazebo_topic_pub = + node_handle_->Advertise( + "~/" + kConnectRosToGazeboSubtopic, 1); + + // ============================================================ // + // === ACTUATORS (MOTOR VELOCITY) MSG SETUP (GAZEBO -> ROS) === // + // ============================================================ // + + // TODO This topic is missing the "~" and is in a completely different + // namespace, fix? + + gzdbg << "GazeboControllerInterface creating Gazebo publisher on \"" + << namespace_ + "/" + motor_velocity_reference_pub_topic_ << "\"." + << std::endl; + motor_velocity_reference_pub_ = + node_handle_->Advertise( + namespace_ + "/" + motor_velocity_reference_pub_topic_, 1); + + // Connect to ROS + gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg; + connect_gazebo_to_ros_topic_msg.set_gazebo_topic( + namespace_ + "/" + motor_velocity_reference_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic( + namespace_ + "/" + motor_velocity_reference_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::ACTUATORS); + gz_connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); + + // ================================================ // + // ===== MOTOR SPEED MSG SETUP (ROS -> GAZEBO) ==== // + // ================================================ // + gzdbg << "Subscribing to Gazebo topic \"" + << "~/" + namespace_ + "/" + command_motor_speed_sub_topic_ << "\"." + << std::endl; + cmd_motor_sub_ = node_handle_->Subscribe( + "~/" + namespace_ + "/" + command_motor_speed_sub_topic_, + &GazeboControllerInterface::CommandMotorCallback, this); + + // Connect to ROS + gz_std_msgs::ConnectRosToGazeboTopic connect_ros_to_gazebo_topic_msg; + connect_ros_to_gazebo_topic_msg.set_ros_topic(namespace_ + "/" + + command_motor_speed_sub_topic_); + // connect_ros_to_gazebo_topic_msg.set_gazebo_namespace(namespace_); + connect_ros_to_gazebo_topic_msg.set_gazebo_topic( + "~/" + namespace_ + "/" + command_motor_speed_sub_topic_); + connect_ros_to_gazebo_topic_msg.set_msgtype( + gz_std_msgs::ConnectRosToGazeboTopic::ACTUATORS); + gz_connect_ros_to_gazebo_topic_pub->Publish(connect_ros_to_gazebo_topic_msg, + true); + + gzdbg << __FUNCTION__ << "() called." << std::endl; +} + +void GazeboControllerInterface::CommandMotorCallback( + GzActuatorsMsgPtr& actuators_msg) { + if (kPrintOnMsgCallback) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + input_reference_.resize(actuators_msg->angular_velocities_size()); + for (int i = 0; i < actuators_msg->angular_velocities_size(); ++i) { + input_reference_[i] = actuators_msg->angular_velocities(i); + } + + // We have received a motor command reference (it may not be the first, but + // this + // does not matter) + received_first_reference_ = true; +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboControllerInterface); +} diff --git a/rotors_gazebo_plugins/src/gazebo_fw_dynamics_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_fw_dynamics_plugin.cpp new file mode 100644 index 0000000..5355e7b --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_fw_dynamics_plugin.cpp @@ -0,0 +1,409 @@ +/* + * Copyright 2017 Pavel Vechersky, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// MODULE HEADER +#include "rotors_gazebo_plugins/gazebo_fw_dynamics_plugin.h" + +#include "ConnectRosToGazeboTopic.pb.h" + +namespace gazebo { + +GazeboFwDynamicsPlugin::GazeboFwDynamicsPlugin() + : ModelPlugin(), + node_handle_(0), + W_wind_speed_W_B_(0, 0, 0), + delta_aileron_left_(0.0), + delta_aileron_right_(0.0), + delta_elevator_(0.0), + delta_flap_(0.0), + delta_rudder_(0.0), + throttle_(0.0), + pubs_and_subs_created_(false) { +} + +GazeboFwDynamicsPlugin::~GazeboFwDynamicsPlugin() { +} + +void GazeboFwDynamicsPlugin::Load(physics::ModelPtr _model, + sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + gzdbg << "_model = " << _model->GetName() << std::endl; + + // Store the pointer to the model. + model_ = _model; + world_ = model_->GetWorld(); + + namespace_.clear(); + + // Get the robot namespace. + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzerr << "[gazebo_fw_dynamics_plugin] Please specify a robotNamespace.\n"; + + // Create the node handle. + node_handle_ = transport::NodePtr(new transport::Node()); + + // Initialise with default namespace (typically /gazebo/default/). + node_handle_->Init(); + + // Get the link name. + std::string link_name; + if (_sdf->HasElement("linkName")) + link_name = _sdf->GetElement("linkName")->Get(); + else + gzerr << "[gazebo_fw_dynamics_plugin] Please specify a linkName.\n"; + // Get the pointer to the link. + link_ = model_->GetLink(link_name); + if (link_ == NULL) { + gzthrow("[gazebo_fw_dynamics_plugin] Couldn't find specified link \"" + << link_name << "\"."); + } + + // Get the path to fixed-wing aerodynamics parameters YAML file. If not + // provided, default Techpod parameters are used. + if (_sdf->HasElement("aeroParamsYAML")) { + std::string aero_params_yaml = + _sdf->GetElement("aeroParamsYAML")->Get(); + + aero_params_.LoadAeroParamsYAML(aero_params_yaml); + } else { + gzwarn << "[gazebo_fw_dynamics_plugin] No aerodynamic paramaters YAML file" + << " specified, using default Techpod parameters.\n"; + } + + // Get the path to fixed-wing vehicle parameters YAML file. If not provided, + // default Techpod parameters are used. + if (_sdf->HasElement("vehicleParamsYAML")) { + std::string vehicle_params_yaml = + _sdf->GetElement("vehicleParamsYAML")->Get(); + + vehicle_params_.LoadVehicleParamsYAML(vehicle_params_yaml); + } else { + gzwarn << "[gazebo_fw_dynamics_plugin] No vehicle paramaters YAML file" + << " specified, using default Techpod parameters.\n"; + } + + // Get the rest of the sdf parameters. + getSdfParam(_sdf, "isInputJoystick", is_input_joystick_, + kDefaultIsInputJoystick); + getSdfParam(_sdf, "actuatorsSubTopic", + actuators_sub_topic_, + mav_msgs::default_topics::COMMAND_ACTUATORS); + getSdfParam(_sdf, "rollPitchYawrateThrustSubTopic", + roll_pitch_yawrate_thrust_sub_topic_, + mav_msgs::default_topics:: + COMMAND_ROLL_PITCH_YAWRATE_THRUST); + getSdfParam(_sdf, "windSpeedSubTopic", + wind_speed_sub_topic_, + mav_msgs::default_topics::WIND_SPEED); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->updateConnection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboFwDynamicsPlugin::OnUpdate, this, _1)); +} + +void GazeboFwDynamicsPlugin::OnUpdate(const common::UpdateInfo& _info) { + if (kPrintOnUpdates) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!pubs_and_subs_created_) { + CreatePubsAndSubs(); + pubs_and_subs_created_ = true; + } + + UpdateForcesAndMoments(); +} + +void GazeboFwDynamicsPlugin::UpdateForcesAndMoments() { + // Express the air speed and angular velocity in the body frame. + // B denotes body frame and W world frame ... e.g., W_rot_W_B denotes + // rotation of B wrt. W expressed in W. + ignition::math::Quaterniond W_rot_W_B = link_->WorldPose().Rot(); + ignition::math::Vector3d B_air_speed_W_B = W_rot_W_B.RotateVectorReverse( + link_->WorldLinearVel() - W_wind_speed_W_B_); + ignition::math::Vector3d B_angular_velocity_W_B = link_->RelativeAngularVel(); + + // Traditionally, fixed-wing aerodynamics use NED (North-East-Down) frame, + // but since our model's body frame is in North-West-Up frame we rotate the + // linear and angular velocities by 180 degrees around the X axis. + double u = B_air_speed_W_B.X(); + double v = -B_air_speed_W_B.Y(); + double w = -B_air_speed_W_B.Z(); + + double p = B_angular_velocity_W_B.X(); + double q = -B_angular_velocity_W_B.Y(); + double r = -B_angular_velocity_W_B.Z(); + + // Compute the angle of attack (alpha) and the sideslip angle (beta). To + // avoid division by zero, there is a minimum air speed threshold below which + // alpha and beta are zero. + double V = B_air_speed_W_B.Length(); + double beta = (V < kMinAirSpeedThresh) ? 0.0 : asin(v / V); + double alpha = (u < kMinAirSpeedThresh) ? 0.0 : atan(w / u); + + // Bound the angle of attack. + if (alpha > aero_params_.alpha_max) + alpha = aero_params_.alpha_max; + else if (alpha < aero_params_.alpha_min) + alpha = aero_params_.alpha_min; + + // Pre-compute the common component in the force and moment calculations. + const double q_bar_S = 0.5 * kAirDensity * V * V * vehicle_params_.wing_surface; + + // Combine some of the control surface deflections. + double aileron_sum = delta_aileron_left_ + delta_aileron_right_; + double aileron_diff = delta_aileron_left_ - delta_aileron_right_; + double flap_sum = 2.0 * delta_flap_; + double flap_diff = 0.0; + + // Compute the forces in the wind frame. + const double drag = q_bar_S * + (aero_params_.c_drag_alpha.dot( + Eigen::Vector3d(1.0, alpha, alpha * alpha)) + + aero_params_.c_drag_beta.dot( + Eigen::Vector3d(0.0, beta, beta * beta)) + + aero_params_.c_drag_delta_ail.dot( + Eigen::Vector3d(0.0, aileron_sum, aileron_sum * aileron_sum)) + + aero_params_.c_drag_delta_flp.dot( + Eigen::Vector3d(0.0, flap_sum, flap_sum * flap_sum))); + + const double side_force = q_bar_S * + (aero_params_.c_side_force_beta.dot( + Eigen::Vector2d(0.0, beta))); + + const double lift = q_bar_S * + (aero_params_.c_lift_alpha.dot( + Eigen::Vector4d(1.0, alpha, alpha * alpha, alpha * alpha * alpha)) + + aero_params_.c_lift_delta_ail.dot( + Eigen::Vector2d(0.0, aileron_sum)) + + aero_params_.c_lift_delta_flp.dot( + Eigen::Vector2d(0.0, flap_sum))); + + const Eigen::Vector3d forces_Wind(-drag, side_force, -lift); + + // Non-dimensionalize the angular rates for inclusion in the computation of + // moments. To avoid division by zero, there is a minimum air speed threshold + // below which the values are zero. + const double p_hat = (V < kMinAirSpeedThresh) ? 0.0 : + p * vehicle_params_.wing_span / (2.0 * V); + const double q_hat = (V < kMinAirSpeedThresh) ? 0.0 : + q * vehicle_params_.chord_length / (2.0 * V); + const double r_hat = (V < kMinAirSpeedThresh) ? 0.0 : + r * vehicle_params_.wing_span / (2.0 * V); + + // Compute the moments in the wind frame. + const double rolling_moment = q_bar_S * vehicle_params_.wing_span * + (aero_params_.c_roll_moment_beta.dot( + Eigen::Vector2d(0.0, beta)) + + aero_params_.c_roll_moment_p.dot( + Eigen::Vector2d(0.0, p_hat)) + + aero_params_.c_roll_moment_r.dot( + Eigen::Vector2d(0.0, r_hat)) + + aero_params_.c_roll_moment_delta_ail.dot( + Eigen::Vector2d(0.0, aileron_diff)) + + aero_params_.c_roll_moment_delta_flp.dot( + Eigen::Vector2d(0.0, flap_diff))); + + const double pitching_moment = q_bar_S * vehicle_params_.chord_length * + (aero_params_.c_pitch_moment_alpha.dot( + Eigen::Vector2d(1.0, alpha)) + + aero_params_.c_pitch_moment_q.dot( + Eigen::Vector2d(0.0, q_hat)) + + aero_params_.c_pitch_moment_delta_elv.dot( + Eigen::Vector2d(0.0, delta_elevator_))); + + const double yawing_moment = q_bar_S * vehicle_params_.wing_span * + (aero_params_.c_yaw_moment_beta.dot( + Eigen::Vector2d(0.0, beta)) + + aero_params_.c_yaw_moment_r.dot( + Eigen::Vector2d(0.0, r_hat)) + + aero_params_.c_yaw_moment_delta_rud.dot( + Eigen::Vector2d(0.0, delta_rudder_))); + + const Eigen::Vector3d moments_Wind(rolling_moment, + pitching_moment, + yawing_moment); + + // Compute the thrust force in the body frame. + const double thrust = aero_params_.c_thrust.dot( + Eigen::Vector3d(1.0, throttle_, throttle_ * throttle_)); + + const Eigen::Vector3d force_thrust_B = thrust * Eigen::Vector3d( + cos(vehicle_params_.thrust_inclination), + 0.0, + sin(vehicle_params_.thrust_inclination)); + + // Compute the transform between the body frame and the wind frame. + double ca = cos(alpha); + double sa = sin(alpha); + double cb = cos(beta); + double sb = sin(beta); + + Eigen::Matrix3d R_Wind_B; + R_Wind_B << ca * cb, sb, sa * cb, + -sb * ca, cb, -sa * sb, + -sa, 0.0, ca; + + const Eigen::Matrix3d R_Wind_B_t = R_Wind_B.transpose(); + + // Transform all the forces and moments into the body frame + const Eigen::Vector3d forces_B = R_Wind_B_t * forces_Wind + force_thrust_B; + const Eigen::Vector3d moments_B = R_Wind_B_t * moments_Wind; + + // Once again account for the difference between our body frame orientation + // and the traditional aerodynamics frame. + const ignition::math::Vector3d forces = + ignition::math::Vector3d (forces_B[0], -forces_B[1], -forces_B[2]); + const ignition::math::Vector3d moments = + ignition::math::Vector3d (moments_B[0], -moments_B[1], -moments_B[2]); + + // Apply the calculated forced and moments to the main body link. + link_->AddRelativeForce(forces); + link_->AddRelativeTorque(moments); +} + +double GazeboFwDynamicsPlugin::NormalizedInputToAngle( + const ControlSurface& surface, double input) { + return (surface.deflection_max + surface.deflection_min) * 0.5 + + (surface.deflection_max - surface.deflection_min) * 0.5 * input; +} + +void GazeboFwDynamicsPlugin::CreatePubsAndSubs() { + gzdbg << __PRETTY_FUNCTION__ << " called." << std::endl; + + // Create temporary "ConnectRosToGazeboTopic" publisher and message + gazebo::transport::PublisherPtr gz_connect_ros_to_gazebo_topic_pub = + node_handle_->Advertise( + "~/" + kConnectRosToGazeboSubtopic, 1); + gz_std_msgs::ConnectRosToGazeboTopic connect_ros_to_gazebo_topic_msg; + + // ============================================ // + // ==== WIND SPEED MSG SETUP (ROS->GAZEBO) ==== // + // ============================================ // + + // Wind speed subscriber (Gazebo). + wind_speed_sub_ = + node_handle_->Subscribe("~/" + namespace_ + "/" + wind_speed_sub_topic_, + &GazeboFwDynamicsPlugin::WindSpeedCallback, + this); + + connect_ros_to_gazebo_topic_msg.set_ros_topic(namespace_ + "/" + + wind_speed_sub_topic_); + connect_ros_to_gazebo_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + wind_speed_sub_topic_); + connect_ros_to_gazebo_topic_msg.set_msgtype( + gz_std_msgs::ConnectRosToGazeboTopic::WIND_SPEED); + gz_connect_ros_to_gazebo_topic_pub->Publish(connect_ros_to_gazebo_topic_msg, + true); + + // If we are using a joystick for control inputs then subscribe to the + // RollPitchYawrateThrust msgs, otherwise subscribe to the Actuator msgs. + if (is_input_joystick_) { + // ========================================================= // + // === ROLL_PITCH_YAWRATE_THRUST MSG SETUP (ROS->GAZEBO) === // + // ========================================================= // + + roll_pitch_yawrate_thrust_sub_ = + node_handle_->Subscribe("~/" + namespace_ + "/" + + roll_pitch_yawrate_thrust_sub_topic_, + &GazeboFwDynamicsPlugin::RollPitchYawrateThrustCallback, this); + + connect_ros_to_gazebo_topic_msg.set_ros_topic(namespace_ + "/" + + roll_pitch_yawrate_thrust_sub_topic_); + connect_ros_to_gazebo_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + roll_pitch_yawrate_thrust_sub_topic_); + connect_ros_to_gazebo_topic_msg.set_msgtype( + gz_std_msgs::ConnectRosToGazeboTopic::ROLL_PITCH_YAWRATE_THRUST); + } else { + // ============================================ // + // ===== ACTUATORS MSG SETUP (ROS->GAZEBO) ==== // + // ============================================ // + + actuators_sub_ = + node_handle_->Subscribe("~/" + namespace_ + "/" + actuators_sub_topic_, + &GazeboFwDynamicsPlugin::ActuatorsCallback, + this); + + connect_ros_to_gazebo_topic_msg.set_ros_topic(namespace_ + "/" + + actuators_sub_topic_); + connect_ros_to_gazebo_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + actuators_sub_topic_); + connect_ros_to_gazebo_topic_msg.set_msgtype( + gz_std_msgs::ConnectRosToGazeboTopic::ACTUATORS); + } + + gz_connect_ros_to_gazebo_topic_pub->Publish(connect_ros_to_gazebo_topic_msg, + true); +} + +void GazeboFwDynamicsPlugin::ActuatorsCallback( + GzActuatorsMsgPtr &actuators_msg) { + if (kPrintOnMsgCallback) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + delta_aileron_left_ = NormalizedInputToAngle(vehicle_params_.aileron_left, + actuators_msg->normalized(vehicle_params_.aileron_left.channel)); + delta_aileron_right_ = NormalizedInputToAngle(vehicle_params_.aileron_right, + actuators_msg->normalized(vehicle_params_.aileron_right.channel)); + delta_elevator_ = NormalizedInputToAngle(vehicle_params_.elevator, + actuators_msg->normalized(vehicle_params_.elevator.channel)); + delta_flap_ = NormalizedInputToAngle(vehicle_params_.flap, + actuators_msg->normalized(vehicle_params_.flap.channel)); + delta_rudder_ = NormalizedInputToAngle(vehicle_params_.rudder, + actuators_msg->normalized(vehicle_params_.rudder.channel)); + + throttle_ = actuators_msg->normalized(vehicle_params_.throttle_channel); +} + +void GazeboFwDynamicsPlugin::RollPitchYawrateThrustCallback( + GzRollPitchYawrateThrustMsgPtr& roll_pitch_yawrate_thrust_msg) { + if (kPrintOnMsgCallback) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + delta_aileron_left_ = NormalizedInputToAngle(vehicle_params_.aileron_left, + roll_pitch_yawrate_thrust_msg->roll()); + delta_aileron_right_ = -NormalizedInputToAngle(vehicle_params_.aileron_right, + roll_pitch_yawrate_thrust_msg->roll()); + delta_elevator_ = NormalizedInputToAngle(vehicle_params_.elevator, + roll_pitch_yawrate_thrust_msg->pitch()); + delta_rudder_ = NormalizedInputToAngle(vehicle_params_.rudder, + roll_pitch_yawrate_thrust_msg->yaw_rate()); + + throttle_ = roll_pitch_yawrate_thrust_msg->thrust().x(); +} + +void GazeboFwDynamicsPlugin::WindSpeedCallback( + GzWindSpeedMsgPtr& wind_speed_msg) { + if (kPrintOnMsgCallback) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + W_wind_speed_W_B_.X() = wind_speed_msg->velocity().x(); + W_wind_speed_W_B_.Y() = wind_speed_msg->velocity().y(); + W_wind_speed_W_B_.Z() = wind_speed_msg->velocity().z(); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboFwDynamicsPlugin); + +} // namespace gazebo diff --git a/rotors_gazebo_plugins/src/gazebo_gps_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_gps_plugin.cpp new file mode 100644 index 0000000..9cc3500 --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_gps_plugin.cpp @@ -0,0 +1,245 @@ +/* + * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// MODULE +#include "rotors_gazebo_plugins/gazebo_gps_plugin.h" + +// 3RD PARTY +#include "mav_msgs/default_topics.h" + +// USER +#include "ConnectGazeboToRosTopic.pb.h" + +namespace gazebo { + +GazeboGpsPlugin::GazeboGpsPlugin() + : SensorPlugin(), + // node_handle_(0), + random_generator_(random_device_()), + pubs_and_subs_created_(false) {} + +GazeboGpsPlugin::~GazeboGpsPlugin() { +} + +void GazeboGpsPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + +// Store the pointer to the parent sensor. + parent_sensor_ = std::dynamic_pointer_cast(_sensor); + world_ = physics::get_world(parent_sensor_->WorldName()); + + //==============================================// + //========== READ IN PARAMS FROM SDF ===========// + //==============================================// + + std::string link_name; + + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzerr << "[gazebo_gps_plugin] Please specify a robotNamespace.\n"; + + node_handle_ = gazebo::transport::NodePtr(new transport::Node()); + + // Initialise with default namespace (typically /gazebo/default/) + node_handle_->Init(); + + if (_sdf->HasElement("linkName")) + link_name = _sdf->GetElement("linkName")->Get(); + else + gzerr << "[gazebo_gps_plugin] Please specify a linkName.\n"; + + std::string frame_id = link_name; + + // Get the pointer to the link that holds the sensor. + link_ = + boost::dynamic_pointer_cast(world_->EntityByName(link_name)); + if (link_ == NULL) + gzerr << "[gazebo_gps_plugin] Couldn't find specified link \"" << link_name + << "\"\n"; + + // Retrieve the rest of the SDF parameters. + double hor_pos_std_dev; + double ver_pos_std_dev; + double hor_vel_std_dev; + double ver_vel_std_dev; + + getSdfParam(_sdf, "gpsTopic", gps_topic_, + mav_msgs::default_topics::GPS); + + getSdfParam(_sdf, "groundSpeedTopic", ground_speed_topic_, + mav_msgs::default_topics::GROUND_SPEED); + + getSdfParam(_sdf, "horPosStdDev", hor_pos_std_dev, + kDefaultHorPosStdDev); + getSdfParam(_sdf, "verPosStdDev", ver_pos_std_dev, + kDefaultVerPosStdDev); + getSdfParam(_sdf, "horVelStdDev", hor_vel_std_dev, + kDefaultHorVelStdDev); + getSdfParam(_sdf, "verVelStdDev", ver_vel_std_dev, + kDefaultVerVelStdDev); + + // Connect to the sensor update event. + this->updateConnection_ = this->parent_sensor_->ConnectUpdated( + boost::bind(&GazeboGpsPlugin::OnUpdate, this)); + + // Make sure the parent sensor is active. + parent_sensor_->SetActive(true); + + // Initialize the normal distributions for ground speed. + ground_speed_n_[0] = NormalDistribution(0, hor_vel_std_dev); + ground_speed_n_[1] = NormalDistribution(0, hor_vel_std_dev); + ground_speed_n_[2] = NormalDistribution(0, ver_vel_std_dev); + + // ============================================ // + // ======= POPULATE STATIC PARTS OF MSGS ====== // + // ============================================ // + + // Fill the static parts of the GPS message. + gz_gps_message_.mutable_header()->set_frame_id(frame_id); + gz_gps_message_.set_service(gz_sensor_msgs::NavSatFix::SERVICE_GPS); + gz_gps_message_.set_status(gz_sensor_msgs::NavSatFix::STATUS_FIX); + gz_gps_message_.set_position_covariance_type( + gz_sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN); + + for (int i = 0; i < 9; i++) { + switch (i) { + case 0: + gz_gps_message_.add_position_covariance(hor_pos_std_dev * + hor_pos_std_dev); + break; + case 1: + case 2: + case 3: + gz_gps_message_.add_position_covariance(0); + break; + case 4: + gz_gps_message_.add_position_covariance(hor_pos_std_dev * + hor_pos_std_dev); + break; + case 5: + case 6: + case 7: + gz_gps_message_.add_position_covariance(0); + break; + case 8: + gz_gps_message_.add_position_covariance(ver_pos_std_dev * + ver_pos_std_dev); + break; + } + } + + // Fill the static parts of the ground speed message. + gz_ground_speed_message_.mutable_header()->set_frame_id(frame_id); + gz_ground_speed_message_.mutable_twist()->mutable_angular()->set_x(0.0); + gz_ground_speed_message_.mutable_twist()->mutable_angular()->set_y(0.0); + gz_ground_speed_message_.mutable_twist()->mutable_angular()->set_z(0.0); +} + +void GazeboGpsPlugin::OnUpdate() { + if (kPrintOnUpdates) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!pubs_and_subs_created_) { + CreatePubsAndSubs(); + pubs_and_subs_created_ = true; + } + + // Get the time of the last measurement. + common::Time current_time; + + // Get the linear velocity in the world frame. + ignition::math::Vector3d W_ground_speed_W_L = link_->WorldLinearVel(); + + // Apply noise to ground speed. + W_ground_speed_W_L += ignition::math::Vector3d (ground_speed_n_[0](random_generator_), + ground_speed_n_[1](random_generator_), + ground_speed_n_[2](random_generator_)); + + // Fill the GPS message. + current_time = parent_sensor_->LastMeasurementTime(); + + gz_gps_message_.set_latitude(parent_sensor_->Latitude().Degree()); + gz_gps_message_.set_longitude(parent_sensor_->Longitude().Degree()); + gz_gps_message_.set_altitude(parent_sensor_->Altitude()); + + gz_gps_message_.mutable_header()->mutable_stamp()->set_sec(current_time.sec); + gz_gps_message_.mutable_header()->mutable_stamp()->set_nsec( + current_time.nsec); + + // Fill the ground speed message. + gz_ground_speed_message_.mutable_twist()->mutable_linear()->set_x( + W_ground_speed_W_L.X()); + gz_ground_speed_message_.mutable_twist()->mutable_linear()->set_y( + W_ground_speed_W_L.Y()); + gz_ground_speed_message_.mutable_twist()->mutable_linear()->set_z( + W_ground_speed_W_L.Z()); + gz_ground_speed_message_.mutable_header()->mutable_stamp()->set_sec( + current_time.sec); + gz_ground_speed_message_.mutable_header()->mutable_stamp()->set_nsec( + current_time.nsec); + + // Publish the GPS message. + gz_gps_pub_->Publish(gz_gps_message_); + + // Publish the ground speed message. + gz_ground_speed_pub_->Publish(gz_ground_speed_message_); +} + +void GazeboGpsPlugin::CreatePubsAndSubs() { + // Create temporary "ConnectGazeboToRosTopic" publisher and message + gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub = + node_handle_->Advertise( + "~/" + kConnectGazeboToRosSubtopic, 1); + + gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg; + + // ============================================ // + // =========== NAV SAT FIX MSG SETUP ========== // + // ============================================ // + gz_gps_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + gps_topic_, 1); + + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + gps_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + gps_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::NAV_SAT_FIX); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); + + // ============================================ // + // == GROUND SPEED (TWIST STAMPED) MSG SETUP == // + // ============================================ // + gz_ground_speed_pub_ = + node_handle_->Advertise( + "~/" + namespace_ + "/" + ground_speed_topic_, 1); + + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + ground_speed_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + + ground_speed_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::TWIST_STAMPED); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg); +} + +GZ_REGISTER_SENSOR_PLUGIN(GazeboGpsPlugin); +} diff --git a/rotors_gazebo_plugins/src/gazebo_imu_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_imu_plugin.cpp new file mode 100644 index 0000000..7b642fd --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_imu_plugin.cpp @@ -0,0 +1,366 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// MODULE HEADER +#include "rotors_gazebo_plugins/gazebo_imu_plugin.h" + +// SYSTEM LIBS +#include +#include +#include +#include +#include + +// 3RD PARTY +#include "mav_msgs/default_topics.h" + +// USER HEADERS +#include "ConnectGazeboToRosTopic.pb.h" + +namespace gazebo { + +GazeboImuPlugin::GazeboImuPlugin() + : ModelPlugin(), + node_handle_(0), + velocity_prev_W_(0, 0, 0), + pubs_and_subs_created_(false) {} + +GazeboImuPlugin::~GazeboImuPlugin() { +} + +void GazeboImuPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + gzdbg << "_model = " << _model->GetName() << std::endl; + + // Store the pointer to the model + model_ = _model; + world_ = model_->GetWorld(); + + // default params + namespace_.clear(); + + //==============================================// + //========== READ IN PARAMS FROM SDF ===========// + //==============================================// + + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzerr << "[gazebo_imu_plugin] Please specify a robotNamespace.\n"; + + // Get node handle + node_handle_ = transport::NodePtr(new transport::Node()); + + // Initialise with default namespace (typically /gazebo/default/) + node_handle_->Init(); + + if (_sdf->HasElement("linkName")) + link_name_ = _sdf->GetElement("linkName")->Get(); + else + gzerr << "[gazebo_imu_plugin] Please specify a linkName.\n"; + // Get the pointer to the link + link_ = model_->GetLink(link_name_); + if (link_ == NULL) + gzthrow("[gazebo_imu_plugin] Couldn't find specified link \"" << link_name_ + << "\"."); + + frame_id_ = link_name_; + + getSdfParam(_sdf, "imuTopic", imu_topic_, + mav_msgs::default_topics::IMU); + getSdfParam(_sdf, "gyroscopeNoiseDensity", + imu_parameters_.gyroscope_noise_density, + imu_parameters_.gyroscope_noise_density); + getSdfParam(_sdf, "gyroscopeBiasRandomWalk", + imu_parameters_.gyroscope_random_walk, + imu_parameters_.gyroscope_random_walk); + getSdfParam(_sdf, "gyroscopeBiasCorrelationTime", + imu_parameters_.gyroscope_bias_correlation_time, + imu_parameters_.gyroscope_bias_correlation_time); + assert(imu_parameters_.gyroscope_bias_correlation_time > 0.0); + getSdfParam(_sdf, "gyroscopeTurnOnBiasSigma", + imu_parameters_.gyroscope_turn_on_bias_sigma, + imu_parameters_.gyroscope_turn_on_bias_sigma); + getSdfParam(_sdf, "accelerometerNoiseDensity", + imu_parameters_.accelerometer_noise_density, + imu_parameters_.accelerometer_noise_density); + getSdfParam(_sdf, "accelerometerRandomWalk", + imu_parameters_.accelerometer_random_walk, + imu_parameters_.accelerometer_random_walk); + getSdfParam(_sdf, "accelerometerBiasCorrelationTime", + imu_parameters_.accelerometer_bias_correlation_time, + imu_parameters_.accelerometer_bias_correlation_time); + assert(imu_parameters_.accelerometer_bias_correlation_time > 0.0); + getSdfParam(_sdf, "accelerometerTurnOnBiasSigma", + imu_parameters_.accelerometer_turn_on_bias_sigma, + imu_parameters_.accelerometer_turn_on_bias_sigma); + + last_time_ = world_->SimTime(); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->updateConnection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboImuPlugin::OnUpdate, this, _1)); + + //==============================================// + //====== POPULATE STATIC PARTS OF IMU MSG ======// + //==============================================// + + // imu_message_.header.frame_id = frame_id_; + imu_message_.mutable_header()->set_frame_id(frame_id_); + + // We assume uncorrelated noise on the 3 channels -> only set diagonal + // elements. Only the broadband noise component is considered, specified as a + // continuous-time density (two-sided spectrum); not the true covariance of + // the measurements. + + for (int i = 0; i < 9; i++) { + switch (i) { + case 0: + imu_message_.add_angular_velocity_covariance( + imu_parameters_.gyroscope_noise_density * + imu_parameters_.gyroscope_noise_density); + + imu_message_.add_orientation_covariance(-1.0); + + imu_message_.add_linear_acceleration_covariance( + imu_parameters_.accelerometer_noise_density * + imu_parameters_.accelerometer_noise_density); + break; + case 1: + case 2: + case 3: + imu_message_.add_angular_velocity_covariance(0.0); + + imu_message_.add_orientation_covariance(-1.0); + + imu_message_.add_linear_acceleration_covariance(0.0); + break; + case 4: + imu_message_.add_angular_velocity_covariance( + imu_parameters_.gyroscope_noise_density * + imu_parameters_.gyroscope_noise_density); + + imu_message_.add_orientation_covariance(-1.0); + + imu_message_.add_linear_acceleration_covariance( + imu_parameters_.accelerometer_noise_density * + imu_parameters_.accelerometer_noise_density); + break; + case 5: + case 6: + case 7: + imu_message_.add_angular_velocity_covariance(0.0); + + imu_message_.add_orientation_covariance(-1.0); + + imu_message_.add_linear_acceleration_covariance(0.0); + break; + case 8: + imu_message_.add_angular_velocity_covariance( + imu_parameters_.gyroscope_noise_density * + imu_parameters_.gyroscope_noise_density); + + imu_message_.add_orientation_covariance(-1.0); + + imu_message_.add_linear_acceleration_covariance( + imu_parameters_.accelerometer_noise_density * + imu_parameters_.accelerometer_noise_density); + break; + } + } + + gravity_W_ = world_->Gravity(); + imu_parameters_.gravity_magnitude = gravity_W_.Length(); + + standard_normal_distribution_ = std::normal_distribution(0.0, 1.0); + + double sigma_bon_g = imu_parameters_.gyroscope_turn_on_bias_sigma; + double sigma_bon_a = imu_parameters_.accelerometer_turn_on_bias_sigma; + for (int i = 0; i < 3; ++i) { + gyroscope_turn_on_bias_[i] = + sigma_bon_g * standard_normal_distribution_(random_generator_); + accelerometer_turn_on_bias_[i] = + sigma_bon_a * standard_normal_distribution_(random_generator_); + } + + // TODO(nikolicj) incorporate steady-state covariance of bias process + gyroscope_bias_.setZero(); + accelerometer_bias_.setZero(); +} + +void GazeboImuPlugin::AddNoise(Eigen::Vector3d* linear_acceleration, + Eigen::Vector3d* angular_velocity, + const double dt) { + GZ_ASSERT(linear_acceleration != nullptr, "Linear acceleration was null."); + GZ_ASSERT(angular_velocity != nullptr, "Angular velocity was null."); + GZ_ASSERT(dt > 0.0, "Change in time must be greater than 0."); + + // Gyrosocpe + double tau_g = imu_parameters_.gyroscope_bias_correlation_time; + // Discrete-time standard deviation equivalent to an "integrating" sampler + // with integration time dt. + double sigma_g_d = 1 / sqrt(dt) * imu_parameters_.gyroscope_noise_density; + double sigma_b_g = imu_parameters_.gyroscope_random_walk; + // Compute exact covariance of the process after dt [Maybeck 4-114]. + double sigma_b_g_d = sqrt(-sigma_b_g * sigma_b_g * tau_g / 2.0 * + (exp(-2.0 * dt / tau_g) - 1.0)); + // Compute state-transition. + double phi_g_d = exp(-1.0 / tau_g * dt); + // Simulate gyroscope noise processes and add them to the true angular rate. + for (int i = 0; i < 3; ++i) { + gyroscope_bias_[i] = + phi_g_d * gyroscope_bias_[i] + + sigma_b_g_d * standard_normal_distribution_(random_generator_); + (*angular_velocity)[i] = + (*angular_velocity)[i] + gyroscope_bias_[i] + + sigma_g_d * standard_normal_distribution_(random_generator_) + + gyroscope_turn_on_bias_[i]; + } + + // Accelerometer + double tau_a = imu_parameters_.accelerometer_bias_correlation_time; + // Discrete-time standard deviation equivalent to an "integrating" sampler + // with integration time dt. + double sigma_a_d = 1 / sqrt(dt) * imu_parameters_.accelerometer_noise_density; + double sigma_b_a = imu_parameters_.accelerometer_random_walk; + // Compute exact covariance of the process after dt [Maybeck 4-114]. + double sigma_b_a_d = sqrt(-sigma_b_a * sigma_b_a * tau_a / 2.0 * + (exp(-2.0 * dt / tau_a) - 1.0)); + // Compute state-transition. + double phi_a_d = exp(-1.0 / tau_a * dt); + // Simulate accelerometer noise processes and add them to the true linear + // acceleration. + for (int i = 0; i < 3; ++i) { + accelerometer_bias_[i] = + phi_a_d * accelerometer_bias_[i] + + sigma_b_a_d * standard_normal_distribution_(random_generator_); + (*linear_acceleration)[i] = + (*linear_acceleration)[i] + accelerometer_bias_[i] + + sigma_a_d * standard_normal_distribution_(random_generator_) + + accelerometer_turn_on_bias_[i]; + } +} + +void GazeboImuPlugin::OnUpdate(const common::UpdateInfo& _info) { + if (kPrintOnUpdates) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!pubs_and_subs_created_) { + CreatePubsAndSubs(); + pubs_and_subs_created_ = true; + } + + common::Time current_time = world_->SimTime(); + double dt = (current_time - last_time_).Double(); + last_time_ = current_time; + double t = current_time.Double(); + + ignition::math::Pose3d T_W_I = link_->WorldPose(); // TODO(burrimi): Check tf. + ignition::math::Quaterniond C_W_I = T_W_I.Rot(); + + ignition::math::Vector3d acceleration_I = + link_->RelativeLinearAccel() - C_W_I.RotateVectorReverse(gravity_W_); + + ignition::math::Vector3d angular_vel_I = link_->RelativeAngularVel(); + + Eigen::Vector3d linear_acceleration_I(acceleration_I.X(), acceleration_I.Y(), + acceleration_I.Z()); + Eigen::Vector3d angular_velocity_I(angular_vel_I.X(), angular_vel_I.Y(), + angular_vel_I.Z()); + + AddNoise(&linear_acceleration_I, &angular_velocity_I, dt); + + // Fill IMU message. + // imu_message_.header.stamp.sec = current_time.sec; + imu_message_.mutable_header()->mutable_stamp()->set_sec(current_time.sec); + + // imu_message_.header.stamp.nsec = current_time.nsec; + imu_message_.mutable_header()->mutable_stamp()->set_nsec(current_time.nsec); + + /// \todo(burrimi): Add orientation estimator. + // NOTE: rotors_simulator used to set the orientation to "0", since it is + // not raw IMU data but rather a calculation (and could confuse users). + // However, the orientation is now set as it is used by PX4. + /*gazebo::msgs::Quaternion* orientation = new gazebo::msgs::Quaternion(); + orientation->set_x(0); + orientation->set_y(0); + orientation->set_z(0); + orientation->set_w(1); + imu_message_.set_allocated_orientation(orientation);*/ + + /// \todo(burrimi): add noise. + gazebo::msgs::Quaternion* orientation = new gazebo::msgs::Quaternion(); + orientation->set_w(C_W_I.W()); + orientation->set_x(C_W_I.X()); + orientation->set_y(C_W_I.Y()); + orientation->set_z(C_W_I.Z()); + imu_message_.set_allocated_orientation(orientation); + + gazebo::msgs::Vector3d* linear_acceleration = new gazebo::msgs::Vector3d(); + linear_acceleration->set_x(linear_acceleration_I[0]); + linear_acceleration->set_y(linear_acceleration_I[1]); + linear_acceleration->set_z(linear_acceleration_I[2]); + imu_message_.set_allocated_linear_acceleration(linear_acceleration); + + gazebo::msgs::Vector3d* angular_velocity = new gazebo::msgs::Vector3d(); + angular_velocity->set_x(angular_velocity_I[0]); + angular_velocity->set_y(angular_velocity_I[1]); + angular_velocity->set_z(angular_velocity_I[2]); + imu_message_.set_allocated_angular_velocity(angular_velocity); + + // Publish the IMU message + imu_pub_->Publish(imu_message_); + + // std::cout << "Published IMU message.\n"; +} + +void GazeboImuPlugin::CreatePubsAndSubs() { + // Create temporary "ConnectGazeboToRosTopic" publisher and message + gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub = + node_handle_->Advertise( + "~/" + kConnectGazeboToRosSubtopic, 1); + + // ============================================ // + // =============== IMU MSG SETUP ============== // + // ============================================ // + + imu_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + imu_topic_, 1); + + gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg; + // connect_gazebo_to_ros_topic_msg.set_gazebo_namespace(namespace_); + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + imu_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + imu_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::IMU); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboImuPlugin); + +} // namespace gazebo diff --git a/rotors_gazebo_plugins/src/gazebo_magnetometer_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_magnetometer_plugin.cpp new file mode 100644 index 0000000..c6dc815 --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_magnetometer_plugin.cpp @@ -0,0 +1,205 @@ +/* + * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// MODULE HEADER INCLUDE +#include "rotors_gazebo_plugins/gazebo_magnetometer_plugin.h" + +#include // This comes from the mav_comm repo + +#include "ConnectGazeboToRosTopic.pb.h" + +namespace gazebo { + +GazeboMagnetometerPlugin::GazeboMagnetometerPlugin() + : ModelPlugin(), + random_generator_(random_device_()), + pubs_and_subs_created_(false) { + // Nothing +} + +GazeboMagnetometerPlugin::~GazeboMagnetometerPlugin() { +} + +void GazeboMagnetometerPlugin::Load(physics::ModelPtr _model, + sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + // Store the pointer to the model and the world + model_ = _model; + world_ = model_->GetWorld(); + + // Use the robot namespace to create the node handle + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzerr << "[gazebo_magnetometer_plugin] Please specify a robotNamespace.\n"; + + node_handle_ = gazebo::transport::NodePtr(new transport::Node()); + + // Initialise with default namespace (typically /gazebo/default/) + node_handle_->Init(); + + // Use the link name as the frame id + std::string link_name; + if (_sdf->HasElement("linkName")) + link_name = _sdf->GetElement("linkName")->Get(); + else + gzerr << "[gazebo_magnetometer_plugin] Please specify a linkName.\n"; + // Get the pointer to the link + link_ = model_->GetLink(link_name); + if (link_ == NULL) + gzthrow("[gazebo_magnetometer_plugin] Couldn't find specified link \"" + << link_name << "\"."); + + frame_id_ = link_name; + + double ref_mag_north; + double ref_mag_east; + double ref_mag_down; + SdfVector3 noise_normal; + SdfVector3 noise_uniform_initial_bias; + const SdfVector3 zeros3(0.0, 0.0, 0.0); + + // Retrieve the rest of the SDF parameters + getSdfParam(_sdf, "magnetometerTopic", magnetometer_topic_, + mav_msgs::default_topics::MAGNETIC_FIELD); + + getSdfParam(_sdf, "refMagNorth", ref_mag_north, kDefaultRefMagNorth); + getSdfParam(_sdf, "refMagEast", ref_mag_east, kDefaultRefMagEast); + getSdfParam(_sdf, "refMagDown", ref_mag_down, kDefaultRefMagDown); + getSdfParam(_sdf, "noiseNormal", noise_normal, zeros3); + getSdfParam(_sdf, "noiseUniformInitialBias", + noise_uniform_initial_bias, zeros3); + + // Listen to the update event. This event is broadcast every simulation + // iteration. + this->updateConnection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboMagnetometerPlugin::OnUpdate, this, _1)); + + // Create the normal noise distributions + noise_n_[0] = NormalDistribution(0, noise_normal.X()); + noise_n_[1] = NormalDistribution(0, noise_normal.Y()); + noise_n_[2] = NormalDistribution(0, noise_normal.Z()); + + // Create the uniform noise distribution for initial bias + UniformDistribution initial_bias[3]; + initial_bias[0] = UniformDistribution(-noise_uniform_initial_bias.X(), + noise_uniform_initial_bias.X()); + initial_bias[1] = UniformDistribution(-noise_uniform_initial_bias.Y(), + noise_uniform_initial_bias.Y()); + initial_bias[2] = UniformDistribution(-noise_uniform_initial_bias.Z(), + noise_uniform_initial_bias.Z()); + + // Initialize the reference magnetic field vector in world frame, taking into + // account the initial bias + mag_W_ = ignition::math::Vector3d (ref_mag_north + initial_bias[0](random_generator_), + ref_mag_east + initial_bias[1](random_generator_), + ref_mag_down + initial_bias[2](random_generator_)); + + // Fill the static parts of the magnetometer message. + mag_message_.mutable_header()->set_frame_id(frame_id_); + + for (int i = 0; i < 9; i++) { + switch (i) { + case 0: + mag_message_.add_magnetic_field_covariance(noise_normal.X() * + noise_normal.X()); + break; + case 1: + case 2: + case 3: + mag_message_.add_magnetic_field_covariance(0); + break; + case 4: + mag_message_.add_magnetic_field_covariance(noise_normal.Y() * + noise_normal.Y()); + break; + case 5: + case 6: + case 7: + mag_message_.add_magnetic_field_covariance(0); + break; + case 8: + mag_message_.add_magnetic_field_covariance(noise_normal.Z() * + noise_normal.Z()); + break; + } + } +} + +void GazeboMagnetometerPlugin::OnUpdate(const common::UpdateInfo& _info) { + if (kPrintOnUpdates) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!pubs_and_subs_created_) { + CreatePubsAndSubs(); + pubs_and_subs_created_ = true; + } + + // Get the current pose and time from Gazebo + ignition::math::Pose3d T_W_B = link_->WorldPose(); + common::Time current_time = world_->SimTime(); + + // Calculate the magnetic field noise. + ignition::math::Vector3d mag_noise(noise_n_[0](random_generator_), + noise_n_[1](random_generator_), + noise_n_[2](random_generator_)); + + // Rotate the earth magnetic field into the inertial frame + ignition::math::Vector3d field_B = T_W_B.Rot().RotateVectorReverse(mag_W_ + mag_noise); + + // Fill the magnetic field message + mag_message_.mutable_header()->mutable_stamp()->set_sec(current_time.sec); + mag_message_.mutable_header()->mutable_stamp()->set_nsec(current_time.nsec); + mag_message_.mutable_magnetic_field()->set_x(field_B.X()); + mag_message_.mutable_magnetic_field()->set_y(field_B.Y()); + mag_message_.mutable_magnetic_field()->set_z(field_B.Z()); + + // Publish the message + magnetometer_pub_->Publish(mag_message_); +} + +void GazeboMagnetometerPlugin::CreatePubsAndSubs() { + // Create temporary "ConnectGazeboToRosTopic" publisher and message + gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub = + node_handle_->Advertise( + "~/" + kConnectGazeboToRosSubtopic, 1); + + // ============================================ // + // ========= MAGNETIC FIELD MSG SETUP ========= // + // ============================================ // + + magnetometer_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + magnetometer_topic_, 1); + + gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg; + // connect_gazebo_to_ros_topic_msg.set_gazebo_namespace(namespace_); + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + magnetometer_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + + magnetometer_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::MAGNETIC_FIELD); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboMagnetometerPlugin); + +} // namespace gazebo diff --git a/rotors_gazebo_plugins/src/gazebo_mavlink_interface.cpp b/rotors_gazebo_plugins/src/gazebo_mavlink_interface.cpp new file mode 100644 index 0000000..3990e59 --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_mavlink_interface.cpp @@ -0,0 +1,943 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2015-2018 PX4 Pro Development Team + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +namespace gazebo { + + // Set global reference point + // Zurich Irchel Park: 47.397742, 8.545594, 488m + // Seattle downtown (15 deg declination): 47.592182, -122.316031, 86m + // Moscow downtown: 55.753395, 37.625427, 155m + + // Zurich Irchel Park + static const double kLatZurich_rad = 47.397742 * M_PI / 180; // rad + static const double kLonZurich_rad = 8.545594 * M_PI / 180; // rad + static const double kAltZurich_m = 488.0; // meters + static const float kEarthRadius_m = 6353000; // m + +GZ_REGISTER_MODEL_PLUGIN(GazeboMavlinkInterface); + +GazeboMavlinkInterface::~GazeboMavlinkInterface() { + updateConnection_->~Connection(); +} + +void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { + // Store the pointer to the model. + model_ = _model; + + world_ = model_->GetWorld(); + + const char *env_alt = std::getenv("PX4_HOME_ALT"); + if (env_alt) { + gzmsg << "Home altitude is set to " << env_alt << ".\n"; + alt_home = std::stod(env_alt); + } + + namespace_.clear(); + if (_sdf->HasElement("robotNamespace")) { + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + } else { + gzerr << "[gazebo_mavlink_interface] Please specify a robotNamespace.\n"; + } + + if (_sdf->HasElement("protocol_version")) { + protocol_version_ = _sdf->GetElement("protocol_version")->Get(); + } + + node_handle_ = transport::NodePtr(new transport::Node()); + node_handle_->Init(); + + getSdfParam(_sdf, "motorSpeedCommandPubTopic", motor_velocity_reference_pub_topic_, + motor_velocity_reference_pub_topic_); + gzdbg << "motorSpeedCommandPubTopic = \"" << motor_velocity_reference_pub_topic_ << "\"." << std::endl; + getSdfParam(_sdf, "imuSubTopic", imu_sub_topic_, imu_sub_topic_); + getSdfParam(_sdf, "lidarSubTopic", lidar_sub_topic_, lidar_sub_topic_); + getSdfParam(_sdf, "opticalFlowSubTopic", + opticalFlow_sub_topic_, opticalFlow_sub_topic_); + + // set input_reference_ from inputs.control + input_reference_.resize(n_out_max); + joints_.resize(n_out_max); + pids_.resize(n_out_max); + for (int i = 0; i < n_out_max; ++i) + { + pids_[i].Init(0, 0, 0, 0, 0, 0, 0); + input_reference_[i] = 0; + } + + if (_sdf->HasElement("control_channels")) { + sdf::ElementPtr control_channels = _sdf->GetElement("control_channels"); + sdf::ElementPtr channel = control_channels->GetElement("channel"); + while (channel) + { + if (channel->HasElement("input_index")) + { + int index = channel->Get("input_index"); + if (index < n_out_max) + { + input_offset_[index] = channel->Get("input_offset"); + input_scaling_[index] = channel->Get("input_scaling"); + zero_position_disarmed_[index] = channel->Get("zero_position_disarmed"); + zero_position_armed_[index] = channel->Get("zero_position_armed"); + if (channel->HasElement("joint_control_type")) + { + joint_control_type_[index] = channel->Get("joint_control_type"); + } + else + { + gzwarn << "joint_control_type[" << index << "] not specified, using velocity.\n"; + joint_control_type_[index] = "velocity"; + } + + // start gz transport node handle + if (joint_control_type_[index] == "position_gztopic") + { + // setup publisher handle to topic + if (channel->HasElement("gztopic")) + gztopic_[index] = "~/" + model_->GetName() + channel->Get("gztopic"); + else + gztopic_[index] = "control_position_gztopic_" + std::to_string(index); + #if GAZEBO_MAJOR_VERSION >= 7 && GAZEBO_MINOR_VERSION >= 4 + /// only gazebo 7.4 and above support Any + joint_control_pub_[index] = node_handle_->Advertise( + gztopic_[index]); + #else + joint_control_pub_[index] = node_handle_->Advertise( + gztopic_[index]); + #endif + } + + if (channel->HasElement("joint_name")) + { + std::string joint_name = channel->Get("joint_name"); + joints_[index] = model_->GetJoint(joint_name); + if (joints_[index] == nullptr) + { + gzwarn << "joint [" << joint_name << "] not found for channel[" + << index << "] no joint control for this channel.\n"; + } + else + { + gzdbg << "joint [" << joint_name << "] found for channel[" + << index << "] joint control active for this channel.\n"; + } + } + else + { + gzdbg << " not found for channel[" << index + << "] no joint control will be performed for this channel.\n"; + } + + // setup joint control pid to control joint + if (channel->HasElement("joint_control_pid")) + { + sdf::ElementPtr pid = channel->GetElement("joint_control_pid"); + double p = 0; + if (pid->HasElement("p")) + p = pid->Get("p"); + double i = 0; + if (pid->HasElement("i")) + i = pid->Get("i"); + double d = 0; + if (pid->HasElement("d")) + d = pid->Get("d"); + double iMax = 0; + if (pid->HasElement("iMax")) + iMax = pid->Get("iMax"); + double iMin = 0; + if (pid->HasElement("iMin")) + iMin = pid->Get("iMin"); + double cmdMax = 0; + if (pid->HasElement("cmdMax")) + cmdMax = pid->Get("cmdMax"); + double cmdMin = 0; + if (pid->HasElement("cmdMin")) + cmdMin = pid->Get("cmdMin"); + pids_[index].Init(p, i, d, iMax, iMin, cmdMax, cmdMin); + } + } + else + { + gzerr << "input_index[" << index << "] out of range, not parsing.\n"; + } + } + else + { + gzerr << "no input_index, not parsing.\n"; + break; + } + channel = channel->GetNextElement("channel"); + } + } + + // Listen to the update event. This event is broadcast every + // simulation iteration. + updateConnection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboMavlinkInterface::OnUpdate, this, _1)); + + // Subscriber to IMU sensor_msgs::Imu Message and SITL message + imu_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + imu_sub_topic_, &GazeboMavlinkInterface::ImuCallback, this); + lidar_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + lidar_sub_topic_, &GazeboMavlinkInterface::LidarCallback, this); + opticalFlow_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + opticalFlow_sub_topic_, &GazeboMavlinkInterface::OpticalFlowCallback, this); + + // Publish gazebo's motor_speed message + motor_velocity_reference_pub_ = node_handle_->Advertise("~/" + model_->GetName() + motor_velocity_reference_pub_topic_, 1); + + _rotor_count = 5; +#if GAZEBO_MAJOR_VERSION >= 9 + last_time_ = world_->SimTime(); + last_imu_time_ = world_->SimTime(); + gravity_W_ = world_->Gravity(); +#else + last_time_ = world_->GetSimTime(); + last_imu_time_ = world_->GetSimTime(); + gravity_W_ = ignitionFromGazeboMath(world_->GetPhysicsEngine()->GetGravity()); +#endif + + if (_sdf->HasElement("imu_rate")) { + imu_update_interval_ = 1 / _sdf->GetElement("imu_rate")->Get(); + } + + // Magnetic field data for Zurich from WMM2015 (10^5xnanoTesla (N, E D) n-frame ) + // mag_n_ = {0.21523, 0.00771, -0.42741}; + // We set the world Y component to zero because we apply + // the declination based on the global position, + // and so we need to start without any offsets. + // The real value for Zurich would be 0.00771 + // frame d is the magnetic north frame + mag_d_.X() = 0.21523; + mag_d_.Y() = 0; + mag_d_.Z() = -0.42741; + + if(_sdf->HasElement("hil_state_level")) + { + hil_mode_ = _sdf->GetElement("hil_mode")->Get(); + } + + if(_sdf->HasElement("hil_state_level")) + { + hil_state_level_ = _sdf->GetElement("hil_state_level")->Get(); + } + + // Get serial params + if(_sdf->HasElement("serialEnabled")) + { + serial_enabled_ = _sdf->GetElement("serialEnabled")->Get(); + } + + if(serial_enabled_) { + // Set up serial interface + if(_sdf->HasElement("serialDevice")) + { + device_ = _sdf->GetElement("serialDevice")->Get(); + } + + if (_sdf->HasElement("baudRate")) { + baudrate_ = _sdf->GetElement("baudRate")->Get(); + } + io_service.post(std::bind(&GazeboMavlinkInterface::do_read, this)); + + // run io_service for async io + io_thread = std::thread([this] () { + io_service.run(); + }); + open(); + } + + //Create socket + // udp socket data + mavlink_addr_ = htonl(INADDR_ANY); + if (_sdf->HasElement("mavlink_addr")) { + std::string mavlink_addr = _sdf->GetElement("mavlink_addr")->Get(); + if (mavlink_addr != "INADDR_ANY") { + mavlink_addr_ = inet_addr(mavlink_addr.c_str()); + if (mavlink_addr_ == INADDR_NONE) { + fprintf(stderr, "invalid mavlink_addr \"%s\"\n", mavlink_addr.c_str()); + return; + } + } + } + if (_sdf->HasElement("mavlink_udp_port")) { + mavlink_udp_port_ = _sdf->GetElement("mavlink_udp_port")->Get(); + } +#if GAZEBO_MAJOR_VERSION >= 9 + auto worldName = world_->Name(); +#else + auto worldName = world_->GetName(); +#endif + model_param(worldName, model_->GetName(), "mavlink_udp_port", mavlink_udp_port_); + + qgc_addr_ = htonl(INADDR_ANY); + if (_sdf->HasElement("qgc_addr")) { + std::string qgc_addr = _sdf->GetElement("qgc_addr")->Get(); + if (qgc_addr != "INADDR_ANY") { + qgc_addr_ = inet_addr(qgc_addr.c_str()); + if (qgc_addr_ == INADDR_NONE) { + fprintf(stderr, "invalid qgc_addr \"%s\"\n", qgc_addr.c_str()); + return; + } + } + } + if (_sdf->HasElement("qgc_udp_port")) { + qgc_udp_port_ = _sdf->GetElement("qgc_udp_port")->Get(); + } + + // try to setup udp socket for communcation + if ((_fd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) { + printf("create socket failed\n"); + return; + } + + memset((char *)&_myaddr, 0, sizeof(_myaddr)); + _myaddr.sin_family = AF_INET; + _srcaddr.sin_family = AF_INET; + + if (serial_enabled_) { + // gcs link + _myaddr.sin_addr.s_addr = mavlink_addr_; + _myaddr.sin_port = htons(mavlink_udp_port_); + _srcaddr.sin_addr.s_addr = qgc_addr_; + _srcaddr.sin_port = htons(qgc_udp_port_); + } + + else { + _myaddr.sin_addr.s_addr = htonl(INADDR_ANY); + // Let the OS pick the port + _myaddr.sin_port = htons(0); + _srcaddr.sin_addr.s_addr = mavlink_addr_; + _srcaddr.sin_port = htons(mavlink_udp_port_); + } + + _addrlen = sizeof(_srcaddr); + + if (bind(_fd, (struct sockaddr *)&_myaddr, sizeof(_myaddr)) < 0) { + printf("bind failed\n"); + return; + } + + fds[0].fd = _fd; + fds[0].events = POLLIN; + + mavlink_status_t* chan_state = mavlink_get_channel_status(MAVLINK_COMM_0); + + // set the Mavlink protocol version to use on the link + if (protocol_version_ == 2.0) { + chan_state->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); + gzmsg << "Using MAVLink protocol v2.0\n"; + } + else if (protocol_version_ == 1.0) { + chan_state->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + gzmsg << "Using MAVLink protocol v1.0\n"; + } + else { + gzerr << "Unkown protocol version! Using v" << protocol_version_ << "by default \n"; + } +} + +// This gets called by the world update start event. +void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) { +#if GAZEBO_MAJOR_VERSION >= 9 + common::Time current_time = world_->SimTime(); +#else + common::Time current_time = world_->GetSimTime(); +#endif + double dt = (current_time - last_time_).Double(); + + pollForMAVLinkMessages(dt, 1000); + + handle_control(dt); + + if (received_first_referenc_) { + gz_mav_msgs::CommandMotorSpeed turning_velocities_msg; + + for (int i = 0; i < input_reference_.size(); i++) { + if (last_actuator_time_ == 0 || (current_time - last_actuator_time_).Double() > 0.2) { + turning_velocities_msg.add_motor_speed(0); + } else { + turning_velocities_msg.add_motor_speed(input_reference_[i]); + } + } + // TODO Add timestamp and Header + // turning_velocities_msg->header.stamp.sec = current_time.sec; + // turning_velocities_msg->header.stamp.nsec = current_time.nsec; + + motor_velocity_reference_pub_->Publish(turning_velocities_msg); + } + + last_time_ = current_time; +} + +void GazeboMavlinkInterface::send_mavlink_message(const mavlink_message_t *message, const int destination_port) +{ + + if(serial_enabled_ && destination_port == 0) { + assert(message != nullptr); + if (!is_open()) { + gzerr << "Serial port closed! \n"; + return; + } + + { + lock_guard lock(mutex); + + if (tx_q.size() >= MAX_TXQ_SIZE) { +// gzwarn << "TX queue overflow. \n"; + } + tx_q.emplace_back(message); + } + io_service.post(std::bind(&GazeboMavlinkInterface::do_write, this, true)); + } + + else { + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + int packetlen = mavlink_msg_to_send_buffer(buffer, message); + + struct sockaddr_in dest_addr; + memcpy(&dest_addr, &_srcaddr, sizeof(_srcaddr)); + + if (destination_port != 0) { + dest_addr.sin_port = htons(destination_port); + } + + ssize_t len = sendto(_fd, buffer, packetlen, 0, (struct sockaddr *)&_srcaddr, sizeof(_srcaddr)); + + if (len <= 0) { + printf("Failed sending mavlink message\n"); + } + } + +} + +void GazeboMavlinkInterface::ImuCallback(ImuPtr& imu_message) { +#if GAZEBO_MAJOR_VERSION >= 9 + common::Time current_time = world_->SimTime(); +#else + common::Time current_time = world_->GetSimTime(); +#endif + double dt = (current_time - last_imu_time_).Double(); + + ignition::math::Quaterniond q_br(0, 1, 0, 0); + ignition::math::Quaterniond q_ng(0, 0.70711, 0.70711, 0); + + ignition::math::Quaterniond q_gr = ignition::math::Quaterniond( + imu_message->orientation().w(), + imu_message->orientation().x(), + imu_message->orientation().y(), + imu_message->orientation().z()); + + ignition::math::Quaterniond q_gb = q_gr*q_br.Inverse(); + ignition::math::Quaterniond q_nb = q_ng*q_gb; + +#if GAZEBO_MAJOR_VERSION >= 9 + ignition::math::Vector3d pos_g = model_->WorldPose().Pos(); +#else + ignition::math::Vector3d pos_g = ignitionFromGazeboMath(model_->GetWorldPose().pos); +#endif + ignition::math::Vector3d pos_n = q_ng.RotateVector(pos_g); + + float declination = get_mag_declination(lat_rad_, lon_rad_); + + ignition::math::Quaterniond q_dn(0.0, 0.0, declination); + ignition::math::Vector3d mag_n = q_dn.RotateVector(mag_d_); + +#if GAZEBO_MAJOR_VERSION >= 9 + ignition::math::Vector3d vel_b = q_br.RotateVector(model_->RelativeLinearVel()); + ignition::math::Vector3d vel_n = q_ng.RotateVector(model_->WorldLinearVel()); + ignition::math::Vector3d omega_nb_b = q_br.RotateVector(model_->RelativeAngularVel()); +#else + ignition::math::Vector3d vel_b = q_br.RotateVector(ignitionFromGazeboMath(model_->GetRelativeLinearVel())); + ignition::math::Vector3d vel_n = q_ng.RotateVector(ignitionFromGazeboMath(model_->GetWorldLinearVel())); + ignition::math::Vector3d omega_nb_b = q_br.RotateVector(ignitionFromGazeboMath(model_->GetRelativeAngularVel())); +#endif + + ignition::math::Vector3d mag_noise_b( + 0.01 * randn_(rand_), + 0.01 * randn_(rand_), + 0.01 * randn_(rand_)); + + ignition::math::Vector3d accel_b = q_br.RotateVector(ignition::math::Vector3d( + imu_message->linear_acceleration().x(), + imu_message->linear_acceleration().y(), + imu_message->linear_acceleration().z())); + ignition::math::Vector3d gyro_b = q_br.RotateVector(ignition::math::Vector3d( + imu_message->angular_velocity().x(), + imu_message->angular_velocity().y(), + imu_message->angular_velocity().z())); + ignition::math::Vector3d mag_b = q_nb.RotateVectorReverse(mag_n) + mag_noise_b; + + if (imu_update_interval_!=0 && dt >= imu_update_interval_) + { + mavlink_hil_sensor_t sensor_msg; +#if GAZEBO_MAJOR_VERSION >= 9 + sensor_msg.time_usec = world_->SimTime().Double() * 1e6; +#else + sensor_msg.time_usec = world_->GetSimTime().Double() * 1e6; +#endif + sensor_msg.xacc = accel_b.X(); + sensor_msg.yacc = accel_b.Y(); + sensor_msg.zacc = accel_b.Z(); + sensor_msg.xgyro = gyro_b.X(); + sensor_msg.ygyro = gyro_b.Y(); + sensor_msg.zgyro = gyro_b.Z(); + sensor_msg.xmag = mag_b.X(); + sensor_msg.ymag = mag_b.Y(); + sensor_msg.zmag = mag_b.Z(); + + // calculate abs_pressure using an ISA model for the tropsphere (valid up to 11km above MSL) + const float lapse_rate = 0.0065f; // reduction in temperature with altitude (Kelvin/m) + const float temperature_msl = 288.0f; // temperature at MSL (Kelvin) + float alt_msl = (float)alt_home - pos_n.Z(); + float temperature_local = temperature_msl - lapse_rate * alt_msl; + float pressure_ratio = powf((temperature_msl/temperature_local) , 5.256f); + const float pressure_msl = 101325.0f; // pressure at MSL + sensor_msg.abs_pressure = pressure_msl / pressure_ratio; + + // generate Gaussian noise sequence using polar form of Box-Muller transformation + // http://www.design.caltech.edu/erik/Misc/Gaussian.html + double x1, x2, w, y1, y2; + do { + x1 = 2.0 * (rand() * (1.0 / (double)RAND_MAX)) - 1.0; + x2 = 2.0 * (rand() * (1.0 / (double)RAND_MAX)) - 1.0; + w = x1 * x1 + x2 * x2; + } while ( w >= 1.0 ); + w = sqrt( (-2.0 * log( w ) ) / w ); + y1 = x1 * w; + y2 = x2 * w; + + // Apply 1 Pa RMS noise + float abs_pressure_noise = 1.0f * (float)w; + sensor_msg.abs_pressure += abs_pressure_noise; + + // convert to hPa + sensor_msg.abs_pressure *= 0.01f; + + // calculate density using an ISA model for the tropsphere (valid up to 11km above MSL) + const float density_ratio = powf((temperature_msl/temperature_local) , 4.256f); + float rho = 1.225f / density_ratio; + + // calculate pressure altitude including effect of pressure noise + sensor_msg.pressure_alt = alt_msl - abs_pressure_noise / (gravity_W_.Length() * rho); + + // calculate differential pressure in hPa + sensor_msg.diff_pressure = 0.005f*rho*vel_b.X()*vel_b.X(); + + // calculate temperature in Celsius + sensor_msg.temperature = temperature_local - 273.0f; + + sensor_msg.fields_updated = 4095; + + //accumulate gyro measurements that are needed for the optical flow message + static uint32_t last_dt_us = sensor_msg.time_usec; + uint32_t dt_us = sensor_msg.time_usec - last_dt_us; + if (dt_us > 1000) { + optflow_gyro += gyro_b * (dt_us / 1000000.0f); + last_dt_us = sensor_msg.time_usec; + } + + mavlink_message_t msg; + mavlink_msg_hil_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &sensor_msg); + if (hil_mode_) { + if (!hil_state_level_){ + send_mavlink_message(&msg); + } + } + + else { + send_mavlink_message(&msg); + } + last_imu_time_ = current_time; + } + + // ground truth +#if GAZEBO_MAJOR_VERSION >= 9 + ignition::math::Vector3d accel_true_b = q_br.RotateVector(model_->RelativeLinearAccel()); +#else + ignition::math::Vector3d accel_true_b = q_br.RotateVector(ignitionFromGazeboMath(model_->GetRelativeLinearAccel())); +#endif + + // send ground truth + + mavlink_hil_state_quaternion_t hil_state_quat; +#if GAZEBO_MAJOR_VERSION >= 9 + hil_state_quat.time_usec = world_->SimTime().Double() * 1e6; +#else + hil_state_quat.time_usec = world_->GetSimTime().Double() * 1e6; +#endif + hil_state_quat.attitude_quaternion[0] = q_nb.W(); + hil_state_quat.attitude_quaternion[1] = q_nb.X(); + hil_state_quat.attitude_quaternion[2] = q_nb.Y(); + hil_state_quat.attitude_quaternion[3] = q_nb.Z(); + + hil_state_quat.rollspeed = omega_nb_b.X(); + hil_state_quat.pitchspeed = omega_nb_b.Y(); + hil_state_quat.yawspeed = omega_nb_b.Z(); + + hil_state_quat.lat = lat_rad_ * 180 / M_PI * 1e7; + hil_state_quat.lon = lon_rad_ * 180 / M_PI * 1e7; + hil_state_quat.alt = (-pos_n.Z() + kAltZurich_m) * 1000; + + hil_state_quat.vx = vel_n.X() * 100; + hil_state_quat.vy = vel_n.Y() * 100; + hil_state_quat.vz = vel_n.Z() * 100; + + // assumed indicated airspeed due to flow aligned with pitot (body x) + hil_state_quat.ind_airspeed = vel_b.X(); + +#if GAZEBO_MAJOR_VERSION >= 9 + hil_state_quat.true_airspeed = model_->WorldLinearVel().Length() * 100; //no wind simulated +#else + hil_state_quat.true_airspeed = model_->GetWorldLinearVel().GetLength() * 100; //no wind simulated +#endif + + hil_state_quat.xacc = accel_true_b.X() * 1000; + hil_state_quat.yacc = accel_true_b.Y() * 1000; + hil_state_quat.zacc = accel_true_b.Z() * 1000; + + mavlink_message_t msg; + mavlink_msg_hil_state_quaternion_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &hil_state_quat); + if (hil_mode_) { + if (hil_state_level_){ + send_mavlink_message(&msg); + } + } + + else { + send_mavlink_message(&msg); + } +} + +void GazeboMavlinkInterface::LidarCallback(LidarPtr& lidar_message) { + mavlink_distance_sensor_t sensor_msg; + sensor_msg.time_boot_ms = lidar_message->time_msec(); + sensor_msg.min_distance = lidar_message->min_distance() * 100.0; + sensor_msg.max_distance = lidar_message->max_distance() * 100.0; + sensor_msg.current_distance = lidar_message->current_distance() * 100.0; + sensor_msg.type = 0; + sensor_msg.id = 0; + sensor_msg.orientation = 25;//downward facing + sensor_msg.covariance = 0; + + //distance needed for optical flow message + optflow_distance = lidar_message->current_distance(); //[m] + + mavlink_message_t msg; + mavlink_msg_distance_sensor_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &sensor_msg); + send_mavlink_message(&msg); +} + +void GazeboMavlinkInterface::OpticalFlowCallback(OpticalFlowPtr& opticalFlow_message) { + mavlink_hil_optical_flow_t sensor_msg; +#if GAZEBO_MAJOR_VERSION >= 9 + sensor_msg.time_usec = world_->SimTime().Double() * 1e6; +#else + sensor_msg.time_usec = world_->GetSimTime().Double() * 1e6; +#endif + sensor_msg.sensor_id = opticalFlow_message->sensor_id(); + sensor_msg.integration_time_us = opticalFlow_message->integration_time_us(); + sensor_msg.integrated_x = opticalFlow_message->integrated_x(); + sensor_msg.integrated_y = opticalFlow_message->integrated_y(); + sensor_msg.integrated_xgyro = opticalFlow_message->quality() ? -optflow_gyro.Y() : 0.0f;//xy switched + sensor_msg.integrated_ygyro = opticalFlow_message->quality() ? optflow_gyro.X() : 0.0f; //xy switched + sensor_msg.integrated_zgyro = opticalFlow_message->quality() ? -optflow_gyro.Z() : 0.0f;//change direction + sensor_msg.temperature = opticalFlow_message->temperature(); + sensor_msg.quality = opticalFlow_message->quality(); + sensor_msg.time_delta_distance_us = opticalFlow_message->time_delta_distance_us(); + sensor_msg.distance = optflow_distance; + + //reset gyro integral + optflow_gyro.Set(); + + mavlink_message_t msg; + mavlink_msg_hil_optical_flow_encode_chan(1, 200, MAVLINK_COMM_0, &msg, &sensor_msg); + send_mavlink_message(&msg); +} + +void GazeboMavlinkInterface::pollForMAVLinkMessages(double _dt, uint32_t _timeoutMs) +{ + // convert timeout in ms to timeval + struct timeval tv; + tv.tv_sec = _timeoutMs / 1000; + tv.tv_usec = (_timeoutMs % 1000) * 1000UL; + + // poll + ::poll(&fds[0], (sizeof(fds[0]) / sizeof(fds[0])), 0); + + if (fds[0].revents & POLLIN) { + int len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen); + if (len > 0) { + mavlink_message_t msg; + mavlink_status_t status; + for (unsigned i = 0; i < len; ++i) + { + if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status)) + { + if (serial_enabled_) { + // forward message from qgc to serial + send_mavlink_message(&msg); + } + // have a message, handle it + handle_message(&msg); + } + } + } + } +} + +void GazeboMavlinkInterface::handle_message(mavlink_message_t *msg) +{ + switch (msg->msgid) { + case MAVLINK_MSG_ID_HIL_ACTUATOR_CONTROLS: + mavlink_hil_actuator_controls_t controls; + mavlink_msg_hil_actuator_controls_decode(msg, &controls); + bool armed = false; + + if ((controls.mode & MAV_MODE_FLAG_SAFETY_ARMED) > 0) { + armed = true; + } + +#if GAZEBO_MAJOR_VERSION >= 9 + last_actuator_time_ = world_->SimTime(); +#else + last_actuator_time_ = world_->GetSimTime(); +#endif + + for (unsigned i = 0; i < n_out_max; i++) { + input_index_[i] = i; + } + + // Set rotor speeds and controller targets for flagged messages. + if (controls.flags == kMotorSpeedFlag) { + input_reference_.resize(n_out_max); + for (unsigned i = 0; i < n_motors; ++i) { + if (armed) { + input_reference_[i] = + (controls.controls[input_index_[i]] + input_offset_[i]) * + input_scaling_[i] + + zero_position_armed_[i]; + } else { + input_reference_[i] = zero_position_disarmed_[i]; + } + } + received_first_referenc_ = true; + } + else if (controls.flags == kServoPositionFlag) { + for (unsigned i = n_motors; i < (n_motors + n_servos); ++i) { + if (armed) { + input_reference_[i] = + (controls.controls[input_index_[i - n_motors]] + input_offset_[i]) * + input_scaling_[i] + + zero_position_armed_[i]; + } else { + input_reference_[i] = zero_position_disarmed_[i]; + } + } + } + // Set rotor speeds, controller targets for unflagged messages. + else { + input_reference_.resize(n_out_max); + for (unsigned i = 0; i < n_out_max; ++i) { + if (armed) { + input_reference_[i] = + (controls.controls[input_index_[i]] + input_offset_[i]) * + input_scaling_[i] + + zero_position_armed_[i]; + } else { + input_reference_[i] = zero_position_disarmed_[i]; + } + } + received_first_referenc_ = true; + } + break; + } +} + +void GazeboMavlinkInterface::handle_control(double _dt) +{ + // set joint positions + for (int i = 0; i < input_reference_.size(); i++) { + if (joints_[i]) { + double target = input_reference_[i]; + if (joint_control_type_[i] == "velocity") + { + double current = joints_[i]->GetVelocity(0); + double err = current - target; + double force = pids_[i].Update(err, _dt); + joints_[i]->SetForce(0, force); + } + else if (joint_control_type_[i] == "position") + { + +#if GAZEBO_MAJOR_VERSION >= 9 + double current = joints_[i]->Position(0); +#else + double current = joints_[i]->GetAngle(0).Radian(); +#endif + + double err = current - target; + double force = pids_[i].Update(err, _dt); + joints_[i]->SetForce(0, force); + } + else if (joint_control_type_[i] == "position_gztopic") + { + #if GAZEBO_MAJOR_VERSION >= 7 && GAZEBO_MINOR_VERSION >= 4 + /// only gazebo 7.4 and above support Any + gazebo::msgs::Any m; + m.set_type(gazebo::msgs::Any_ValueType_DOUBLE); + m.set_double_value(target); + #else + std::stringstream ss; + gazebo::msgs::GzString m; + ss << target; + m.set_data(ss.str()); + #endif + joint_control_pub_[i]->Publish(m); + } + else if (joint_control_type_[i] == "position_kinematic") + { + /// really not ideal if your drone is moving at all, + /// mixing kinematic updates with dynamics calculation is + /// non-physical. + #if GAZEBO_MAJOR_VERSION >= 6 + joints_[i]->SetPosition(0, input_reference_[i]); + #else + joints_[i]->SetAngle(0, input_reference_[i]); + #endif + } + else + { + gzerr << "joint_control_type[" << joint_control_type_[i] << "] undefined.\n"; + } + } + } +} + +void GazeboMavlinkInterface::open() { + try{ + serial_dev.open(device_); + serial_dev.set_option(boost::asio::serial_port_base::baud_rate(baudrate_)); + serial_dev.set_option(boost::asio::serial_port_base::character_size(8)); + serial_dev.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); + serial_dev.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); + serial_dev.set_option(boost::asio::serial_port_base::flow_control(boost::asio::serial_port_base::flow_control::none)); + gzdbg << "Opened serial device " << device_ << "\n"; + } + catch (boost::system::system_error &err) { + gzerr <<"Error opening serial device: " << err.what() << "\n"; + } +} + +void GazeboMavlinkInterface::close() +{ + lock_guard lock(mutex); + if (!is_open()) + return; + + io_service.stop(); + serial_dev.close(); + + if (io_thread.joinable()) + io_thread.join(); +} + +void GazeboMavlinkInterface::do_read(void) +{ + serial_dev.async_read_some(boost::asio::buffer(rx_buf), boost::bind( + &GazeboMavlinkInterface::parse_buffer, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred + ) + ); +} + +// Based on MAVConnInterface::parse_buffer in MAVROS +void GazeboMavlinkInterface::parse_buffer(const boost::system::error_code& err, std::size_t bytes_t){ + mavlink_status_t status; + mavlink_message_t message; + uint8_t *buf = this->rx_buf.data(); + + assert(rx_buf.size() >= bytes_t); + + for(; bytes_t > 0; bytes_t--) + { + auto c = *buf++; + + auto msg_received = static_cast(mavlink_frame_char_buffer(&m_buffer, &m_status, c, &message, &status)); + if (msg_received == Framing::bad_crc || msg_received == Framing::bad_signature) { + _mav_parse_error(&m_status); + m_status.msg_received = MAVLINK_FRAMING_INCOMPLETE; + m_status.parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) { + m_status.parse_state = MAVLINK_PARSE_STATE_GOT_STX; + m_buffer.len = 0; + mavlink_start_checksum(&m_buffer); + } + } + + if(msg_received != Framing::incomplete){ + // send to gcs + send_mavlink_message(&message, qgc_udp_port_); + handle_message(&message); + } + } + do_read(); +} + +void GazeboMavlinkInterface::do_write(bool check_tx_state){ + if (check_tx_state && tx_in_progress) + return; + + lock_guard lock(mutex); + if (tx_q.empty()) + return; + + tx_in_progress = true; + auto &buf_ref = tx_q.front(); + + serial_dev.async_write_some( + boost::asio::buffer(buf_ref.dpos(), buf_ref.nbytes()), [this, &buf_ref] (boost::system::error_code error, size_t bytes_transferred) + { + assert(bytes_transferred <= buf_ref.len); + if(error) { + gzerr << "Serial error: " << error.message() << "\n"; + return; + } + + lock_guard lock(mutex); + + if (tx_q.empty()) { + tx_in_progress = false; + return; + } + + buf_ref.pos += bytes_transferred; + if (buf_ref.nbytes() == 0) { + tx_q.pop_front(); + } + + if (!tx_q.empty()) { + do_write(false); + } + else { + tx_in_progress = false; + } + }); +} + +} diff --git a/rotors_gazebo_plugins/src/gazebo_motor_model.cpp b/rotors_gazebo_plugins/src/gazebo_motor_model.cpp new file mode 100644 index 0000000..bae8d46 --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_motor_model.cpp @@ -0,0 +1,456 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.M + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "rotors_gazebo_plugins/gazebo_motor_model.h" + +#include "ConnectGazeboToRosTopic.pb.h" +#include "ConnectRosToGazeboTopic.pb.h" + +namespace gazebo { + +GazeboMotorModel::~GazeboMotorModel() { +} + +void GazeboMotorModel::InitializeParams() {} + +void GazeboMotorModel::Publish() { + if (publish_speed_) { + turning_velocity_msg_.set_data(joint_->GetVelocity(0)); + motor_velocity_pub_->Publish(turning_velocity_msg_); + } + if (publish_position_) { + position_msg_.set_data(joint_->Position(0)); + motor_position_pub_->Publish(position_msg_); + } + if (publish_force_) { + force_msg_.set_data(joint_->GetForce(0)); + motor_force_pub_->Publish(force_msg_); + } +} + +void GazeboMotorModel::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + model_ = _model; + + namespace_.clear(); + + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzerr << "[gazebo_motor_model] Please specify a robotNamespace.\n"; + + node_handle_ = gazebo::transport::NodePtr(new transport::Node()); + + // Initialise with default namespace (typically /gazebo/default/) + node_handle_->Init(); + + if (_sdf->HasElement("jointName")) + joint_name_ = _sdf->GetElement("jointName")->Get(); + else + gzerr << "[gazebo_motor_model] Please specify a jointName, where the rotor " + "is attached.\n"; + + // Get the pointer to the joint. + joint_ = model_->GetJoint(joint_name_); + if (joint_ == NULL) + gzthrow( + "[gazebo_motor_model] Couldn't find specified joint \"" << joint_name_ + << "\"."); + + if (_sdf->HasElement("linkName")) + link_name_ = _sdf->GetElement("linkName")->Get(); + else + gzerr << "[gazebo_motor_model] Please specify a linkName of the rotor.\n"; + link_ = model_->GetLink(link_name_); + if (link_ == NULL) + gzthrow( + "[gazebo_motor_model] Couldn't find specified link \"" << link_name_ + << "\"."); + + if (_sdf->HasElement("motorNumber")) + motor_number_ = _sdf->GetElement("motorNumber")->Get(); + else + gzerr << "[gazebo_motor_model] Please specify a motorNumber.\n"; + + if (_sdf->HasElement("turningDirection")) { + std::string turning_direction = + _sdf->GetElement("turningDirection")->Get(); + if (turning_direction == "cw") + turning_direction_ = turning_direction::CW; + else if (turning_direction == "ccw") + turning_direction_ = turning_direction::CCW; + else + gzerr << "[gazebo_motor_model] Please only use 'cw' or 'ccw' as " + "turningDirection.\n"; + } else + gzerr << "[gazebo_motor_model] Please specify a turning direction ('cw' or " + "'ccw').\n"; + + if (_sdf->HasElement("motorType")) { + std::string motor_type = _sdf->GetElement("motorType")->Get(); + if (motor_type == "velocity") + motor_type_ = MotorType::kVelocity; + else if (motor_type == "position") + motor_type_ = MotorType::kPosition; + else if (motor_type == "force") { + motor_type_ = MotorType::kForce; + } else + gzerr << "[gazebo_motor_model] Please only use 'velocity', 'position' or " + "'force' as motorType.\n"; + } else { + gzwarn << "[gazebo_motor_model] motorType not specified, using velocity.\n"; + motor_type_ = MotorType::kVelocity; + } + + // Set up joint control PID to control joint. + if (motor_type_ == MotorType::kPosition) { + if (_sdf->HasElement("joint_control_pid")) { + sdf::ElementPtr pid = _sdf->GetElement("joint_control_pid"); + double p = 0; + if (pid->HasElement("p")) + p = pid->Get("p"); + double i = 0; + if (pid->HasElement("i")) + i = pid->Get("i"); + double d = 0; + if (pid->HasElement("d")) + d = pid->Get("d"); + double iMax = 0; + if (pid->HasElement("iMax")) + iMax = pid->Get("iMax"); + double iMin = 0; + if (pid->HasElement("iMin")) + iMin = pid->Get("iMin"); + double cmdMax = 0; + if (pid->HasElement("cmdMax")) + cmdMax = pid->Get("cmdMax"); + double cmdMin = 0; + if (pid->HasElement("cmdMin")) + cmdMin = pid->Get("cmdMin"); + pids_.Init(p, i, d, iMax, iMin, cmdMax, cmdMin); + } else { + pids_.Init(0, 0, 0, 0, 0, 0, 0); + gzerr << "[gazebo_motor_model] PID values not found, Setting all values " + "to zero!\n"; + } + } else { + pids_.Init(0, 0, 0, 0, 0, 0, 0); + } + + getSdfParam( + _sdf, "commandSubTopic", command_sub_topic_, command_sub_topic_); + getSdfParam( + _sdf, "windSpeedSubTopic", wind_speed_sub_topic_, wind_speed_sub_topic_); + getSdfParam( + _sdf, "motorSpeedPubTopic", motor_speed_pub_topic_, + motor_speed_pub_topic_); + + // Only publish position and force messages if a topic is specified in sdf. + if (_sdf->HasElement("motorPositionPubTopic")) { + publish_position_ = true; + motor_position_pub_topic_ = + _sdf->GetElement("motorPositionPubTopic")->Get(); + } + if (_sdf->HasElement("motorForcePubTopic")) { + publish_force_ = true; + motor_force_pub_topic_ = + _sdf->GetElement("motorForcePubTopic")->Get(); + } + + getSdfParam( + _sdf, "rotorDragCoefficient", rotor_drag_coefficient_, + rotor_drag_coefficient_); + getSdfParam( + _sdf, "rollingMomentCoefficient", rolling_moment_coefficient_, + rolling_moment_coefficient_); + getSdfParam( + _sdf, "maxRotVelocity", max_rot_velocity_, max_rot_velocity_); + getSdfParam(_sdf, "motorConstant", motor_constant_, motor_constant_); + getSdfParam( + _sdf, "momentConstant", moment_constant_, moment_constant_); + + getSdfParam( + _sdf, "timeConstantUp", time_constant_up_, time_constant_up_); + getSdfParam( + _sdf, "timeConstantDown", time_constant_down_, time_constant_down_); + getSdfParam( + _sdf, "rotorVelocitySlowdownSim", rotor_velocity_slowdown_sim_, 10); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + updateConnection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboMotorModel::OnUpdate, this, _1)); + + // Create the first order filter. + rotor_velocity_filter_.reset( + new FirstOrderFilter( + time_constant_up_, time_constant_down_, ref_motor_input_)); +} + +// This gets called by the world update start event. +void GazeboMotorModel::OnUpdate(const common::UpdateInfo& _info) { + if (kPrintOnUpdates) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!pubs_and_subs_created_) { + CreatePubsAndSubs(); + pubs_and_subs_created_ = true; + } + + sampling_time_ = _info.simTime.Double() - prev_sim_time_; + prev_sim_time_ = _info.simTime.Double(); + UpdateForcesAndMoments(); + Publish(); +} + +void GazeboMotorModel::CreatePubsAndSubs() { + gzdbg << __PRETTY_FUNCTION__ << " called." << std::endl; + + // Create temporary "ConnectGazeboToRosTopic" publisher and message + gazebo::transport::PublisherPtr gz_connect_gazebo_to_ros_topic_pub = + node_handle_->Advertise( + "~/" + kConnectGazeboToRosSubtopic, 1); + gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg; + + // Create temporary "ConnectRosToGazeboTopic" publisher and message + gazebo::transport::PublisherPtr gz_connect_ros_to_gazebo_topic_pub = + node_handle_->Advertise( + "~/" + kConnectRosToGazeboSubtopic, 1); + gz_std_msgs::ConnectRosToGazeboTopic connect_ros_to_gazebo_topic_msg; + + // ============================================ // + // ACTUAL MOTOR SPEED MSG SETUP (GAZEBO->ROS) // + // ============================================ // + if (publish_speed_) { + motor_velocity_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + motor_speed_pub_topic_, 1); + + connect_gazebo_to_ros_topic_msg.set_gazebo_topic( + "~/" + namespace_ + "/" + motor_speed_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic( + namespace_ + "/" + motor_speed_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::FLOAT_32); + gz_connect_gazebo_to_ros_topic_pub->Publish( + connect_gazebo_to_ros_topic_msg, true); + } + + // =============================================== // + // ACTUAL MOTOR POSITION MSG SETUP (GAZEBO->ROS) // + // =============================================== // + + if (publish_position_) { + motor_position_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + motor_position_pub_topic_, 1); + + connect_gazebo_to_ros_topic_msg.set_gazebo_topic( + "~/" + namespace_ + "/" + motor_position_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic( + namespace_ + "/" + motor_position_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::FLOAT_32); + gz_connect_gazebo_to_ros_topic_pub->Publish( + connect_gazebo_to_ros_topic_msg, true); + } + + // ============================================ // + // ACTUAL MOTOR FORCE MSG SETUP (GAZEBO->ROS) // + // ============================================ // + + if (publish_force_) { + motor_force_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + motor_force_pub_topic_, 1); + + connect_gazebo_to_ros_topic_msg.set_gazebo_topic( + "~/" + namespace_ + "/" + motor_force_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic( + namespace_ + "/" + motor_force_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::FLOAT_32); + gz_connect_gazebo_to_ros_topic_pub->Publish( + connect_gazebo_to_ros_topic_msg, true); + } + + // ============================================ // + // = CONTROL COMMAND MSG SETUP (ROS->GAZEBO) = // + // ============================================ // + + command_sub_ = node_handle_->Subscribe( + "~/" + namespace_ + "/" + command_sub_topic_, + &GazeboMotorModel::ControlCommandCallback, this); + + connect_ros_to_gazebo_topic_msg.set_ros_topic( + namespace_ + "/" + command_sub_topic_); + connect_ros_to_gazebo_topic_msg.set_gazebo_topic( + "~/" + namespace_ + "/" + command_sub_topic_); + connect_ros_to_gazebo_topic_msg.set_msgtype( + gz_std_msgs::ConnectRosToGazeboTopic::COMMAND_MOTOR_SPEED); + gz_connect_ros_to_gazebo_topic_pub->Publish( + connect_ros_to_gazebo_topic_msg, true); + + // ============================================ // + // ==== WIND SPEED MSG SETUP (ROS->GAZEBO) ==== // + // ============================================ // + + /// TODO(gbmhunter): Do we need this? There is a separate Gazebo wind plugin. + wind_speed_sub_ = node_handle_->Subscribe( + "~/" + namespace_ + "/" + wind_speed_sub_topic_, + &GazeboMotorModel::WindSpeedCallback, this); + + connect_ros_to_gazebo_topic_msg.set_ros_topic( + namespace_ + "/" + wind_speed_sub_topic_); + connect_ros_to_gazebo_topic_msg.set_gazebo_topic( + "~/" + namespace_ + "/" + wind_speed_sub_topic_); + connect_ros_to_gazebo_topic_msg.set_msgtype( + gz_std_msgs::ConnectRosToGazeboTopic::WIND_SPEED); + gz_connect_ros_to_gazebo_topic_pub->Publish( + connect_ros_to_gazebo_topic_msg, true); +} + +void GazeboMotorModel::ControlCommandCallback( + GzCommandMotorInputMsgPtr& command_motor_input_msg) { + if (kPrintOnMsgCallback) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (motor_number_ > command_motor_input_msg->motor_speed_size() - 1) { + gzerr << "You tried to access index " << motor_number_ + << " of the MotorSpeed message array which is of size " + << command_motor_input_msg->motor_speed_size(); + } + + if (motor_type_ == MotorType::kVelocity) { + ref_motor_input_ = std::min( + static_cast( + command_motor_input_msg->motor_speed(motor_number_)), + static_cast(max_rot_velocity_)); + } else if (motor_type_ == MotorType::kPosition) { + ref_motor_input_ = command_motor_input_msg->motor_speed(motor_number_); + } else { // if (motor_type_ == MotorType::kForce) { + ref_motor_input_ = std::min( + static_cast( + command_motor_input_msg->motor_speed(motor_number_)), + static_cast(max_force_)); + } +} + +void GazeboMotorModel::WindSpeedCallback(GzWindSpeedMsgPtr& wind_speed_msg) { + if (kPrintOnMsgCallback) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + // TODO(burrimi): Transform velocity to world frame if frame_id is set to + // something else. + wind_speed_W_.X() = wind_speed_msg->velocity().x(); + wind_speed_W_.Y() = wind_speed_msg->velocity().y(); + wind_speed_W_.Z() = wind_speed_msg->velocity().z(); +} + +void GazeboMotorModel::UpdateForcesAndMoments() { + switch (motor_type_) { + case (MotorType::kPosition): { + double err = joint_->Position(0) - ref_motor_input_; + double force = pids_.Update(err, sampling_time_); + joint_->SetForce(0, force); + break; + } + case (MotorType::kForce): { + joint_->SetForce(0, ref_motor_input_); + break; + } + default: // MotorType::kVelocity + { + motor_rot_vel_ = joint_->GetVelocity(0); + if (motor_rot_vel_ / (2 * M_PI) > 1 / (2 * sampling_time_)) { + gzerr << "Aliasing on motor [" << motor_number_ + << "] might occur. Consider making smaller simulation time " + "steps or raising the rotor_velocity_slowdown_sim_ param.\n"; + } + double real_motor_velocity = + motor_rot_vel_ * rotor_velocity_slowdown_sim_; + // Get the direction of the rotor rotation. + int real_motor_velocity_sign = + (real_motor_velocity > 0) - (real_motor_velocity < 0); + // Assuming symmetric propellers (or rotors) for the thrust calculation. + double thrust = turning_direction_ * real_motor_velocity_sign * + real_motor_velocity * real_motor_velocity * + motor_constant_; + + // Apply a force to the link. + link_->AddRelativeForce(ignition::math::Vector3d (0, 0, thrust)); + + // Forces from Philppe Martin's and Erwan Salaün's + // 2010 IEEE Conference on Robotics and Automation paper + // The True Role of Accelerometer Feedback in Quadrotor Control + // - \omega * \lambda_1 * V_A^{\perp} + ignition::math::Vector3d joint_axis = joint_->GlobalAxis(0); + ignition::math::Vector3d body_velocity_W = link_->WorldLinearVel(); + ignition::math::Vector3d relative_wind_velocity_W = body_velocity_W - wind_speed_W_; + ignition::math::Vector3d body_velocity_perpendicular = + relative_wind_velocity_W - + (relative_wind_velocity_W.Dot(joint_axis) * joint_axis); + ignition::math::Vector3d air_drag = -std::abs(real_motor_velocity) * + rotor_drag_coefficient_ * + body_velocity_perpendicular; + + // Apply air_drag to link. + link_->AddForce(air_drag); + // Moments get the parent link, such that the resulting torques can be + // applied. + physics::Link_V parent_links = link_->GetParentJointsLinks(); + // The tansformation from the parent_link to the link_. + ignition::math::Pose3d pose_difference = + link_->WorldCoGPose() - parent_links.at(0)->WorldCoGPose(); + ignition::math::Vector3d drag_torque( + 0, 0, -turning_direction_ * thrust * moment_constant_); + // Transforming the drag torque into the parent frame to handle + // arbitrary rotor orientations. + ignition::math::Vector3d drag_torque_parent_frame = + pose_difference.Rot().RotateVector(drag_torque); + parent_links.at(0)->AddRelativeTorque(drag_torque_parent_frame); + + ignition::math::Vector3d rolling_moment; + // - \omega * \mu_1 * V_A^{\perp} + rolling_moment = -std::abs(real_motor_velocity) * + rolling_moment_coefficient_ * + body_velocity_perpendicular; + parent_links.at(0)->AddTorque(rolling_moment); + // Apply the filter on the motor's velocity. + double ref_motor_rot_vel; + ref_motor_rot_vel = rotor_velocity_filter_->updateFilter( + ref_motor_input_, sampling_time_); + + // Make sure max force is set, as it may be reset to 0 by a world reset any + // time. (This cannot be done during Reset() because the change will be undone + // by the Joint's reset function afterwards.) + joint_->SetVelocity( + 0, turning_direction_ * ref_motor_rot_vel / + rotor_velocity_slowdown_sim_); + } + } +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboMotorModel); +} diff --git a/rotors_gazebo_plugins/src/gazebo_multirotor_base_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_multirotor_base_plugin.cpp new file mode 100644 index 0000000..6d1c704 --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_multirotor_base_plugin.cpp @@ -0,0 +1,173 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// MODULE HEADER INCLUDE +#include "rotors_gazebo_plugins/gazebo_multirotor_base_plugin.h" + +// STANDARD LIB INCLUDES +#include + +#include "ConnectGazeboToRosTopic.pb.h" + +namespace gazebo { + +GazeboMultirotorBasePlugin::~GazeboMultirotorBasePlugin() { + +} + +void GazeboMultirotorBasePlugin::Load(physics::ModelPtr _model, + sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + model_ = _model; + world_ = model_->GetWorld(); + namespace_.clear(); + + getSdfParam(_sdf, "robotNamespace", namespace_, namespace_, + true); + getSdfParam(_sdf, "linkName", link_name_, link_name_, true); + getSdfParam(_sdf, "motorPubTopic", actuators_pub_topic_, + actuators_pub_topic_); + getSdfParam(_sdf, "rotorVelocitySlowdownSim", + rotor_velocity_slowdown_sim_, + rotor_velocity_slowdown_sim_); + + node_handle_ = gazebo::transport::NodePtr(new transport::Node()); + + // Initialise with default namespace (typically /gazebo/default/) + node_handle_->Init(); + + frame_id_ = link_name_; + + link_ = model_->GetLink(link_name_); + if (link_ == NULL) + gzthrow("[gazebo_multirotor_base_plugin] Couldn't find specified link \"" + << link_name_ << "\"."); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboMultirotorBasePlugin::OnUpdate, this, _1)); + + child_links_ = link_->GetChildJointsLinks(); + for (unsigned int i = 0; i < child_links_.size(); i++) { + std::string link_name = child_links_[i]->GetScopedName(); + + // Check if link contains rotor_ in its name. + int pos = link_name.find("rotor_"); + if (pos != link_name.npos) { + std::string motor_number_str = link_name.substr(pos + 6); + unsigned int motor_number = std::stoi(motor_number_str); + std::string joint_name = child_links_[i]->GetName() + "_joint"; + physics::JointPtr joint = this->model_->GetJoint(joint_name); + motor_joints_.insert(MotorNumberToJointPair(motor_number, joint)); + } + } +} + +// This gets called by the world update start event. +void GazeboMultirotorBasePlugin::OnUpdate(const common::UpdateInfo& _info) { + if (kPrintOnUpdates) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!pubs_and_subs_created_) { + CreatePubsAndSubs(); + pubs_and_subs_created_ = true; + } + + // Get the current simulation time. + common::Time now = world_->SimTime(); + + actuators_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec); + actuators_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec); + actuators_msg_.mutable_header()->set_frame_id(frame_id_); + + joint_state_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec); + joint_state_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec); + joint_state_msg_.mutable_header()->set_frame_id(frame_id_); + + actuators_msg_.clear_angular_velocities(); + + joint_state_msg_.clear_name(); + joint_state_msg_.clear_position(); + + MotorNumberToJointMap::iterator m; + for (m = motor_joints_.begin(); m != motor_joints_.end(); ++m) { + double motor_rot_vel = + m->second->GetVelocity(0) * rotor_velocity_slowdown_sim_; + + actuators_msg_.add_angular_velocities(motor_rot_vel); + + joint_state_msg_.add_name(m->second->GetName()); + joint_state_msg_.add_position(m->second->Position(0)); + } + + joint_state_pub_->Publish(joint_state_msg_); + motor_pub_->Publish(actuators_msg_); +} + +void GazeboMultirotorBasePlugin::CreatePubsAndSubs() { + // Create temporary "ConnectGazeboToRosTopic" publisher and message + gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub = + node_handle_->Advertise( + "~/" + kConnectGazeboToRosSubtopic, 1); + + gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg; + + // ============================================ // + // =========== ACTUATORS MSG SETUP ============ // + // ============================================ // + motor_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + actuators_pub_topic_, 10); + + // connect_gazebo_to_ros_topic_msg.set_gazebo_namespace(namespace_); + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + model_->GetName() + + "/" + actuators_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + + actuators_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::ACTUATORS); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); + + // ============================================ // + // ========== JOINT STATE MSG SETUP =========== // + // ============================================ // + joint_state_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + joint_state_pub_topic_, 1); + + // connect_gazebo_to_ros_topic_msg.set_gazebo_namespace(namespace_); + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + joint_state_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + + joint_state_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::JOINT_STATE); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboMultirotorBasePlugin); + +} // namespace gazebo diff --git a/rotors_gazebo_plugins/src/gazebo_noisydepth_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_noisydepth_plugin.cpp new file mode 100644 index 0000000..2399b6c --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_noisydepth_plugin.cpp @@ -0,0 +1,298 @@ +/* + * Copyright 2018 Michael Pantic, ASL, ETH Zurich, Switzerland + * + * Forked from Openni/Kinect Depth Plugin, retaining original copyright header: + * Copyright 2013 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + * + * Desc: GazeboNoisyDepth plugin for simulating cameras in Gazebo + * Author: Michael Pantic / John Hsu (original Depth Plugin) + * Date: 03 Dez 18 + */ +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gazebo { +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboNoisyDepth) + +GazeboNoisyDepth::GazeboNoisyDepth() { + this->depth_info_connect_count_ = 0; + this->depth_image_connect_count_ = 0; + this->last_depth_image_camera_info_update_time_ = common::Time(0); +} + +GazeboNoisyDepth::~GazeboNoisyDepth() {} + +void GazeboNoisyDepth::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) { + DepthCameraPlugin::Load(_parent, _sdf); + + // copying from DepthCameraPlugin into GazeboRosCameraUtils + this->parentSensor_ = this->parentSensor; + this->width_ = this->width; + this->height_ = this->height; + this->depth_ = this->depth; + this->format_ = this->format; + this->camera_ = this->depthCamera; + + // using a different default + if (!_sdf->HasElement("imageTopicName")) { + this->image_topic_name_ = "ir/image_raw"; + } + + if (!_sdf->HasElement("cameraInfoTopicName")) { + this->camera_info_topic_name_ = "ir/camera_info"; + } + + // depth image stuff + if (!_sdf->HasElement("depthImageTopicName")) { + this->depth_image_topic_name_ = "depth/image_raw"; + } else { + this->depth_image_topic_name_ = + _sdf->GetElement("depthImageTopicName")->Get(); + } + + if (!_sdf->HasElement("depthImageCameraInfoTopicName")) { + this->depth_image_camera_info_topic_name_ = "depth/camera_info"; + } else { + this->depth_image_camera_info_topic_name_ = + _sdf->GetElement("depthImageCameraInfoTopicName")->Get(); + } + + // noise model stuff + std::string noise_model; + if (!_sdf->HasElement("depthNoiseModelName")) { + noise_model = "Kinect"; + ROS_WARN_NAMED("NoisyDepth", + "depthNoiseModelName not defined, assuming 'Kinect'"); + } else { + noise_model = _sdf->GetElement("depthNoiseModelName")->Get(); + } + + if (boost::iequals(noise_model, "Kinect")) { + this->noise_model.reset(new KinectDepthNoiseModel()); + + /* no other properties for Kinect */ + } else if (boost::iequals(noise_model, "D435")) { + D435DepthNoiseModel *model = new D435DepthNoiseModel(); + this->noise_model.reset(model); + + if (_sdf->HasElement("horizontal_fov")) { + model->h_fov = _sdf->GetElement("horizontal_fov")->Get(); + } + + if (_sdf->HasElement("baseline")) { + model->baseline = _sdf->GetElement("baseline")->Get(); + } + + /* Load D435 specific noise params if available*/ + if (_sdf->HasElement("D435NoiseSubpixelErr")) { + model->subpixel_err = + _sdf->GetElement("D435NoiseSubpixelErr")->Get(); + } + + if (_sdf->HasElement("D435MaxStdev")) { + model->max_stdev = _sdf->GetElement("D435MaxStdev")->Get(); + } + + ROS_INFO_STREAM_NAMED( + "NoisyDepth", "D435 Depth noise configuration: " + << "\tHorizontal FoV: " << model->h_fov << std::endl + << "\tBaseline: " << model->baseline << std::endl + << "\tSubpixel Err: " << model->subpixel_err + << std::endl + << "\tNoise StDev cutoff: " << model->max_stdev); + } else { + ROS_WARN_NAMED("NoisyDepth", + "Invalid depthNoiseModelName (%s), assuming 'Kinect'", + noise_model.c_str()); + this->noise_model.reset(new KinectDepthNoiseModel()); + } + + /* Load properties that apply for all noise model */ + /* Note that these min/max values may not be the same as near/far clip range + * of the camera */ + if (_sdf->HasElement("depthNoiseMinDist")) { + this->noise_model->min_depth = + _sdf->GetElement("depthNoiseMinDist")->Get(); + } + + if (_sdf->HasElement("depthNoiseMaxDist")) { + this->noise_model->max_depth = + _sdf->GetElement("depthNoiseMaxDist")->Get(); + } + + load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboNoisyDepth::Advertise, this)); + + GazeboRosCameraUtils::Load(_parent, _sdf); +} + +void GazeboNoisyDepth::Advertise() { + ros::AdvertiseOptions depth_image_ao = + ros::AdvertiseOptions::create( + this->depth_image_topic_name_, 1, + boost::bind(&GazeboNoisyDepth::DepthImageConnect, this), + boost::bind(&GazeboNoisyDepth::DepthImageDisconnect, this), + ros::VoidPtr(), &this->camera_queue_); + + this->depth_image_pub_ = this->rosnode_->advertise(depth_image_ao); + + ros::AdvertiseOptions depth_image_camera_info_ao = + ros::AdvertiseOptions::create( + this->depth_image_camera_info_topic_name_, 1, + boost::bind(&GazeboNoisyDepth::DepthInfoConnect, this), + boost::bind(&GazeboNoisyDepth::DepthInfoDisconnect, this), + ros::VoidPtr(), &this->camera_queue_); + + this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao); +} + +void GazeboNoisyDepth::DepthImageConnect() { + ++this->depth_image_connect_count_; + this->parentSensor->SetActive(true); +} + +void GazeboNoisyDepth::DepthImageDisconnect() { + --this->depth_image_connect_count_; +} + +void GazeboNoisyDepth::DepthInfoConnect() { + ++this->depth_info_connect_count_; +} + +void GazeboNoisyDepth::DepthInfoDisconnect() { + --this->depth_info_connect_count_; +} + +void GazeboNoisyDepth::OnNewDepthFrame(const float *_image, unsigned int _width, + unsigned int _height, + unsigned int _depth, + const std::string &_format) { + if (!this->initialized_ || this->height_ <= 0 || this->width_ <= 0) return; + + this->depth_sensor_update_time_ = this->parentSensor->LastMeasurementTime(); + + // check if there are subscribers, if not disable parent, else process images.. + if (this->parentSensor->IsActive()) { + if (this->depth_image_connect_count_ <= 0 && (*this->image_connect_count_) <= 0) { + this->parentSensor->SetActive(false); + } else { + if (this->depth_image_connect_count_ > 0) this->FillDepthImage(_image); + } + } + else { + // if parent is disabled, but has subscribers, enable it. + if ((*this->image_connect_count_) > 0){ + this->parentSensor->SetActive(true); + } + } + PublishCameraInfo(); +} + +void GazeboNoisyDepth::OnNewImageFrame(const unsigned char *_image, + unsigned int _width, + unsigned int _height, + unsigned int _depth, + const std::string &_format) { + if (!this->initialized_ || this->height_ <= 0 || this->width_ <= 0) return; + + this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime(); + + // check if there are subscribers, if not disable parent, else process images.. + if (this->parentSensor->IsActive()) { + if (this->depth_image_connect_count_ <= 0 && + (*this->image_connect_count_) <= 0) { + this->parentSensor->SetActive(false); + } else { + if ((*this->image_connect_count_) > 0) this->PutCameraData(_image); + } + } else { + if ((*this->image_connect_count_) > 0) { + // if parent is disabled, but has subscribers, enable it. + this->parentSensor->SetActive(true); + } + } +} + +void GazeboNoisyDepth::FillDepthImage(const float *_src) { + this->lock_.lock(); + // copy data into image + this->depth_image_msg_.header.frame_id = this->frame_name_; + this->depth_image_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; + this->depth_image_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; + + // copy from depth to depth image message + if(FillDepthImageHelper(this->height, this->width,this->skip_, _src, &this->depth_image_msg_)){ + this->depth_image_pub_.publish(this->depth_image_msg_); + } + + this->lock_.unlock(); +} + +bool GazeboNoisyDepth::FillDepthImageHelper(const uint32_t rows_arg, + const uint32_t cols_arg, + const uint32_t step_arg, + const float *data_arg, + sensor_msgs::Image *image_msg) { + if(data_arg == nullptr){ + ROS_WARN_NAMED("NoisyDepth", "Invalid data array received - nullptr."); + return false; + } + + image_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1; + image_msg->height = rows_arg; + image_msg->width = cols_arg; + image_msg->step = sizeof(float) * cols_arg; + image_msg->data.resize(rows_arg * cols_arg * sizeof(float)); + image_msg->is_bigendian = 0; + + float *dest = (float *)(&(image_msg->data[0])); + memcpy(dest, data_arg, sizeof(float) * width * height); + + noise_model->ApplyNoise(width, height, dest); + + return true; +} + +void GazeboNoisyDepth::PublishCameraInfo() { + + // first publish parent camera info (ir camera) + GazeboRosCameraUtils::PublishCameraInfo(); + + if (this->depth_info_connect_count_ > 0) { + this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime(); +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time cur_time = this->world_->SimTime(); +#else + common::Time cur_time = this->world_->GetSimTime(); +#endif + + if (this->sensor_update_time_ - + this->last_depth_image_camera_info_update_time_ >= this->update_period_) { + this->GazeboRosCameraUtils::PublishCameraInfo(this->depth_image_camera_info_pub_); + this->last_depth_image_camera_info_update_time_ = this->sensor_update_time_; + } + } +} +} \ No newline at end of file diff --git a/rotors_gazebo_plugins/src/gazebo_octomap_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_octomap_plugin.cpp new file mode 100644 index 0000000..9d99205 --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_octomap_plugin.cpp @@ -0,0 +1,276 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "rotors_gazebo_plugins/gazebo_octomap_plugin.h" + +#include +#include +#include +#include + +namespace gazebo { + +OctomapFromGazeboWorld::~OctomapFromGazeboWorld() { + delete octomap_; + octomap_ = NULL; +} + +void OctomapFromGazeboWorld::Load(physics::WorldPtr _parent, + sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + world_ = _parent; + + std::string service_name = "world/get_octomap"; + std::string octomap_pub_topic = "world/octomap"; + getSdfParam(_sdf, "octomapPubTopic", octomap_pub_topic, + octomap_pub_topic); + getSdfParam(_sdf, "octomapServiceName", service_name, + service_name); + + gzlog << "Advertising service: " << service_name << std::endl; + srv_ = node_handle_.advertiseService( + service_name, &OctomapFromGazeboWorld::ServiceCallback, this); + octomap_publisher_ = + node_handle_.advertise(octomap_pub_topic, 1, true); +} + +bool OctomapFromGazeboWorld::ServiceCallback( + rotors_comm::Octomap::Request& req, rotors_comm::Octomap::Response& res) { + gzlog << "Creating octomap with origin at (" << req.bounding_box_origin.x + << ", " << req.bounding_box_origin.y << ", " + << req.bounding_box_origin.z << "), and bounding box lengths (" + << req.bounding_box_lengths.x << ", " << req.bounding_box_lengths.y + << ", " << req.bounding_box_lengths.z + << "), and leaf size: " << req.leaf_size << ".\n"; + CreateOctomap(req); + if (req.filename != "") { + if (octomap_) { + std::string path = req.filename; + octomap_->writeBinary(path); + gzlog << std::endl << "Octree saved as " << path << std::endl; + } else { + ROS_ERROR("The octree is NULL. Will not save that."); + } + } + common::Time now = world_->GetSimTime(); + res.map.header.frame_id = "world"; + res.map.header.stamp = ros::Time(now.sec, now.nsec); + + if (!octomap_msgs::binaryMapToMsg(*octomap_, res.map)) { + ROS_ERROR("Error serializing OctoMap"); + } + + if (req.publish_octomap) { + gzlog << "Publishing Octomap." << std::endl; + octomap_publisher_.publish(res.map); + } + + common::SphericalCoordinatesPtr sphericalCoordinates = world_->GetSphericalCoordinates(); + ignition::ignition::math::Vector3d origin_cartesian(0.0, 0.0, 0.0); + ignition::ignition::math::Vector3d origin_spherical = sphericalCoordinates-> + SphericalFromLocal(origin_cartesian); + + res.origin_latitude = origin_spherical.X(); + res.origin_longitude = origin_spherical.Y(); + res.origin_altitude = origin_spherical.Z(); + return true; +} + +void OctomapFromGazeboWorld::FloodFill( + const ignition::math::Vector3d & seed_point, const ignition::math::Vector3d & bounding_box_origin, + const ignition::math::Vector3d & bounding_box_lengths, const double leaf_size) { + octomap::OcTreeNode* seed = + octomap_->search(seed_point.x, seed_point.y, seed_point.z); + // do nothing if point occupied + if (seed != NULL && seed->getOccupancy()) return; + + std::stack to_check; + to_check.push(octoignition::math::Vector3d (seed_point.x, seed_point.y, seed_point.z)); + + while (to_check.size() > 0) { + octoignition::math::Vector3d p = to_check.top(); + + if ((p.x() > bounding_box_origin.x - bounding_box_lengths.x / 2) && + (p.x() < bounding_box_origin.x + bounding_box_lengths.x / 2) && + (p.y() > bounding_box_origin.y - bounding_box_lengths.y / 2) && + (p.y() < bounding_box_origin.y + bounding_box_lengths.y / 2) && + (p.z() > bounding_box_origin.z - bounding_box_lengths.z / 2) && + (p.z() < bounding_box_origin.z + bounding_box_lengths.z / 2) && + (!octomap_->search(p))) { + octomap_->setNodeValue(p, 0); + to_check.pop(); + to_check.push(octoignition::math::Vector3d (p.x() + leaf_size, p.y(), p.z())); + to_check.push(octoignition::math::Vector3d (p.x() - leaf_size, p.y(), p.z())); + to_check.push(octoignition::math::Vector3d (p.x(), p.y() + leaf_size, p.z())); + to_check.push(octoignition::math::Vector3d (p.x(), p.y() - leaf_size, p.z())); + to_check.push(octoignition::math::Vector3d (p.x(), p.y(), p.z() + leaf_size)); + to_check.push(octoignition::math::Vector3d (p.x(), p.y(), p.z() - leaf_size)); + + } else { + to_check.pop(); + } + } +} + +bool OctomapFromGazeboWorld::CheckIfInterest(const ignition::math::Vector3d & central_point, + gazebo::physics::RayShapePtr ray, + const double leaf_size) { + ignition::math::Vector3d start_point = central_point; + ignition::math::Vector3d end_point = central_point; + + double dist; + std::string entity_name; + + start_point.x += leaf_size / 2; + end_point.x -= leaf_size / 2; + ray->SetPoints(start_point, end_point); + ray->GetIntersection(dist, entity_name); + + if (dist <= leaf_size) return true; + + start_point = central_point; + end_point = central_point; + start_point.y += leaf_size / 2; + end_point.y -= leaf_size / 2; + ray->SetPoints(start_point, end_point); + ray->GetIntersection(dist, entity_name); + + if (dist <= leaf_size) return true; + + start_point = central_point; + end_point = central_point; + start_point.z += leaf_size / 2; + end_point.z -= leaf_size / 2; + ray->SetPoints(start_point, end_point); + ray->GetIntersection(dist, entity_name); + + if (dist <= leaf_size) return true; + + return false; +} + +void OctomapFromGazeboWorld::CreateOctomap( + const rotors_comm::Octomap::Request& msg) { + const double epsilon = 0.00001; + const int far_away = 100000; + ignition::math::Vector3d bounding_box_origin(msg.bounding_box_origin.x, + msg.bounding_box_origin.y, + msg.bounding_box_origin.z); + // epsilion prevents undefiened behaviour if a point is inserted exactly + // between two octomap cells + ignition::math::Vector3d bounding_box_lengths(msg.bounding_box_lengths.x + epsilon, + msg.bounding_box_lengths.y + epsilon, + msg.bounding_box_lengths.z + epsilon); + double leaf_size = msg.leaf_size; + octomap_ = new octomap::OcTree(leaf_size); + octomap_->clear(); + octomap_->setProbHit(0.7); + octomap_->setProbMiss(0.4); + octomap_->setClampingThresMin(0.12); + octomap_->setClampingThresMax(0.97); + octomap_->setOccupancyThres(0.7); + + gazebo::physics::PhysicsEnginePtr engine = world_->PhysicsEngine(); + engine->InitForThread(); + gazebo::physics::RayShapePtr ray = + boost::dynamic_pointer_cast( + engine->CreateShape("ray", gazebo::physics::CollisionPtr())); + + std::cout << "Rasterizing world and checking collisions" << std::endl; + + for (double x = + leaf_size / 2 + bounding_box_origin.x - bounding_box_lengths.x / 2; + x < bounding_box_origin.x + bounding_box_lengths.x / 2; x += leaf_size) { + int progress = + round(100 * (x + bounding_box_lengths.x / 2 - bounding_box_origin.x) / + bounding_box_lengths.x); + std::cout << "\rPlacing model edges into octomap... " << progress + << "% "; + + for (double y = + leaf_size / 2 + bounding_box_origin.y - bounding_box_lengths.y / 2; + y < bounding_box_origin.y + bounding_box_lengths.y / 2; + y += leaf_size) { + for (double z = leaf_size / 2 + bounding_box_origin.z - + bounding_box_lengths.z / 2; + z < bounding_box_origin.z + bounding_box_lengths.z / 2; + z += leaf_size) { + ignition::math::Vector3d point(x, y, z); + if (CheckIfInterest(point, ray, leaf_size)) { + octomap_->setNodeValue(x, y, z, 1); + } + } + } + } + octomap_->prune(); + octomap_->updateInnerOccupancy(); + + // flood fill from top and bottom + std::cout << "\rFlood filling freespace... "; + FloodFill(ignition::math::Vector3d (bounding_box_origin.x + leaf_size / 2, + bounding_box_origin.y + leaf_size / 2, + bounding_box_origin.z + bounding_box_lengths.z / 2 - + leaf_size / 2), + bounding_box_origin, bounding_box_lengths, leaf_size); + FloodFill(ignition::math::Vector3d (bounding_box_origin.x + leaf_size / 2, + bounding_box_origin.y + leaf_size / 2, + bounding_box_origin.z - bounding_box_lengths.z / 2 + + leaf_size / 2), + bounding_box_origin, bounding_box_lengths, leaf_size); + + octomap_->prune(); + octomap_->updateInnerOccupancy(); + + // set unknown to filled + for (double x = + leaf_size / 2 + bounding_box_origin.x - bounding_box_lengths.x / 2; + x < bounding_box_origin.x + bounding_box_lengths.x / 2; x += leaf_size) { + int progress = + round(100 * (x + bounding_box_lengths.x / 2 - bounding_box_origin.x) / + bounding_box_lengths.x); + std::cout << "\rFilling closed spaces... " << progress << "% "; + + for (double y = + leaf_size / 2 + bounding_box_origin.y - bounding_box_lengths.y / 2; + y < bounding_box_origin.y + bounding_box_lengths.y / 2; + y += leaf_size) { + for (double z = leaf_size / 2 + bounding_box_origin.z - + bounding_box_lengths.z / 2; + z < bounding_box_origin.z + bounding_box_lengths.z / 2; + z += leaf_size) { + octomap::OcTreeNode* seed = octomap_->search(x, y, z); + if (!seed) octomap_->setNodeValue(x, y, z, 1); + } + } + } + + octomap_->prune(); + octomap_->updateInnerOccupancy(); + + std::cout << "\rOctomap generation completed " << std::endl; +} + +// Register this plugin with the simulator +GZ_REGISTER_WORLD_PLUGIN(OctomapFromGazeboWorld) + +} // namespace gazebo diff --git a/rotors_gazebo_plugins/src/gazebo_odometry_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_odometry_plugin.cpp new file mode 100644 index 0000000..1b87bb2 --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_odometry_plugin.cpp @@ -0,0 +1,617 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// MODULE +#include "rotors_gazebo_plugins/gazebo_odometry_plugin.h" + +// SYSTEM +#include +#include + +// 3RD PARTY +#include +#include + +// USER +#include +#include "ConnectGazeboToRosTopic.pb.h" +#include "ConnectRosToGazeboTopic.pb.h" +#include "PoseStamped.pb.h" +#include "PoseWithCovarianceStamped.pb.h" +#include "TransformStamped.pb.h" +#include "TransformStampedWithFrameIds.pb.h" +#include "Vector3dStamped.pb.h" + +namespace gazebo { + +GazeboOdometryPlugin::~GazeboOdometryPlugin() { +} + +void GazeboOdometryPlugin::Load(physics::ModelPtr _model, + sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + // Store the pointer to the model + model_ = _model; + world_ = model_->GetWorld(); + + SdfVector3 noise_normal_position; + SdfVector3 noise_normal_quaternion; + SdfVector3 noise_normal_linear_velocity; + SdfVector3 noise_normal_angular_velocity; + SdfVector3 noise_uniform_position; + SdfVector3 noise_uniform_quaternion; + SdfVector3 noise_uniform_linear_velocity; + SdfVector3 noise_uniform_angular_velocity; + const SdfVector3 zeros3(0.0, 0.0, 0.0); + + odometry_queue_.clear(); + + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzerr << "[gazebo_odometry_plugin] Please specify a robotNamespace.\n"; + + node_handle_ = gazebo::transport::NodePtr(new transport::Node()); + + // Initialise with default namespace (typically /gazebo/default/) + node_handle_->Init(); + + if (_sdf->HasElement("linkName")) + link_name_ = _sdf->GetElement("linkName")->Get(); + else + gzerr << "[gazebo_odometry_plugin] Please specify a linkName.\n"; + link_ = model_->GetLink(link_name_); + if (link_ == NULL) + gzthrow("[gazebo_odometry_plugin] Couldn't find specified link \"" + << link_name_ << "\"."); + + if (_sdf->HasElement("covarianceImage")) { + std::string image_name = + _sdf->GetElement("covarianceImage")->Get(); + covariance_image_ = cv::imread(image_name, CV_LOAD_IMAGE_GRAYSCALE); + if (covariance_image_.data == NULL) + gzerr << "loading covariance image " << image_name << " failed" + << std::endl; + else + gzlog << "loading covariance image " << image_name << " successful" + << std::endl; + } + + if (_sdf->HasElement("randomEngineSeed")) { + random_generator_.seed( + _sdf->GetElement("randomEngineSeed")->Get()); + } else { + random_generator_.seed( + std::chrono::system_clock::now().time_since_epoch().count()); + } + getSdfParam(_sdf, "poseTopic", pose_pub_topic_, pose_pub_topic_); + getSdfParam(_sdf, "poseWithCovarianceTopic", + pose_with_covariance_stamped_pub_topic_, + pose_with_covariance_stamped_pub_topic_); + getSdfParam(_sdf, "positionTopic", position_stamped_pub_topic_, + position_stamped_pub_topic_); + getSdfParam(_sdf, "transformTopic", transform_stamped_pub_topic_, + transform_stamped_pub_topic_); + getSdfParam(_sdf, "odometryTopic", odometry_pub_topic_, + odometry_pub_topic_); + getSdfParam(_sdf, "parentFrameId", parent_frame_id_, + parent_frame_id_); + getSdfParam(_sdf, "childFrameId", child_frame_id_, + child_frame_id_); + getSdfParam(_sdf, "noiseNormalPosition", noise_normal_position, + zeros3); + getSdfParam(_sdf, "noiseNormalQuaternion", + noise_normal_quaternion, zeros3); + getSdfParam(_sdf, "noiseNormalLinearVelocity", + noise_normal_linear_velocity, zeros3); + getSdfParam(_sdf, "noiseNormalAngularVelocity", + noise_normal_angular_velocity, zeros3); + getSdfParam(_sdf, "noiseUniformPosition", noise_uniform_position, + zeros3); + getSdfParam(_sdf, "noiseUniformQuaternion", + noise_uniform_quaternion, zeros3); + getSdfParam(_sdf, "noiseUniformLinearVelocity", + noise_uniform_linear_velocity, zeros3); + getSdfParam(_sdf, "noiseUniformAngularVelocity", + noise_uniform_angular_velocity, zeros3); + getSdfParam(_sdf, "measurementDelay", measurement_delay_, + measurement_delay_); + getSdfParam(_sdf, "measurementDivisor", measurement_divisor_, + measurement_divisor_); + getSdfParam(_sdf, "unknownDelay", unknown_delay_, unknown_delay_); + getSdfParam(_sdf, "covarianceImageScale", covariance_image_scale_, + covariance_image_scale_); + + parent_link_ = world_->EntityByName(parent_frame_id_); + if (parent_link_ == NULL && parent_frame_id_ != kDefaultParentFrameId) { + gzthrow("[gazebo_odometry_plugin] Couldn't find specified parent link \"" + << parent_frame_id_ << "\"."); + } + + position_n_[0] = NormalDistribution(0, noise_normal_position.X()); + position_n_[1] = NormalDistribution(0, noise_normal_position.Y()); + position_n_[2] = NormalDistribution(0, noise_normal_position.Z()); + + attitude_n_[0] = NormalDistribution(0, noise_normal_quaternion.X()); + attitude_n_[1] = NormalDistribution(0, noise_normal_quaternion.Y()); + attitude_n_[2] = NormalDistribution(0, noise_normal_quaternion.Z()); + + linear_velocity_n_[0] = + NormalDistribution(0, noise_normal_linear_velocity.X()); + linear_velocity_n_[1] = + NormalDistribution(0, noise_normal_linear_velocity.Y()); + linear_velocity_n_[2] = + NormalDistribution(0, noise_normal_linear_velocity.Z()); + + angular_velocity_n_[0] = + NormalDistribution(0, noise_normal_angular_velocity.X()); + angular_velocity_n_[1] = + NormalDistribution(0, noise_normal_angular_velocity.Y()); + angular_velocity_n_[2] = + NormalDistribution(0, noise_normal_angular_velocity.Z()); + + position_u_[0] = UniformDistribution(-noise_uniform_position.X(), + noise_uniform_position.X()); + position_u_[1] = UniformDistribution(-noise_uniform_position.Y(), + noise_uniform_position.Y()); + position_u_[2] = UniformDistribution(-noise_uniform_position.Z(), + noise_uniform_position.Z()); + + attitude_u_[0] = UniformDistribution(-noise_uniform_quaternion.X(), + noise_uniform_quaternion.X()); + attitude_u_[1] = UniformDistribution(-noise_uniform_quaternion.Y(), + noise_uniform_quaternion.Y()); + attitude_u_[2] = UniformDistribution(-noise_uniform_quaternion.Z(), + noise_uniform_quaternion.Z()); + + linear_velocity_u_[0] = UniformDistribution( + -noise_uniform_linear_velocity.X(), noise_uniform_linear_velocity.X()); + linear_velocity_u_[1] = UniformDistribution( + -noise_uniform_linear_velocity.Y(), noise_uniform_linear_velocity.Y()); + linear_velocity_u_[2] = UniformDistribution( + -noise_uniform_linear_velocity.Z(), noise_uniform_linear_velocity.Z()); + + angular_velocity_u_[0] = UniformDistribution( + -noise_uniform_angular_velocity.X(), noise_uniform_angular_velocity.X()); + angular_velocity_u_[1] = UniformDistribution( + -noise_uniform_angular_velocity.Y(), noise_uniform_angular_velocity.Y()); + angular_velocity_u_[2] = UniformDistribution( + -noise_uniform_angular_velocity.Z(), noise_uniform_angular_velocity.Z()); + + // Fill in covariance. We omit uniform noise here. + Eigen::Map > pose_covariance( + pose_covariance_matrix_.data()); + Eigen::Matrix pose_covd; + + pose_covd << noise_normal_position.X() * noise_normal_position.X(), + noise_normal_position.Y() * noise_normal_position.Y(), + noise_normal_position.Z() * noise_normal_position.Z(), + noise_normal_quaternion.X() * noise_normal_quaternion.X(), + noise_normal_quaternion.Y() * noise_normal_quaternion.Y(), + noise_normal_quaternion.Z() * noise_normal_quaternion.Z(); + pose_covariance = pose_covd.asDiagonal(); + + // Fill in covariance. We omit uniform noise here. + Eigen::Map > twist_covariance( + twist_covariance_matrix_.data()); + Eigen::Matrix twist_covd; + + twist_covd << noise_normal_linear_velocity.X() * + noise_normal_linear_velocity.X(), + noise_normal_linear_velocity.Y() * noise_normal_linear_velocity.Y(), + noise_normal_linear_velocity.Z() * noise_normal_linear_velocity.Z(), + noise_normal_angular_velocity.X() * noise_normal_angular_velocity.X(), + noise_normal_angular_velocity.Y() * noise_normal_angular_velocity.Y(), + noise_normal_angular_velocity.Z() * noise_normal_angular_velocity.Z(); + twist_covariance = twist_covd.asDiagonal(); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + updateConnection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboOdometryPlugin::OnUpdate, this, _1)); +} + +// This gets called by the world update start event. +void GazeboOdometryPlugin::OnUpdate(const common::UpdateInfo& _info) { + if (kPrintOnUpdates) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!pubs_and_subs_created_) { + CreatePubsAndSubs(); + pubs_and_subs_created_ = true; + } + + // C denotes child frame, P parent frame, and W world frame. + // Further C_pose_W_P denotes pose of P wrt. W expressed in C. + ignition::math::Pose3d W_pose_W_C = link_->WorldCoGPose(); + ignition::math::Vector3d C_linear_velocity_W_C = link_->RelativeLinearVel(); + ignition::math::Vector3d C_angular_velocity_W_C = link_->RelativeAngularVel(); + + ignition::math::Vector3d gazebo_linear_velocity = C_linear_velocity_W_C; + ignition::math::Vector3d gazebo_angular_velocity = C_angular_velocity_W_C; + ignition::math::Pose3d gazebo_pose = W_pose_W_C; + + if (parent_frame_id_ != kDefaultParentFrameId) { + ignition::math::Pose3d W_pose_W_P = parent_link_->WorldPose(); + ignition::math::Vector3d P_linear_velocity_W_P = parent_link_->RelativeLinearVel(); + ignition::math::Vector3d P_angular_velocity_W_P = + parent_link_->RelativeAngularVel(); + ignition::math::Pose3d C_pose_P_C_ = W_pose_W_C - W_pose_W_P; + ignition::math::Vector3d C_linear_velocity_P_C; + // \prescript{}{C}{\dot{r}}_{PC} = -R_{CP} + // \cdot \prescript{}{P}{\omega}_{WP} \cross \prescript{}{P}{r}_{PC} + // + \prescript{}{C}{v}_{WC} + // - R_{CP} \cdot \prescript{}{P}{v}_{WP} + C_linear_velocity_P_C = + -C_pose_P_C_.Rot().Inverse() * + P_angular_velocity_W_P.Cross(C_pose_P_C_.Pos()) + + C_linear_velocity_W_C - + C_pose_P_C_.Rot().Inverse() * P_linear_velocity_W_P; + + // \prescript{}{C}{\omega}_{PC} = \prescript{}{C}{\omega}_{WC} + // - R_{CP} \cdot \prescript{}{P}{\omega}_{WP} + gazebo_angular_velocity = + C_angular_velocity_W_C - + C_pose_P_C_.Rot().Inverse() * P_angular_velocity_W_P; + gazebo_linear_velocity = C_linear_velocity_P_C; + gazebo_pose = C_pose_P_C_; + } + + // This flag could be set to false in the following code... + bool publish_odometry = true; + + // First, determine whether we should publish a odometry. + if (covariance_image_.data != NULL) { + // We have an image. + + // Image is always centered around the origin: + int width = covariance_image_.cols; + int height = covariance_image_.rows; + int x = static_cast( + std::floor(gazebo_pose.Pos().X() / covariance_image_scale_)) + + width / 2; + int y = static_cast( + std::floor(gazebo_pose.Pos().Y() / covariance_image_scale_)) + + height / 2; + + if (x >= 0 && x < width && y >= 0 && y < height) { + uint8_t pixel_value = covariance_image_.at(y, x); + if (pixel_value == 0) { + publish_odometry = false; + // TODO: covariance scaling, according to the intensity values could be + // implemented here. + } + } + } + + if (gazebo_sequence_ % measurement_divisor_ == 0) { + gz_geometry_msgs::Odometry odometry; + odometry.mutable_header()->set_frame_id(parent_frame_id_); + odometry.mutable_header()->mutable_stamp()->set_sec( + (world_->SimTime()).sec + static_cast(unknown_delay_)); + odometry.mutable_header()->mutable_stamp()->set_nsec( + (world_->SimTime()).nsec + static_cast(unknown_delay_)); + odometry.set_child_frame_id(child_frame_id_); + + odometry.mutable_pose()->mutable_pose()->mutable_position()->set_x( + gazebo_pose.Pos().X()); + odometry.mutable_pose()->mutable_pose()->mutable_position()->set_y( + gazebo_pose.Pos().Y()); + odometry.mutable_pose()->mutable_pose()->mutable_position()->set_z( + gazebo_pose.Pos().Z()); + + odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_x( + gazebo_pose.Rot().X()); + odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_y( + gazebo_pose.Rot().Y()); + odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_z( + gazebo_pose.Rot().Z()); + odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_w( + gazebo_pose.Rot().W()); + + odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_x( + gazebo_linear_velocity.X()); + odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_y( + gazebo_linear_velocity.Y()); + odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_z( + gazebo_linear_velocity.Z()); + + odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_x( + gazebo_angular_velocity.X()); + odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_y( + gazebo_angular_velocity.Y()); + odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_z( + gazebo_angular_velocity.Z()); + + if (publish_odometry) + odometry_queue_.push_back( + std::make_pair(gazebo_sequence_ + measurement_delay_, odometry)); + } + + // Is it time to publish the front element? + if (gazebo_sequence_ == odometry_queue_.front().first) { + // Copy the odometry message that is on the queue + gz_geometry_msgs::Odometry odometry_msg(odometry_queue_.front().second); + + // Now that we have copied the first element from the queue, remove it. + odometry_queue_.pop_front(); + + // Calculate position distortions. + Eigen::Vector3d pos_n; + pos_n << position_n_[0](random_generator_) + + position_u_[0](random_generator_), + position_n_[1](random_generator_) + position_u_[1](random_generator_), + position_n_[2](random_generator_) + position_u_[2](random_generator_); + + gazebo::msgs::Vector3d* p = + odometry_msg.mutable_pose()->mutable_pose()->mutable_position(); + p->set_x(p->x() + pos_n[0]); + p->set_y(p->y() + pos_n[1]); + p->set_z(p->z() + pos_n[2]); + + // Calculate attitude distortions. + Eigen::Vector3d theta; + theta << attitude_n_[0](random_generator_) + + attitude_u_[0](random_generator_), + attitude_n_[1](random_generator_) + attitude_u_[1](random_generator_), + attitude_n_[2](random_generator_) + attitude_u_[2](random_generator_); + Eigen::Quaterniond q_n = QuaternionFromSmallAngle(theta); + q_n.normalize(); + + gazebo::msgs::Quaternion* q_W_L = + odometry_msg.mutable_pose()->mutable_pose()->mutable_orientation(); + + Eigen::Quaterniond _q_W_L(q_W_L->w(), q_W_L->x(), q_W_L->y(), q_W_L->z()); + _q_W_L = _q_W_L * q_n; + q_W_L->set_w(_q_W_L.w()); + q_W_L->set_x(_q_W_L.x()); + q_W_L->set_y(_q_W_L.y()); + q_W_L->set_z(_q_W_L.z()); + + // Calculate linear velocity distortions. + Eigen::Vector3d linear_velocity_n; + linear_velocity_n << linear_velocity_n_[0](random_generator_) + + linear_velocity_u_[0](random_generator_), + linear_velocity_n_[1](random_generator_) + + linear_velocity_u_[1](random_generator_), + linear_velocity_n_[2](random_generator_) + + linear_velocity_u_[2](random_generator_); + + gazebo::msgs::Vector3d* linear_velocity = + odometry_msg.mutable_twist()->mutable_twist()->mutable_linear(); + + linear_velocity->set_x(linear_velocity->x() + linear_velocity_n[0]); + linear_velocity->set_y(linear_velocity->y() + linear_velocity_n[1]); + linear_velocity->set_z(linear_velocity->z() + linear_velocity_n[2]); + + // Calculate angular velocity distortions. + Eigen::Vector3d angular_velocity_n; + angular_velocity_n << angular_velocity_n_[0](random_generator_) + + angular_velocity_u_[0](random_generator_), + angular_velocity_n_[1](random_generator_) + + angular_velocity_u_[1](random_generator_), + angular_velocity_n_[2](random_generator_) + + angular_velocity_u_[2](random_generator_); + + gazebo::msgs::Vector3d* angular_velocity = + odometry_msg.mutable_twist()->mutable_twist()->mutable_angular(); + + angular_velocity->set_x(angular_velocity->x() + angular_velocity_n[0]); + angular_velocity->set_y(angular_velocity->y() + angular_velocity_n[1]); + angular_velocity->set_z(angular_velocity->z() + angular_velocity_n[2]); + + odometry_msg.mutable_pose()->mutable_covariance()->Clear(); + for (int i = 0; i < pose_covariance_matrix_.size(); i++) { + odometry_msg.mutable_pose()->mutable_covariance()->Add( + pose_covariance_matrix_[i]); + } + + odometry_msg.mutable_twist()->mutable_covariance()->Clear(); + for (int i = 0; i < twist_covariance_matrix_.size(); i++) { + odometry_msg.mutable_twist()->mutable_covariance()->Add( + twist_covariance_matrix_[i]); + } + + // Publish all the topics, for which the topic name is specified. + if (pose_pub_->HasConnections()) { + pose_pub_->Publish(odometry_msg.pose().pose()); + } + + if (pose_with_covariance_stamped_pub_->HasConnections()) { + gz_geometry_msgs::PoseWithCovarianceStamped + pose_with_covariance_stamped_msg; + + pose_with_covariance_stamped_msg.mutable_header()->CopyFrom( + odometry_msg.header()); + pose_with_covariance_stamped_msg.mutable_pose_with_covariance()->CopyFrom( + odometry_msg.pose()); + + pose_with_covariance_stamped_pub_->Publish( + pose_with_covariance_stamped_msg); + } + + if (position_stamped_pub_->HasConnections()) { + gz_geometry_msgs::Vector3dStamped position_stamped_msg; + position_stamped_msg.mutable_header()->CopyFrom(odometry_msg.header()); + position_stamped_msg.mutable_position()->CopyFrom( + odometry_msg.pose().pose().position()); + + position_stamped_pub_->Publish(position_stamped_msg); + } + + if (transform_stamped_pub_->HasConnections()) { + gz_geometry_msgs::TransformStamped transform_stamped_msg; + + transform_stamped_msg.mutable_header()->CopyFrom(odometry_msg.header()); + transform_stamped_msg.mutable_transform()->mutable_translation()->set_x( + p->x()); + transform_stamped_msg.mutable_transform()->mutable_translation()->set_y( + p->y()); + transform_stamped_msg.mutable_transform()->mutable_translation()->set_z( + p->z()); + transform_stamped_msg.mutable_transform()->mutable_rotation()->CopyFrom( + *q_W_L); + + transform_stamped_pub_->Publish(transform_stamped_msg); + } + + if (odometry_pub_->HasConnections()) { + // DEBUG + odometry_pub_->Publish(odometry_msg); + } + + //==============================================// + //========= BROADCAST TRANSFORM MSG ============// + //==============================================// + + gz_geometry_msgs::TransformStampedWithFrameIds + transform_stamped_with_frame_ids_msg; + transform_stamped_with_frame_ids_msg.mutable_header()->CopyFrom( + odometry_msg.header()); + transform_stamped_with_frame_ids_msg.mutable_transform() + ->mutable_translation() + ->set_x(p->x()); + transform_stamped_with_frame_ids_msg.mutable_transform() + ->mutable_translation() + ->set_y(p->y()); + transform_stamped_with_frame_ids_msg.mutable_transform() + ->mutable_translation() + ->set_z(p->z()); + transform_stamped_with_frame_ids_msg.mutable_transform() + ->mutable_rotation() + ->CopyFrom(*q_W_L); + transform_stamped_with_frame_ids_msg.set_parent_frame_id(parent_frame_id_); + transform_stamped_with_frame_ids_msg.set_child_frame_id(child_frame_id_); + + broadcast_transform_pub_->Publish(transform_stamped_with_frame_ids_msg); + + } // if (gazebo_sequence_ == odometry_queue_.front().first) { + + ++gazebo_sequence_; +} + +void GazeboOdometryPlugin::CreatePubsAndSubs() { + // Create temporary "ConnectGazeboToRosTopic" publisher and message + gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub = + node_handle_->Advertise( + "~/" + kConnectGazeboToRosSubtopic, 1); + + gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg; + + // ============================================ // + // =============== POSE MSG SETUP ============= // + // ============================================ // + + pose_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + pose_pub_topic_, 1); + + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + pose_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + + pose_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::POSE); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); + + // ============================================ // + // == POSE WITH COVARIANCE STAMPED MSG SETUP == // + // ============================================ // + + pose_with_covariance_stamped_pub_ = + node_handle_->Advertise( + "~/" + namespace_ + "/" + pose_with_covariance_stamped_pub_topic_, 1); + + connect_gazebo_to_ros_topic_msg.set_gazebo_topic( + "~/" + namespace_ + "/" + pose_with_covariance_stamped_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic( + namespace_ + "/" + pose_with_covariance_stamped_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::POSE_WITH_COVARIANCE_STAMPED); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); + + // ============================================ // + // ========= POSITION STAMPED MSG SETUP ======= // + // ============================================ // + + position_stamped_pub_ = + node_handle_->Advertise( + "~/" + namespace_ + "/" + position_stamped_pub_topic_, 1); + + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + position_stamped_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + + position_stamped_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::VECTOR_3D_STAMPED); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); + + // ============================================ // + // ============= ODOMETRY MSG SETUP =========== // + // ============================================ // + + odometry_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + odometry_pub_topic_, 1); + + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + odometry_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + + odometry_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::ODOMETRY); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); + + // ============================================ // + // ======== TRANSFORM STAMPED MSG SETUP ======= // + // ============================================ // + + transform_stamped_pub_ = + node_handle_->Advertise( + "~/" + namespace_ + "/" + transform_stamped_pub_topic_, 1); + + connect_gazebo_to_ros_topic_msg.set_gazebo_topic( + "~/" + namespace_ + "/" + transform_stamped_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + + transform_stamped_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::TRANSFORM_STAMPED); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); + + // ============================================ // + // ===== "BROADCAST TRANSFORM" MSG SETUP ===== // + // ============================================ // + + broadcast_transform_pub_ = + node_handle_->Advertise( + "~/" + kBroadcastTransformSubtopic, 1); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboOdometryPlugin); + +} // namespace gazebo diff --git a/rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp new file mode 100644 index 0000000..08803b1 --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp @@ -0,0 +1,166 @@ +/* + * Copyright 2016 Pavel Vechersky, ASL, ETH Zurich, Switzerland + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// MODULE HEADER +#include "rotors_gazebo_plugins/gazebo_pressure_plugin.h" + +// USER HEADERS +#include "ConnectGazeboToRosTopic.pb.h" + +namespace gazebo { + +GazeboPressurePlugin::GazeboPressurePlugin() + : ModelPlugin(), + node_handle_(0), + pubs_and_subs_created_(false) { +} + +GazeboPressurePlugin::~GazeboPressurePlugin() { +} + +void GazeboPressurePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + gzdbg << "_model = " << _model->GetName() << std::endl; + + // Store the pointer to the model and the world. + model_ = _model; + world_ = model_->GetWorld(); + + //==============================================// + //========== READ IN PARAMS FROM SDF ===========// + //==============================================// + + // Use the robot namespace to create the node handle. + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzerr << "[gazebo_pressure_plugin] Please specify a robotNamespace.\n"; + + // Get node handle. + node_handle_ = transport::NodePtr(new transport::Node()); + + // Initialise with default namespace (typically /gazebo/default/). + node_handle_->Init(); + + std::string link_name; + if (_sdf->HasElement("linkName")) + link_name = _sdf->GetElement("linkName")->Get(); + else + gzerr << "[gazebo_pressure_plugin] Please specify a linkName.\n"; + // Get the pointer to the link. + link_ = model_->GetLink(link_name); + if (link_ == NULL) + gzthrow("[gazebo_pressure_plugin] Couldn't find specified link \"" << link_name << "\"."); + + frame_id_ = link_name; + + // Retrieve the rest of the SDF parameters. + getSdfParam(_sdf, "pressureTopic", pressure_topic_, kDefaultPressurePubTopic); + getSdfParam(_sdf, "referenceAltitude", ref_alt_, kDefaultRefAlt); + getSdfParam(_sdf, "pressureVariance", pressure_var_, kDefaultPressureVar); + CHECK(pressure_var_ >= 0.0); + + // Initialize the normal distribution for pressure. + double mean = 0.0; + pressure_n_[0] = NormalDistribution(mean, sqrt(pressure_var_)); + + // Listen to the update event. This event is broadcast every simulation + // iteration. + this->updateConnection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboPressurePlugin::OnUpdate, this, _1)); + + //==============================================// + //=== POPULATE STATIC PARTS OF PRESSURE MSG ====// + //==============================================// + + pressure_message_.mutable_header()->set_frame_id(frame_id_); + pressure_message_.set_variance(pressure_var_); +} + +void GazeboPressurePlugin::OnUpdate(const common::UpdateInfo& _info) { + if (kPrintOnUpdates) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!pubs_and_subs_created_) { + CreatePubsAndSubs(); + pubs_and_subs_created_ = true; + } + + common::Time current_time = world_->SimTime(); + + // Get the current geometric height. + double height_geometric_m = ref_alt_ + model_->WorldPose().Pos().Z(); + + // Compute the geopotential height. + double height_geopotential_m = kEarthRadiusMeters * height_geometric_m / + (kEarthRadiusMeters + height_geometric_m); + + // Compute the temperature at the current altitude. + double temperature_at_altitude_kelvin = + kSeaLevelTempKelvin - kTempLapseKelvinPerMeter * height_geopotential_m; + + // Compute the current air pressure. + double pressure_at_altitude_pascal = + kPressureOneAtmospherePascals * exp(kAirConstantDimensionless * + log(kSeaLevelTempKelvin / temperature_at_altitude_kelvin)); + + // Add noise to pressure measurement. + if(pressure_var_ > 0.0) { + pressure_at_altitude_pascal += pressure_n_[0](random_generator_); + } + + // Fill the pressure message. + pressure_message_.mutable_header()->mutable_stamp()->set_sec( + current_time.sec); + pressure_message_.mutable_header()->mutable_stamp()->set_nsec( + current_time.nsec); + pressure_message_.set_fluid_pressure(pressure_at_altitude_pascal); + + // Publish the pressure message. + pressure_pub_->Publish(pressure_message_); +} + +void GazeboPressurePlugin::CreatePubsAndSubs() { + // Create temporary "ConnectGazeboToRosTopic" publisher and message. + gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub = + node_handle_->Advertise( + "~/" + kConnectGazeboToRosSubtopic, 1); + + // ============================================ // + // ========= FLUID PRESSURE MSG SETUP ========= // + // ============================================ // + + pressure_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + pressure_topic_, 1); + + gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg; + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + pressure_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + + pressure_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::FLUID_PRESSURE); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboPressurePlugin); + +} // namespace gazebo diff --git a/rotors_gazebo_plugins/src/gazebo_ros_interface_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_ros_interface_plugin.cpp new file mode 100644 index 0000000..546a80f --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_ros_interface_plugin.cpp @@ -0,0 +1,1055 @@ +/* + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// MODULE +#include + +// SYSTEM +#include +#include +#include +#include + +// 3RD PARTY +#include +#include + +namespace gazebo { + +GazeboRosInterfacePlugin::GazeboRosInterfacePlugin() + : WorldPlugin(), gz_node_handle_(0), ros_node_handle_(0) {} + +GazeboRosInterfacePlugin::~GazeboRosInterfacePlugin() { + + // Shutdown and delete ROS node handle + if (ros_node_handle_) { + ros_node_handle_->shutdown(); + delete ros_node_handle_; + } +} + +void GazeboRosInterfacePlugin::Load(physics::WorldPtr _world, + sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + /// \brief Store the pointer to the model. + world_ = _world; + + // namespace_.clear(); + + //==============================================// + //========== READ IN PARAMS FROM SDF ===========// + //==============================================// + + /*if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzerr << "Please specify a robotNamespace.\n"; + gzdbg << "namespace_ = \"" << namespace_ << "\"." << std::endl;*/ + + // Get Gazebo node handle + gz_node_handle_ = transport::NodePtr(new transport::Node()); + // gz_node_handle_->Init(namespace_); + gz_node_handle_->Init(); + + // Get ROS node handle + // ros_node_handle_ = new ros::NodeHandle(namespace_); + ros_node_handle_ = new ros::NodeHandle(); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->updateConnection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosInterfacePlugin::OnUpdate, this, _1)); + + // ============================================ // + // === CONNECT GAZEBO TO ROS MESSAGES SETUP === // + // ============================================ // + + gz_connect_gazebo_to_ros_topic_sub_ = gz_node_handle_->Subscribe( + "~/" + kConnectGazeboToRosSubtopic, + &GazeboRosInterfacePlugin::GzConnectGazeboToRosTopicMsgCallback, this); + + // ============================================ // + // === CONNECT ROS TO GAZEBO MESSAGES SETUP === // + // ============================================ // + + gz_connect_ros_to_gazebo_topic_sub_ = gz_node_handle_->Subscribe( + "~/" + kConnectRosToGazeboSubtopic, + &GazeboRosInterfacePlugin::GzConnectRosToGazeboTopicMsgCallback, this); + + // ============================================ // + // ===== BROADCAST TRANSFORM MESSAGE SETUP ==== // + // ============================================ // + + gz_broadcast_transform_sub_ = gz_node_handle_->Subscribe( + "~/" + kBroadcastTransformSubtopic, + &GazeboRosInterfacePlugin::GzBroadcastTransformMsgCallback, this); +} + +void GazeboRosInterfacePlugin::OnUpdate(const common::UpdateInfo& _info) { + // Do nothing + // This plugins actions are all executed through message callbacks. +} + +/// \brief A helper class that provides storage for additional parameters +/// that are inserted into the callback. +/// \details +/// GazeboMsgT The type of the message that will be subscribed to the Gazebo +/// framework. +template +struct ConnectHelperStorage { + /// \brief Pointer to the ROS interface plugin class. + GazeboRosInterfacePlugin* ptr; + + /// \brief Function pointer to the subscriber callback with additional + /// parameters. + void (GazeboRosInterfacePlugin::*fp)( + const boost::shared_ptr&, ros::Publisher ros_publisher); + + /// \brief The ROS publisher that is passed into the modified callback. + ros::Publisher ros_publisher; + + /// \brief This is what gets passed into the Gazebo Subscribe method as a + /// callback, and hence can only + /// have one parameter (note boost::bind() does not work with the + /// current Gazebo Subscribe() definitions). + void callback(const boost::shared_ptr& msg_ptr) { + (ptr->*fp)(msg_ptr, ros_publisher); + } +}; + +template +void GazeboRosInterfacePlugin::ConnectHelper( + void (GazeboRosInterfacePlugin::*fp)( + const boost::shared_ptr&, ros::Publisher), + GazeboRosInterfacePlugin* ptr, std::string gazeboNamespace, + std::string gazeboTopicName, std::string rosTopicName, + transport::NodePtr gz_node_handle) { + // One map will be created for each Gazebo message type + static std::map > callback_map; + + // Create ROS publisher + ros::Publisher ros_publisher = + ros_node_handle_->advertise(rosTopicName, 1); + + auto callback_entry = callback_map.emplace( + gazeboTopicName, + ConnectHelperStorage{ptr, fp, ros_publisher}); + + // Check if element was already present + if (!callback_entry.second) + gzerr << "Tried to add element to map but the gazebo topic name was " + "already present in map." + << std::endl; + + // Create subscriber + gazebo::transport::SubscriberPtr subscriberPtr; + subscriberPtr = gz_node_handle->Subscribe( + gazeboTopicName, &ConnectHelperStorage::callback, + &callback_entry.first->second); + + // Save a reference to the subscriber pointer so subscriber + // won't be deleted. + subscriberPtrs_.push_back(subscriberPtr); +} + +void GazeboRosInterfacePlugin::GzConnectGazeboToRosTopicMsgCallback( + GzConnectGazeboToRosTopicMsgPtr& gz_connect_gazebo_to_ros_topic_msg) { + if (kPrintOnMsgCallback) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + const std::string gazeboNamespace = + ""; // gz_connect_gazebo_to_ros_topic_msg->gazebo_namespace(); + const std::string gazeboTopicName = + gz_connect_gazebo_to_ros_topic_msg->gazebo_topic(); + const std::string rosTopicName = + gz_connect_gazebo_to_ros_topic_msg->ros_topic(); + + gzdbg << "Connecting Gazebo topic \"" << gazeboTopicName + << "\" to ROS topic \"" << rosTopicName << "\"." << std::endl; + + switch (gz_connect_gazebo_to_ros_topic_msg->msgtype()) { + case gz_std_msgs::ConnectGazeboToRosTopic::ACTUATORS: + ConnectHelper( + &GazeboRosInterfacePlugin::GzActuatorsMsgCallback, this, + gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::FLOAT_32: + ConnectHelper( + &GazeboRosInterfacePlugin::GzFloat32MsgCallback, this, + gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::FLUID_PRESSURE: + ConnectHelper( + &GazeboRosInterfacePlugin::GzFluidPressureMsgCallback, this, + gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::IMU: + ConnectHelper( + &GazeboRosInterfacePlugin::GzImuMsgCallback, this, gazeboNamespace, + gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::JOINT_STATE: + ConnectHelper( + &GazeboRosInterfacePlugin::GzJointStateMsgCallback, this, + gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::MAGNETIC_FIELD: + ConnectHelper( + &GazeboRosInterfacePlugin::GzMagneticFieldMsgCallback, this, + gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::NAV_SAT_FIX: + ConnectHelper( + &GazeboRosInterfacePlugin::GzNavSatFixCallback, this, gazeboNamespace, + gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::POSE: + ConnectHelper( + &GazeboRosInterfacePlugin::GzPoseMsgCallback, this, gazeboNamespace, + gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::POSE_WITH_COVARIANCE_STAMPED: + ConnectHelper( + &GazeboRosInterfacePlugin::GzPoseWithCovarianceStampedMsgCallback, + this, gazeboNamespace, gazeboTopicName, rosTopicName, + gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::ODOMETRY: + ConnectHelper( + &GazeboRosInterfacePlugin::GzOdometryMsgCallback, this, + gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::TRANSFORM_STAMPED: + ConnectHelper( + &GazeboRosInterfacePlugin::GzTransformStampedMsgCallback, this, + gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::TWIST_STAMPED: + ConnectHelper( + &GazeboRosInterfacePlugin::GzTwistStampedMsgCallback, this, + gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::VECTOR_3D_STAMPED: + ConnectHelper( + &GazeboRosInterfacePlugin::GzVector3dStampedMsgCallback, this, + gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::WIND_SPEED: + ConnectHelper( + &GazeboRosInterfacePlugin::GzWindSpeedMsgCallback, this, + gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_); + break; + case gz_std_msgs::ConnectGazeboToRosTopic::WRENCH_STAMPED: + ConnectHelper( + &GazeboRosInterfacePlugin::GzWrenchStampedMsgCallback, this, + gazeboNamespace, gazeboTopicName, rosTopicName, gz_node_handle_); + break; + default: + gzthrow("ConnectGazeboToRosTopic message type with enum val = " + << gz_connect_gazebo_to_ros_topic_msg->msgtype() + << " is not supported by GazeboRosInterfacePlugin."); + } + + gzdbg << __FUNCTION__ << "() finished." << std::endl; +} + +void GazeboRosInterfacePlugin::GzConnectRosToGazeboTopicMsgCallback( + GzConnectRosToGazeboTopicMsgPtr& gz_connect_ros_to_gazebo_topic_msg) { + if (kPrintOnMsgCallback) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + static std::vector ros_subscribers; + + switch (gz_connect_ros_to_gazebo_topic_msg->msgtype()) { + case gz_std_msgs::ConnectRosToGazeboTopic::ACTUATORS: { + gazebo::transport::PublisherPtr gz_publisher_ptr = + gz_node_handle_->Advertise( + gz_connect_ros_to_gazebo_topic_msg->gazebo_topic(), 1); + + // Create ROS subscriber. + ros::Subscriber ros_subscriber = + ros_node_handle_->subscribe( + gz_connect_ros_to_gazebo_topic_msg->ros_topic(), 1, + boost::bind(&GazeboRosInterfacePlugin::RosActuatorsMsgCallback, + this, _1, gz_publisher_ptr)); + + // Save reference to the ROS subscriber so callback will continue to be + // called. + ros_subscribers.push_back(ros_subscriber); + + break; + } + case gz_std_msgs::ConnectRosToGazeboTopic::COMMAND_MOTOR_SPEED: { + gazebo::transport::PublisherPtr gz_publisher_ptr = + gz_node_handle_->Advertise( + gz_connect_ros_to_gazebo_topic_msg->gazebo_topic(), 1); + + // Create ROS subscriber. + ros::Subscriber ros_subscriber = + ros_node_handle_->subscribe( + gz_connect_ros_to_gazebo_topic_msg->ros_topic(), 1, + boost::bind( + &GazeboRosInterfacePlugin::RosCommandMotorSpeedMsgCallback, + this, _1, gz_publisher_ptr)); + + // Save reference to the ROS subscriber so callback will continue to be + // called. + ros_subscribers.push_back(ros_subscriber); + + break; + } + case gz_std_msgs::ConnectRosToGazeboTopic::ROLL_PITCH_YAWRATE_THRUST: { + gazebo::transport::PublisherPtr gz_publisher_ptr = + gz_node_handle_->Advertise( + gz_connect_ros_to_gazebo_topic_msg->gazebo_topic(), 1); + + // Create ROS subscriber. + ros::Subscriber ros_subscriber = + ros_node_handle_->subscribe( + gz_connect_ros_to_gazebo_topic_msg->ros_topic(), 1, + boost::bind( + &GazeboRosInterfacePlugin:: + RosRollPitchYawrateThrustMsgCallback, + this, _1, gz_publisher_ptr)); + + // Save reference to the ROS subscriber so callback will continue to be + // called. + ros_subscribers.push_back(ros_subscriber); + + break; + } + case gz_std_msgs::ConnectRosToGazeboTopic::WIND_SPEED: { + gazebo::transport::PublisherPtr gz_publisher_ptr = + gz_node_handle_->Advertise( + gz_connect_ros_to_gazebo_topic_msg->gazebo_topic(), 1); + + // Create ROS subscriber. + ros::Subscriber ros_subscriber = + ros_node_handle_->subscribe( + gz_connect_ros_to_gazebo_topic_msg->ros_topic(), 1, + boost::bind(&GazeboRosInterfacePlugin::RosWindSpeedMsgCallback, + this, _1, gz_publisher_ptr)); + + // Save reference to the ROS subscriber so callback will continue to be + // called. + ros_subscribers.push_back(ros_subscriber); + + break; + } + default: { + gzthrow("ConnectRosToGazeboTopic message type with enum val = " + << gz_connect_ros_to_gazebo_topic_msg->msgtype() + << " is not supported by GazeboRosInterfacePlugin."); + } + } +} + +//===========================================================================// +//==================== HELPER METHODS FOR MSG CONVERSION ====================// +//===========================================================================// + +void GazeboRosInterfacePlugin::ConvertHeaderGzToRos( + const gz_std_msgs::Header& gz_header, + std_msgs::Header_ >* ros_header) { + ros_header->stamp.sec = gz_header.stamp().sec(); + ros_header->stamp.nsec = gz_header.stamp().nsec(); + ros_header->frame_id = gz_header.frame_id(); +} + +void GazeboRosInterfacePlugin::ConvertHeaderRosToGz( + const std_msgs::Header_ >& ros_header, + gz_std_msgs::Header* gz_header) { + gz_header->mutable_stamp()->set_sec(ros_header.stamp.sec); + gz_header->mutable_stamp()->set_nsec(ros_header.stamp.nsec); + gz_header->set_frame_id(ros_header.frame_id); +} + +//===========================================================================// +//================ GAZEBO -> ROS MSG CALLBACKS/CONVERTERS ===================// +//===========================================================================// + +void GazeboRosInterfacePlugin::GzActuatorsMsgCallback( + GzActuatorsMsgPtr& gz_actuators_msg, ros::Publisher ros_publisher) { + // We need to convert the Acutuators message from a Gazebo message to a + // ROS message and then publish it to the ROS framework + + ConvertHeaderGzToRos(gz_actuators_msg->header(), &ros_actuators_msg_.header); + + ros_actuators_msg_.angular_velocities.resize( + gz_actuators_msg->angular_velocities_size()); + for (int i = 0; i < gz_actuators_msg->angular_velocities_size(); i++) { + ros_actuators_msg_.angular_velocities[i] = + gz_actuators_msg->angular_velocities(i); + } + + // Publish to ROS. + ros_publisher.publish(ros_actuators_msg_); +} + +void GazeboRosInterfacePlugin::GzFloat32MsgCallback( + GzFloat32MsgPtr& gz_float_32_msg, ros::Publisher ros_publisher) { + // Convert Gazebo message to ROS message + ros_float_32_msg_.data = gz_float_32_msg->data(); + + // Publish to ROS + ros_publisher.publish(ros_float_32_msg_); +} + +void GazeboRosInterfacePlugin::GzFluidPressureMsgCallback( + GzFluidPressureMsgPtr &gz_fluid_pressure_msg, + ros::Publisher ros_publisher) { + // We need to convert from a Gazebo message to a ROS message, + // and then forward the FluidPressure message onto ROS. + + ConvertHeaderGzToRos(gz_fluid_pressure_msg->header(), + &ros_fluid_pressure_msg_.header); + + ros_fluid_pressure_msg_.fluid_pressure = + gz_fluid_pressure_msg->fluid_pressure(); + + ros_fluid_pressure_msg_.variance = gz_fluid_pressure_msg->variance(); + + // Publish to ROS. + ros_publisher.publish(ros_fluid_pressure_msg_); +} + +void GazeboRosInterfacePlugin::GzImuMsgCallback(GzImuPtr& gz_imu_msg, + ros::Publisher ros_publisher) { + // We need to convert from a Gazebo message to a ROS message, + // and then forward the IMU message onto ROS + + ConvertHeaderGzToRos(gz_imu_msg->header(), &ros_imu_msg_.header); + + ros_imu_msg_.orientation.x = gz_imu_msg->orientation().x(); + ros_imu_msg_.orientation.y = gz_imu_msg->orientation().y(); + ros_imu_msg_.orientation.z = gz_imu_msg->orientation().z(); + ros_imu_msg_.orientation.w = gz_imu_msg->orientation().w(); + + // Orientation covariance should have 9 elements, and both the Gazebo and ROS + // arrays should be the same size! + GZ_ASSERT(gz_imu_msg->orientation_covariance_size() == 9, + "The Gazebo IMU message does not have 9 orientation covariance " + "elements."); + GZ_ASSERT( + ros_imu_msg_.orientation_covariance.size() == 9, + "The ROS IMU message does not have 9 orientation covariance elements."); + for (int i = 0; i < gz_imu_msg->orientation_covariance_size(); i++) { + ros_imu_msg_.orientation_covariance[i] = + gz_imu_msg->orientation_covariance(i); + } + + ros_imu_msg_.angular_velocity.x = gz_imu_msg->angular_velocity().x(); + ros_imu_msg_.angular_velocity.y = gz_imu_msg->angular_velocity().y(); + ros_imu_msg_.angular_velocity.z = gz_imu_msg->angular_velocity().z(); + + GZ_ASSERT(gz_imu_msg->angular_velocity_covariance_size() == 9, + "The Gazebo IMU message does not have 9 angular velocity " + "covariance elements."); + GZ_ASSERT(ros_imu_msg_.angular_velocity_covariance.size() == 9, + "The ROS IMU message does not have 9 angular velocity covariance " + "elements."); + for (int i = 0; i < gz_imu_msg->angular_velocity_covariance_size(); i++) { + ros_imu_msg_.angular_velocity_covariance[i] = + gz_imu_msg->angular_velocity_covariance(i); + } + + ros_imu_msg_.linear_acceleration.x = gz_imu_msg->linear_acceleration().x(); + ros_imu_msg_.linear_acceleration.y = gz_imu_msg->linear_acceleration().y(); + ros_imu_msg_.linear_acceleration.z = gz_imu_msg->linear_acceleration().z(); + + GZ_ASSERT(gz_imu_msg->linear_acceleration_covariance_size() == 9, + "The Gazebo IMU message does not have 9 linear acceleration " + "covariance elements."); + GZ_ASSERT(ros_imu_msg_.linear_acceleration_covariance.size() == 9, + "The ROS IMU message does not have 9 linear acceleration " + "covariance elements."); + for (int i = 0; i < gz_imu_msg->linear_acceleration_covariance_size(); i++) { + ros_imu_msg_.linear_acceleration_covariance[i] = + gz_imu_msg->linear_acceleration_covariance(i); + } + + // Publish to ROS. + ros_publisher.publish(ros_imu_msg_); +} + +void GazeboRosInterfacePlugin::GzJointStateMsgCallback( + GzJointStateMsgPtr& gz_joint_state_msg, ros::Publisher ros_publisher) { + ConvertHeaderGzToRos(gz_joint_state_msg->header(), + &ros_joint_state_msg_.header); + + ros_joint_state_msg_.name.resize(gz_joint_state_msg->name_size()); + for (int i = 0; i < gz_joint_state_msg->name_size(); i++) { + ros_joint_state_msg_.name[i] = gz_joint_state_msg->name(i); + } + + ros_joint_state_msg_.position.resize(gz_joint_state_msg->position_size()); + for (int i = 0; i < gz_joint_state_msg->position_size(); i++) { + ros_joint_state_msg_.position[i] = gz_joint_state_msg->position(i); + } + + // Publish to ROS. + ros_publisher.publish(ros_joint_state_msg_); +} + +void GazeboRosInterfacePlugin::GzMagneticFieldMsgCallback( + GzMagneticFieldMsgPtr& gz_magnetic_field_msg, + ros::Publisher ros_publisher) { + // We need to convert from a Gazebo message to a ROS message, + // and then forward the MagneticField message onto ROS + + ConvertHeaderGzToRos(gz_magnetic_field_msg->header(), + &ros_magnetic_field_msg_.header); + + ros_magnetic_field_msg_.magnetic_field.x = + gz_magnetic_field_msg->magnetic_field().x(); + ros_magnetic_field_msg_.magnetic_field.y = + gz_magnetic_field_msg->magnetic_field().y(); + ros_magnetic_field_msg_.magnetic_field.z = + gz_magnetic_field_msg->magnetic_field().z(); + + // Position covariance should have 9 elements, and both the Gazebo and ROS + // arrays should be the same size! + GZ_ASSERT(gz_magnetic_field_msg->magnetic_field_covariance_size() == 9, + "The Gazebo MagneticField message does not have 9 magnetic field " + "covariance elements."); + GZ_ASSERT(ros_magnetic_field_msg_.magnetic_field_covariance.size() == 9, + "The ROS MagneticField message does not have 9 magnetic field " + "covariance elements."); + for (int i = 0; i < gz_magnetic_field_msg->magnetic_field_covariance_size(); + i++) { + ros_magnetic_field_msg_.magnetic_field_covariance[i] = + gz_magnetic_field_msg->magnetic_field_covariance(i); + } + + // Publish to ROS. + ros_publisher.publish(ros_magnetic_field_msg_); +} + +void GazeboRosInterfacePlugin::GzNavSatFixCallback( + GzNavSatFixPtr& gz_nav_sat_fix_msg, ros::Publisher ros_publisher) { + // We need to convert from a Gazebo message to a ROS message, and then forward + // the NavSatFix message to ROS. + + ConvertHeaderGzToRos(gz_nav_sat_fix_msg->header(), + &ros_nav_sat_fix_msg_.header); + + switch (gz_nav_sat_fix_msg->service()) { + case gz_sensor_msgs::NavSatFix::SERVICE_GPS: + ros_nav_sat_fix_msg_.status.service = + sensor_msgs::NavSatStatus::SERVICE_GPS; + break; + case gz_sensor_msgs::NavSatFix::SERVICE_GLONASS: + ros_nav_sat_fix_msg_.status.service = + sensor_msgs::NavSatStatus::SERVICE_GLONASS; + break; + case gz_sensor_msgs::NavSatFix::SERVICE_COMPASS: + ros_nav_sat_fix_msg_.status.service = + sensor_msgs::NavSatStatus::SERVICE_COMPASS; + break; + case gz_sensor_msgs::NavSatFix::SERVICE_GALILEO: + ros_nav_sat_fix_msg_.status.service = + sensor_msgs::NavSatStatus::SERVICE_GALILEO; + break; + default: + gzthrow( + "Specific value of enum type gz_sensor_msgs::NavSatFix::Service is " + "not yet supported."); + } + + switch (gz_nav_sat_fix_msg->status()) { + case gz_sensor_msgs::NavSatFix::STATUS_NO_FIX: + ros_nav_sat_fix_msg_.status.status = + sensor_msgs::NavSatStatus::STATUS_NO_FIX; + break; + case gz_sensor_msgs::NavSatFix::STATUS_FIX: + ros_nav_sat_fix_msg_.status.status = + sensor_msgs::NavSatStatus::STATUS_FIX; + break; + case gz_sensor_msgs::NavSatFix::STATUS_SBAS_FIX: + ros_nav_sat_fix_msg_.status.status = + sensor_msgs::NavSatStatus::STATUS_SBAS_FIX; + break; + case gz_sensor_msgs::NavSatFix::STATUS_GBAS_FIX: + ros_nav_sat_fix_msg_.status.status = + sensor_msgs::NavSatStatus::STATUS_GBAS_FIX; + break; + default: + gzthrow( + "Specific value of enum type gz_sensor_msgs::NavSatFix::Status is " + "not yet supported."); + } + + ros_nav_sat_fix_msg_.latitude = gz_nav_sat_fix_msg->latitude(); + ros_nav_sat_fix_msg_.longitude = gz_nav_sat_fix_msg->longitude(); + ros_nav_sat_fix_msg_.altitude = gz_nav_sat_fix_msg->altitude(); + + switch (gz_nav_sat_fix_msg->position_covariance_type()) { + case gz_sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN: + ros_nav_sat_fix_msg_.position_covariance_type = + sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; + break; + case gz_sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED: + ros_nav_sat_fix_msg_.position_covariance_type = + sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED; + break; + case gz_sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN: + ros_nav_sat_fix_msg_.position_covariance_type = + sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN; + break; + case gz_sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN: + ros_nav_sat_fix_msg_.position_covariance_type = + sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN; + break; + default: + gzthrow( + "Specific value of enum type " + "gz_sensor_msgs::NavSatFix::PositionCovarianceType is not yet " + "supported."); + } + + // Position covariance should have 9 elements, and both the Gazebo and ROS + // arrays should be the same size! + GZ_ASSERT(gz_nav_sat_fix_msg->position_covariance_size() == 9, + "The Gazebo NavSatFix message does not have 9 position covariance " + "elements."); + GZ_ASSERT(ros_nav_sat_fix_msg_.position_covariance.size() == 9, + "The ROS NavSatFix message does not have 9 position covariance " + "elements."); + for (int i = 0; i < gz_nav_sat_fix_msg->position_covariance_size(); i++) { + ros_nav_sat_fix_msg_.position_covariance[i] = + gz_nav_sat_fix_msg->position_covariance(i); + } + + // Publish to ROS. + ros_publisher.publish(ros_nav_sat_fix_msg_); +} + +void GazeboRosInterfacePlugin::GzOdometryMsgCallback( + GzOdometryMsgPtr& gz_odometry_msg, ros::Publisher ros_publisher) { + // We need to convert from a Gazebo message to a ROS message, and then forward + // the Odometry message to ROS. + + // ============================================ // + // =================== HEADER ================= // + // ============================================ // + ConvertHeaderGzToRos(gz_odometry_msg->header(), &ros_odometry_msg_.header); + + ros_odometry_msg_.child_frame_id = gz_odometry_msg->child_frame_id(); + + // ============================================ // + // ===================== POSE ================= // + // ============================================ // + ros_odometry_msg_.pose.pose.position.x = + gz_odometry_msg->pose().pose().position().x(); + ros_odometry_msg_.pose.pose.position.y = + gz_odometry_msg->pose().pose().position().y(); + ros_odometry_msg_.pose.pose.position.z = + gz_odometry_msg->pose().pose().position().z(); + + ros_odometry_msg_.pose.pose.orientation.w = + gz_odometry_msg->pose().pose().orientation().w(); + ros_odometry_msg_.pose.pose.orientation.x = + gz_odometry_msg->pose().pose().orientation().x(); + ros_odometry_msg_.pose.pose.orientation.y = + gz_odometry_msg->pose().pose().orientation().y(); + ros_odometry_msg_.pose.pose.orientation.z = + gz_odometry_msg->pose().pose().orientation().z(); + + for (int i = 0; i < gz_odometry_msg->pose().covariance_size(); i++) { + ros_odometry_msg_.pose.covariance[i] = + gz_odometry_msg->pose().covariance(i); + } + + // ============================================ // + // ===================== TWIST ================ // + // ============================================ // + ros_odometry_msg_.twist.twist.linear.x = + gz_odometry_msg->twist().twist().linear().x(); + ros_odometry_msg_.twist.twist.linear.y = + gz_odometry_msg->twist().twist().linear().y(); + ros_odometry_msg_.twist.twist.linear.z = + gz_odometry_msg->twist().twist().linear().z(); + + ros_odometry_msg_.twist.twist.angular.x = + gz_odometry_msg->twist().twist().angular().x(); + ros_odometry_msg_.twist.twist.angular.y = + gz_odometry_msg->twist().twist().angular().y(); + ros_odometry_msg_.twist.twist.angular.z = + gz_odometry_msg->twist().twist().angular().z(); + + for (int i = 0; i < gz_odometry_msg->twist().covariance_size(); i++) { + ros_odometry_msg_.twist.covariance[i] = + gz_odometry_msg->twist().covariance(i); + } + + // Publish to ROS framework. + ros_publisher.publish(ros_odometry_msg_); +} + +void GazeboRosInterfacePlugin::GzPoseMsgCallback(GzPoseMsgPtr& gz_pose_msg, + ros::Publisher ros_publisher) { + ros_pose_msg_.position.x = gz_pose_msg->position().x(); + ros_pose_msg_.position.y = gz_pose_msg->position().y(); + ros_pose_msg_.position.z = gz_pose_msg->position().z(); + + ros_pose_msg_.orientation.w = gz_pose_msg->orientation().w(); + ros_pose_msg_.orientation.x = gz_pose_msg->orientation().x(); + ros_pose_msg_.orientation.y = gz_pose_msg->orientation().y(); + ros_pose_msg_.orientation.z = gz_pose_msg->orientation().z(); + + ros_publisher.publish(ros_pose_msg_); +} + +void GazeboRosInterfacePlugin::GzPoseWithCovarianceStampedMsgCallback( + GzPoseWithCovarianceStampedMsgPtr& gz_pose_with_covariance_stamped_msg, + ros::Publisher ros_publisher) { + // ============================================ // + // =================== HEADER ================= // + // ============================================ // + ConvertHeaderGzToRos(gz_pose_with_covariance_stamped_msg->header(), + &ros_pose_with_covariance_stamped_msg_.header); + + // ============================================ // + // === POSE (both position and orientation) === // + // ============================================ // + ros_pose_with_covariance_stamped_msg_.pose.pose.position.x = + gz_pose_with_covariance_stamped_msg->pose_with_covariance() + .pose() + .position() + .x(); + ros_pose_with_covariance_stamped_msg_.pose.pose.position.y = + gz_pose_with_covariance_stamped_msg->pose_with_covariance() + .pose() + .position() + .y(); + ros_pose_with_covariance_stamped_msg_.pose.pose.position.z = + gz_pose_with_covariance_stamped_msg->pose_with_covariance() + .pose() + .position() + .z(); + + ros_pose_with_covariance_stamped_msg_.pose.pose.orientation.w = + gz_pose_with_covariance_stamped_msg->pose_with_covariance() + .pose() + .orientation() + .w(); + ros_pose_with_covariance_stamped_msg_.pose.pose.orientation.x = + gz_pose_with_covariance_stamped_msg->pose_with_covariance() + .pose() + .orientation() + .x(); + ros_pose_with_covariance_stamped_msg_.pose.pose.orientation.y = + gz_pose_with_covariance_stamped_msg->pose_with_covariance() + .pose() + .orientation() + .y(); + ros_pose_with_covariance_stamped_msg_.pose.pose.orientation.z = + gz_pose_with_covariance_stamped_msg->pose_with_covariance() + .pose() + .orientation() + .z(); + + // Covariance should have 36 elements, and both the Gazebo and ROS + // arrays should be the same size! + GZ_ASSERT(gz_pose_with_covariance_stamped_msg->pose_with_covariance() + .covariance_size() == 36, + "The Gazebo PoseWithCovarianceStamped message does not have 9 " + "position covariance elements."); + GZ_ASSERT(ros_pose_with_covariance_stamped_msg_.pose.covariance.size() == 36, + "The ROS PoseWithCovarianceStamped message does not have 9 " + "position covariance elements."); + for (int i = 0; + i < gz_pose_with_covariance_stamped_msg->pose_with_covariance() + .covariance_size(); + i++) { + ros_pose_with_covariance_stamped_msg_.pose.covariance[i] = + gz_pose_with_covariance_stamped_msg->pose_with_covariance().covariance( + i); + } + + ros_publisher.publish(ros_pose_with_covariance_stamped_msg_); +} + +void GazeboRosInterfacePlugin::GzTransformStampedMsgCallback( + GzTransformStampedMsgPtr& gz_transform_stamped_msg, + ros::Publisher ros_publisher) { + // ============================================ // + // =================== HEADER ================= // + // ============================================ // + ConvertHeaderGzToRos(gz_transform_stamped_msg->header(), + &ros_transform_stamped_msg_.header); + + // ============================================ // + // =========== TRANSFORM, TRANSLATION ========= // + // ============================================ // + ros_transform_stamped_msg_.transform.translation.x = + gz_transform_stamped_msg->transform().translation().x(); + ros_transform_stamped_msg_.transform.translation.y = + gz_transform_stamped_msg->transform().translation().y(); + ros_transform_stamped_msg_.transform.translation.z = + gz_transform_stamped_msg->transform().translation().z(); + + // ============================================ // + // ============ TRANSFORM, ROTATION =========== // + // ============================================ // + ros_transform_stamped_msg_.transform.rotation.w = + gz_transform_stamped_msg->transform().rotation().w(); + ros_transform_stamped_msg_.transform.rotation.x = + gz_transform_stamped_msg->transform().rotation().x(); + ros_transform_stamped_msg_.transform.rotation.y = + gz_transform_stamped_msg->transform().rotation().y(); + ros_transform_stamped_msg_.transform.rotation.z = + gz_transform_stamped_msg->transform().rotation().z(); + + ros_publisher.publish(ros_transform_stamped_msg_); +} + +void GazeboRosInterfacePlugin::GzTwistStampedMsgCallback( + GzTwistStampedMsgPtr& gz_twist_stamped_msg, ros::Publisher ros_publisher) { + // ============================================ // + // =================== HEADER ================= // + // ============================================ // + ConvertHeaderGzToRos(gz_twist_stamped_msg->header(), + &ros_twist_stamped_msg_.header); + + // ============================================ // + // =================== TWIST ================== // + // ============================================ // + + ros_twist_stamped_msg_.twist.linear.x = + gz_twist_stamped_msg->twist().linear().x(); + ros_twist_stamped_msg_.twist.linear.y = + gz_twist_stamped_msg->twist().linear().y(); + ros_twist_stamped_msg_.twist.linear.z = + gz_twist_stamped_msg->twist().linear().z(); + + ros_twist_stamped_msg_.twist.angular.x = + gz_twist_stamped_msg->twist().angular().x(); + ros_twist_stamped_msg_.twist.angular.y = + gz_twist_stamped_msg->twist().angular().y(); + ros_twist_stamped_msg_.twist.angular.z = + gz_twist_stamped_msg->twist().angular().z(); + + ros_publisher.publish(ros_twist_stamped_msg_); +} + +void GazeboRosInterfacePlugin::GzVector3dStampedMsgCallback( + GzVector3dStampedMsgPtr& gz_vector_3d_stamped_msg, + ros::Publisher ros_publisher) { + // ============================================ // + // =================== HEADER ================= // + // ============================================ // + ConvertHeaderGzToRos(gz_vector_3d_stamped_msg->header(), + &ros_position_stamped_msg_.header); + + // ============================================ // + // ================== POSITION ================ // + // ============================================ // + + ros_position_stamped_msg_.point.x = gz_vector_3d_stamped_msg->position().x(); + ros_position_stamped_msg_.point.y = gz_vector_3d_stamped_msg->position().y(); + ros_position_stamped_msg_.point.z = gz_vector_3d_stamped_msg->position().z(); + + ros_publisher.publish(ros_position_stamped_msg_); +} + +void GazeboRosInterfacePlugin::GzWindSpeedMsgCallback( + GzWindSpeedMsgPtr& gz_wind_speed_msg, + ros::Publisher ros_publisher) { + // ============================================ // + // =================== HEADER ================= // + // ============================================ // + ConvertHeaderGzToRos(gz_wind_speed_msg->header(), + &ros_wind_speed_msg_.header); + + // ============================================ // + // ================== VELOCITY ================ // + // ============================================ // + ros_wind_speed_msg_.velocity.x = + gz_wind_speed_msg->velocity().x(); + ros_wind_speed_msg_.velocity.y = + gz_wind_speed_msg->velocity().y(); + ros_wind_speed_msg_.velocity.z = + gz_wind_speed_msg->velocity().z(); + ros_publisher.publish(ros_wind_speed_msg_); +} + +void GazeboRosInterfacePlugin::GzWrenchStampedMsgCallback( + GzWrenchStampedMsgPtr& gz_wrench_stamped_msg, + ros::Publisher ros_publisher) { + // ============================================ // + // =================== HEADER ================= // + // ============================================ // + ConvertHeaderGzToRos(gz_wrench_stamped_msg->header(), + &ros_wrench_stamped_msg_.header); + + // ============================================ // + // =================== FORCE ================== // + // ============================================ // + ros_wrench_stamped_msg_.wrench.force.x = + gz_wrench_stamped_msg->wrench().force().x(); + ros_wrench_stamped_msg_.wrench.force.y = + gz_wrench_stamped_msg->wrench().force().y(); + ros_wrench_stamped_msg_.wrench.force.z = + gz_wrench_stamped_msg->wrench().force().z(); + + // ============================================ // + // ==================== TORQUE ================ // + // ============================================ // + ros_wrench_stamped_msg_.wrench.torque.x = + gz_wrench_stamped_msg->wrench().torque().x(); + ros_wrench_stamped_msg_.wrench.torque.y = + gz_wrench_stamped_msg->wrench().torque().y(); + ros_wrench_stamped_msg_.wrench.torque.z = + gz_wrench_stamped_msg->wrench().torque().z(); + + ros_publisher.publish(ros_wrench_stamped_msg_); +} + +//===========================================================================// +//================ ROS -> GAZEBO MSG CALLBACKS/CONVERTERS ===================// +//===========================================================================// + +void GazeboRosInterfacePlugin::RosActuatorsMsgCallback( + const mav_msgs::ActuatorsConstPtr& ros_actuators_msg_ptr, + gazebo::transport::PublisherPtr gz_publisher_ptr) { + // Convert ROS message to Gazebo message + + gz_sensor_msgs::Actuators gz_actuators_msg; + + ConvertHeaderRosToGz(ros_actuators_msg_ptr->header, + gz_actuators_msg.mutable_header()); + + for (int i = 0; i < ros_actuators_msg_ptr->angles.size(); i++) { + gz_actuators_msg.add_angles( + ros_actuators_msg_ptr->angles[i]); + } + + for (int i = 0; i < ros_actuators_msg_ptr->angular_velocities.size(); i++) { + gz_actuators_msg.add_angular_velocities( + ros_actuators_msg_ptr->angular_velocities[i]); + } + + for (int i = 0; i < ros_actuators_msg_ptr->normalized.size(); i++) { + gz_actuators_msg.add_normalized( + ros_actuators_msg_ptr->normalized[i]); + } + + // Publish to Gazebo + gz_publisher_ptr->Publish(gz_actuators_msg); +} + +void GazeboRosInterfacePlugin::RosCommandMotorSpeedMsgCallback( + const mav_msgs::ActuatorsConstPtr& ros_actuators_msg_ptr, + gazebo::transport::PublisherPtr gz_publisher_ptr) { + // Convert ROS message to Gazebo message + + gz_mav_msgs::CommandMotorSpeed gz_command_motor_speed_msg; + + for (int i = 0; i < ros_actuators_msg_ptr->angular_velocities.size(); i++) { + gz_command_motor_speed_msg.add_motor_speed( + ros_actuators_msg_ptr->angular_velocities[i]); + } + + // Publish to Gazebo + gz_publisher_ptr->Publish(gz_command_motor_speed_msg); +} + +void GazeboRosInterfacePlugin::RosRollPitchYawrateThrustMsgCallback( + const mav_msgs::RollPitchYawrateThrustConstPtr& + ros_roll_pitch_yawrate_thrust_msg_ptr, + gazebo::transport::PublisherPtr gz_publisher_ptr) { + // Convert ROS message to Gazebo message + + gz_mav_msgs::RollPitchYawrateThrust gz_roll_pitch_yawrate_thrust_msg; + + ConvertHeaderRosToGz(ros_roll_pitch_yawrate_thrust_msg_ptr->header, + gz_roll_pitch_yawrate_thrust_msg.mutable_header()); + + gz_roll_pitch_yawrate_thrust_msg.set_roll( + ros_roll_pitch_yawrate_thrust_msg_ptr->roll); + gz_roll_pitch_yawrate_thrust_msg.set_pitch( + ros_roll_pitch_yawrate_thrust_msg_ptr->pitch); + gz_roll_pitch_yawrate_thrust_msg.set_yaw_rate( + ros_roll_pitch_yawrate_thrust_msg_ptr->yaw_rate); + + gz_roll_pitch_yawrate_thrust_msg.mutable_thrust()->set_x( + ros_roll_pitch_yawrate_thrust_msg_ptr->thrust.x); + gz_roll_pitch_yawrate_thrust_msg.mutable_thrust()->set_y( + ros_roll_pitch_yawrate_thrust_msg_ptr->thrust.y); + gz_roll_pitch_yawrate_thrust_msg.mutable_thrust()->set_z( + ros_roll_pitch_yawrate_thrust_msg_ptr->thrust.z); + + // Publish to Gazebo + gz_publisher_ptr->Publish(gz_roll_pitch_yawrate_thrust_msg); +} + +void GazeboRosInterfacePlugin::RosWindSpeedMsgCallback( + const rotors_comm::WindSpeedConstPtr& ros_wind_speed_msg_ptr, + gazebo::transport::PublisherPtr gz_publisher_ptr) { + // Convert ROS message to Gazebo message + + gz_mav_msgs::WindSpeed gz_wind_speed_msg; + + ConvertHeaderRosToGz(ros_wind_speed_msg_ptr->header, + gz_wind_speed_msg.mutable_header()); + + gz_wind_speed_msg.mutable_velocity()->set_x( + ros_wind_speed_msg_ptr->velocity.x); + gz_wind_speed_msg.mutable_velocity()->set_y( + ros_wind_speed_msg_ptr->velocity.y); + gz_wind_speed_msg.mutable_velocity()->set_z( + ros_wind_speed_msg_ptr->velocity.z); + + // Publish to Gazebo + gz_publisher_ptr->Publish(gz_wind_speed_msg); +} + +void GazeboRosInterfacePlugin::GzBroadcastTransformMsgCallback( + GzTransformStampedWithFrameIdsMsgPtr& broadcast_transform_msg) { + ros::Time stamp; + stamp.sec = broadcast_transform_msg->header().stamp().sec(); + stamp.nsec = broadcast_transform_msg->header().stamp().nsec(); + + tf::Quaternion tf_q_W_L(broadcast_transform_msg->transform().rotation().x(), + broadcast_transform_msg->transform().rotation().y(), + broadcast_transform_msg->transform().rotation().z(), + broadcast_transform_msg->transform().rotation().w()); + + tf::Vector3 tf_p(broadcast_transform_msg->transform().translation().x(), + broadcast_transform_msg->transform().translation().y(), + broadcast_transform_msg->transform().translation().z()); + + tf_ = tf::Transform(tf_q_W_L, tf_p); + transform_broadcaster_.sendTransform(tf::StampedTransform( + tf_, stamp, broadcast_transform_msg->parent_frame_id(), + broadcast_transform_msg->child_frame_id())); +} + +GZ_REGISTER_WORLD_PLUGIN(GazeboRosInterfacePlugin); + +} // namespace gazebo diff --git a/rotors_gazebo_plugins/src/gazebo_wind_plugin.cpp b/rotors_gazebo_plugins/src/gazebo_wind_plugin.cpp new file mode 100644 index 0000000..4911998 --- /dev/null +++ b/rotors_gazebo_plugins/src/gazebo_wind_plugin.cpp @@ -0,0 +1,445 @@ +/* + * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland + * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland + * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland + * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland + * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland + * Copyright 2016 Geoffrey Hunter + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "rotors_gazebo_plugins/gazebo_wind_plugin.h" + +#include +#include + +#include "ConnectGazeboToRosTopic.pb.h" + +namespace gazebo { + +GazeboWindPlugin::~GazeboWindPlugin() { + +} + +void GazeboWindPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { + if (kPrintOnPluginLoad) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + // Store the pointer to the model. + model_ = _model; + world_ = model_->GetWorld(); + + double wind_gust_start = kDefaultWindGustStart; + double wind_gust_duration = kDefaultWindGustDuration; + + //==============================================// + //========== READ IN PARAMS FROM SDF ===========// + //==============================================// + + if (_sdf->HasElement("robotNamespace")) + namespace_ = _sdf->GetElement("robotNamespace")->Get(); + else + gzerr << "[gazebo_wind_plugin] Please specify a robotNamespace.\n"; + + // Create Gazebo Node. + node_handle_ = gazebo::transport::NodePtr(new transport::Node()); + + // Initialise with default namespace (typically /gazebo/default/). + node_handle_->Init(); + + if (_sdf->HasElement("xyzOffset")) + xyz_offset_ = _sdf->GetElement("xyzOffset")->Get(); + else + gzerr << "[gazebo_wind_plugin] Please specify a xyzOffset.\n"; + + getSdfParam(_sdf, "windForcePubTopic", wind_force_pub_topic_, + wind_force_pub_topic_); + getSdfParam(_sdf, "windSpeedPubTopic", wind_speed_pub_topic_, + wind_speed_pub_topic_); + getSdfParam(_sdf, "frameId", frame_id_, frame_id_); + getSdfParam(_sdf, "linkName", link_name_, link_name_); + // Get the wind speed params from SDF. + getSdfParam(_sdf, "windSpeedMean", wind_speed_mean_, + wind_speed_mean_); + getSdfParam(_sdf, "windSpeedVariance", wind_speed_variance_, + wind_speed_variance_); + getSdfParam(_sdf, "windDirection", wind_direction_, + wind_direction_); + // Check if a custom static wind field should be used. + getSdfParam(_sdf, "useCustomStaticWindField", use_custom_static_wind_field_, + use_custom_static_wind_field_); + if (!use_custom_static_wind_field_) { + gzdbg << "[gazebo_wind_plugin] Using user-defined constant wind field and gusts.\n"; + // Get the wind params from SDF. + getSdfParam(_sdf, "windForceMean", wind_force_mean_, + wind_force_mean_); + getSdfParam(_sdf, "windForceVariance", wind_force_variance_, + wind_force_variance_); + // Get the wind gust params from SDF. + getSdfParam(_sdf, "windGustStart", wind_gust_start, wind_gust_start); + getSdfParam(_sdf, "windGustDuration", wind_gust_duration, + wind_gust_duration); + getSdfParam(_sdf, "windGustForceMean", wind_gust_force_mean_, + wind_gust_force_mean_); + getSdfParam(_sdf, "windGustForceVariance", wind_gust_force_variance_, + wind_gust_force_variance_); + getSdfParam(_sdf, "windGustDirection", wind_gust_direction_, + wind_gust_direction_); + + wind_direction_.Normalize(); + wind_gust_direction_.Normalize(); + wind_gust_start_ = common::Time(wind_gust_start); + wind_gust_end_ = common::Time(wind_gust_start + wind_gust_duration); + } else { + gzdbg << "[gazebo_wind_plugin] Using custom wind field from text file.\n"; + // Get the wind field text file path, read it and save data. + std::string custom_wind_field_path; + getSdfParam(_sdf, "customWindFieldPath", custom_wind_field_path, + custom_wind_field_path); + ReadCustomWindField(custom_wind_field_path); + } + + link_ = model_->GetLink(link_name_); + if (link_ == NULL) + gzthrow("[gazebo_wind_plugin] Couldn't find specified link \"" << link_name_ + << "\"."); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboWindPlugin::OnUpdate, this, _1)); +} + +// This gets called by the world update start event. +void GazeboWindPlugin::OnUpdate(const common::UpdateInfo& _info) { + if (kPrintOnUpdates) { + gzdbg << __FUNCTION__ << "() called." << std::endl; + } + + if (!pubs_and_subs_created_) { + CreatePubsAndSubs(); + pubs_and_subs_created_ = true; + } + + // Get the current simulation time. + common::Time now = world_->SimTime(); + + ignition::math::Vector3d wind_velocity(0.0, 0.0, 0.0); + + // Choose user-specified method for calculating wind velocity. + if (!use_custom_static_wind_field_) { + // Calculate the wind force. + double wind_strength = wind_force_mean_; + ignition::math::Vector3d wind = wind_strength * wind_direction_; + // Apply a force from the constant wind to the link. + link_->AddForceAtRelativePosition(wind, xyz_offset_); + + ignition::math::Vector3d wind_gust(0.0, 0.0, 0.0); + // Calculate the wind gust force. + if (now >= wind_gust_start_ && now < wind_gust_end_) { + double wind_gust_strength = wind_gust_force_mean_; + wind_gust = wind_gust_strength * wind_gust_direction_; + // Apply a force from the wind gust to the link. + link_->AddForceAtRelativePosition(wind_gust, xyz_offset_); + } + + wrench_stamped_msg_.mutable_header()->set_frame_id(frame_id_); + wrench_stamped_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec); + wrench_stamped_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec); + + wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_x(wind.X() + + wind_gust.X()); + wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_y(wind.Y() + + wind_gust.Y()); + wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_z(wind.Z() + + wind_gust.Z()); + + // No torque due to wind, set x,y and z to 0. + wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_x(0); + wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_y(0); + wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_z(0); + + wind_force_pub_->Publish(wrench_stamped_msg_); + + // Calculate the wind speed. + wind_velocity = wind_speed_mean_ * wind_direction_; + } else { + // Get the current position of the aircraft in world coordinates. + ignition::math::Vector3d link_position = link_->WorldPose().Pos(); + + // Calculate the x, y index of the grid points with x, y-coordinate + // just smaller than or equal to aircraft x, y position. + std::size_t x_inf = floor((link_position.X() - min_x_) / res_x_); + std::size_t y_inf = floor((link_position.Y() - min_y_) / res_y_); + + // In case aircraft is on one of the boundary surfaces at max_x or max_y, + // decrease x_inf, y_inf by one to have x_sup, y_sup on max_x, max_y. + if (x_inf == n_x_ - 1u) { + x_inf = n_x_ - 2u; + } + if (y_inf == n_y_ - 1u) { + y_inf = n_y_ - 2u; + } + + // Calculate the x, y index of the grid points with x, y-coordinate just + // greater than the aircraft x, y position. + std::size_t x_sup = x_inf + 1u; + std::size_t y_sup = y_inf + 1u; + + // Save in an array the x, y index of each of the eight grid points + // enclosing the aircraft. + constexpr unsigned int n_vertices = 8; + std::size_t idx_x[n_vertices] = {x_inf, x_inf, x_sup, x_sup, x_inf, x_inf, x_sup, x_sup}; + std::size_t idx_y[n_vertices] = {y_inf, y_inf, y_inf, y_inf, y_sup, y_sup, y_sup, y_sup}; + + // Find the vertical factor of the aircraft in each of the four surrounding + // grid columns, and their minimal/maximal value. + constexpr unsigned int n_columns = 4; + float vertical_factors_columns[n_columns]; + for (std::size_t i = 0u; i < n_columns; ++i) { + vertical_factors_columns[i] = ( + link_position.Z() - bottom_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_]) / + (top_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_] - bottom_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_]); + } + + // Find maximal and minimal value amongst vertical factors. + float vertical_factors_min = std::min(std::min(std::min( + vertical_factors_columns[0], vertical_factors_columns[1]), + vertical_factors_columns[2]), vertical_factors_columns[3]); + float vertical_factors_max = std::max(std::max(std::max( + vertical_factors_columns[0], vertical_factors_columns[1]), + vertical_factors_columns[2]), vertical_factors_columns[3]); + + // Check if aircraft is out of wind field or not, and act accordingly. + if (x_inf >= 0u && y_inf >= 0u && vertical_factors_max >= 0u && + x_sup <= (n_x_ - 1u) && y_sup <= (n_y_ - 1u) && vertical_factors_min <= 1u) { + // Find indices in z-direction for each of the vertices. If link is not + // within the range of one of the columns, set to lowest or highest two. + std::size_t idx_z[n_vertices] = {0u, static_cast(vertical_spacing_factors_.size()) - 1u, + 0u, static_cast(vertical_spacing_factors_.size()) - 1u, + 0u, static_cast(vertical_spacing_factors_.size()) - 1u, + 0u, static_cast(vertical_spacing_factors_.size()) - 1u}; + for (std::size_t i = 0u; i < n_columns; ++i) { + if (vertical_factors_columns[i] < 0u) { + // Link z-position below lowest grid point of that column. + idx_z[2u * i + 1u] = 1u; + } else if (vertical_factors_columns[i] >= 1u) { + // Link z-position above highest grid point of that column. + idx_z[2u * i] = vertical_spacing_factors_.size() - 2u; + } else { + // Link z-position between two grid points in that column. + for (std::size_t j = 0u; j < vertical_spacing_factors_.size() - 1u; ++j) { + if (vertical_spacing_factors_[j] <= vertical_factors_columns[i] && + vertical_spacing_factors_[j + 1u] > vertical_factors_columns[i]) { + idx_z[2u * i] = j; + idx_z[2u * i + 1u] = j + 1u; + break; + } + } + } + } + + // Extract the wind velocities corresponding to each vertex. + ignition::math::Vector3d wind_at_vertices[n_vertices]; + for (std::size_t i = 0u; i < n_vertices; ++i) { + wind_at_vertices[i].X() = u_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_]; + wind_at_vertices[i].Y() = v_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_]; + wind_at_vertices[i].Z() = w_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_]; + } + + // Extract the relevant coordinate of every point needed for trilinear + // interpolation (first z-direction, then x-direction, then y-direction). + constexpr unsigned int n_points_interp_z = 8; + constexpr unsigned int n_points_interp_x = 4; + constexpr unsigned int n_points_interp_y = 2; + double interpolation_points[n_points_interp_x + n_points_interp_y + n_points_interp_z]; + for (std::size_t i = 0u; i < n_points_interp_x + n_points_interp_y + n_points_interp_z; ++i) { + if (i < n_points_interp_z) { + interpolation_points[i] = ( + top_z_[idx_x[i] + idx_y[i] * n_x_] - bottom_z_[idx_x[i] + idx_y[i] * n_x_]) + * vertical_spacing_factors_[idx_z[i]] + bottom_z_[idx_x[i] + idx_y[i] * n_x_]; + } else if (i >= n_points_interp_z && i < n_points_interp_x + n_points_interp_z) { + interpolation_points[i] = min_x_ + res_x_ * idx_x[2u * (i - n_points_interp_z)]; + } else { + interpolation_points[i] = min_y_ + res_y_ * idx_y[4u * (i - n_points_interp_z - n_points_interp_x)]; + } + } + + // Interpolate wind velocity at aircraft position. + wind_velocity = TrilinearInterpolation( + link_position, wind_at_vertices, interpolation_points); + } else { + // Set the wind velocity to the default constant value specified by user. + wind_velocity = wind_speed_mean_ * wind_direction_; + } + } + + wind_speed_msg_.mutable_header()->set_frame_id(frame_id_); + wind_speed_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec); + wind_speed_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec); + + wind_speed_msg_.mutable_velocity()->set_x(wind_velocity.X()); + wind_speed_msg_.mutable_velocity()->set_y(wind_velocity.Y()); + wind_speed_msg_.mutable_velocity()->set_z(wind_velocity.Z()); + + wind_speed_pub_->Publish(wind_speed_msg_); +} + +void GazeboWindPlugin::CreatePubsAndSubs() { + // Create temporary "ConnectGazeboToRosTopic" publisher and message. + gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub = + node_handle_->Advertise( + "~/" + kConnectGazeboToRosSubtopic, 1); + + gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg; + + // ============================================ // + // ========= WRENCH STAMPED MSG SETUP ========= // + // ============================================ // + wind_force_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + wind_force_pub_topic_, 1); + + // connect_gazebo_to_ros_topic_msg.set_gazebo_namespace(namespace_); + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + wind_force_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + + wind_force_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::WRENCH_STAMPED); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); + + // ============================================ // + // ========== WIND SPEED MSG SETUP ============ // + // ============================================ // + wind_speed_pub_ = node_handle_->Advertise( + "~/" + namespace_ + "/" + wind_speed_pub_topic_, 1); + + connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" + + wind_speed_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" + + wind_speed_pub_topic_); + connect_gazebo_to_ros_topic_msg.set_msgtype( + gz_std_msgs::ConnectGazeboToRosTopic::WIND_SPEED); + connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg, + true); +} + +void GazeboWindPlugin::ReadCustomWindField(std::string& custom_wind_field_path) { + std::ifstream fin; + fin.open(custom_wind_field_path); + if (fin.is_open()) { + std::string data_name; + float data; + // Read the line with the variable name. + while (fin >> data_name) { + // Save data on following line into the correct variable. + if (data_name == "min_x:") { + fin >> min_x_; + } else if (data_name == "min_y:") { + fin >> min_y_; + } else if (data_name == "n_x:") { + fin >> n_x_; + } else if (data_name == "n_y:") { + fin >> n_y_; + } else if (data_name == "res_x:") { + fin >> res_x_; + } else if (data_name == "res_y:") { + fin >> res_y_; + } else if (data_name == "vertical_spacing_factors:") { + while (fin >> data) { + vertical_spacing_factors_.push_back(data); + if (fin.peek() == '\n') break; + } + } else if (data_name == "bottom_z:") { + while (fin >> data) { + bottom_z_.push_back(data); + if (fin.peek() == '\n') break; + } + } else if (data_name == "top_z:") { + while (fin >> data) { + top_z_.push_back(data); + if (fin.peek() == '\n') break; + } + } else if (data_name == "u:") { + while (fin >> data) { + u_.push_back(data); + if (fin.peek() == '\n') break; + } + } else if (data_name == "v:") { + while (fin >> data) { + v_.push_back(data); + if (fin.peek() == '\n') break; + } + } else if (data_name == "w:") { + while (fin >> data) { + w_.push_back(data); + if (fin.peek() == '\n') break; + } + } else { + // If invalid data name, read the rest of the invalid line, + // publish a message and ignore data on next line. Then resume reading. + std::string restOfLine; + getline(fin, restOfLine); + gzerr << " [gazebo_wind_plugin] Invalid data name '" << data_name << restOfLine << + "' in custom wind field text file. Ignoring data on next line.\n"; + fin.ignore(std::numeric_limits::max(), '\n'); + } + } + fin.close(); + gzdbg << "[gazebo_wind_plugin] Successfully read custom wind field from text file.\n"; + } else { + gzerr << "[gazebo_wind_plugin] Could not open custom wind field text file.\n"; + } + +} + +ignition::math::Vector3d GazeboWindPlugin::LinearInterpolation( + double position, ignition::math::Vector3d * values, double* points) const { + ignition::math::Vector3d value = values[0] + (values[1] - values[0]) / + (points[1] - points[0]) * (position - points[0]); + return value; +} + +ignition::math::Vector3d GazeboWindPlugin::BilinearInterpolation( + double* position, ignition::math::Vector3d * values, double* points) const { + ignition::math::Vector3d intermediate_values[2] = { LinearInterpolation( + position[0], &(values[0]), &(points[0])), + LinearInterpolation( + position[0], &(values[2]), &(points[2])) }; + ignition::math::Vector3d value = LinearInterpolation( + position[1], intermediate_values, &(points[4])); + return value; +} + +ignition::math::Vector3d GazeboWindPlugin::TrilinearInterpolation( + ignition::math::Vector3d link_position, ignition::math::Vector3d * values, double* points) const { + double position[3] = {link_position.X(),link_position.Y(),link_position.Z()}; + ignition::math::Vector3d intermediate_values[4] = { LinearInterpolation( + position[2], &(values[0]), &(points[0])), + LinearInterpolation( + position[2], &(values[2]), &(points[2])), + LinearInterpolation( + position[2], &(values[4]), &(points[4])), + LinearInterpolation( + position[2], &(values[6]), &(points[6])) }; + ignition::math::Vector3d value = BilinearInterpolation( + &(position[0]), intermediate_values, &(points[8])); + return value; +} + +GZ_REGISTER_MODEL_PLUGIN(GazeboWindPlugin); + +} // namespace gazebo diff --git a/rotors_gazebo_plugins/src/geo_mag_declination.cpp b/rotors_gazebo_plugins/src/geo_mag_declination.cpp new file mode 100644 index 0000000..75e42ab --- /dev/null +++ b/rotors_gazebo_plugins/src/geo_mag_declination.cpp @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (c) 2014 MAV GEO Library (MAVGEO). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name MAVGEO nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** +* @file geo_mag_declination.c +* +* Calculation / lookup table for earth magnetic field declination. +* +* Lookup table from Scott Ferguson +* +* XXX Lookup table currently too coarse in resolution (only full degrees) +* and lat/lon res - needs extension medium term. +* +*/ + +#include +#include +#include "rotors_gazebo_plugins/geo_mag_declination.h" + +/** set this always to the sampling in degrees for the table below */ +#define SAMPLING_RES 10.0f +#define SAMPLING_MIN_LAT -60.0f +#define SAMPLING_MAX_LAT 60.0f +#define SAMPLING_MIN_LON -180.0f +#define SAMPLING_MAX_LON 180.0f + +static const int8_t declination_table[13][37] = \ +{ + { 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46 }, + { 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, 29, 30 }, + { 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21 }, + { 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, 10, 13, 15, 16 }, + { 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12 }, + { 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10 }, + { 9, 9, 9, 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, 7, 8, 9 }, + { 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, 0, 0, 0, 1, 3, 5, 7, 8 }, + { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8 }, + { 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6 }, + { 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5 }, + { 4, 8, 12, 15, 17, 18, 16, 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, 0, 4 }, + { 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 }, +}; + +static float get_lookup_table_val(unsigned lat, unsigned lon); + +float get_mag_declination(float lat_rad, float lon_rad) +{ + float lat = lat_rad / M_PI * 180.0f; + float lon = lon_rad / M_PI * 180.0f; + + /* + * If the values exceed valid ranges, return zero as default + * as we have no way of knowing what the closest real value + * would be. + */ + if (lat < -90.0f || lat > 90.0f || + lon < -180.0f || lon > 180.0f) { + return 0.0f; + } + + /* round down to nearest sampling resolution */ + int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES; + int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES; + + /* for the rare case of hitting the bounds exactly + * the rounding logic wouldn't fit, so enforce it. + */ + + /* limit to table bounds - required for maxima even when table spans full globe range */ + if (lat <= SAMPLING_MIN_LAT) { + min_lat = SAMPLING_MIN_LAT; + } + + if (lat >= SAMPLING_MAX_LAT) { + min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + if (lon <= SAMPLING_MIN_LON) { + min_lon = SAMPLING_MIN_LON; + } + + if (lon >= SAMPLING_MAX_LON) { + min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + /* find index of nearest low sampling point */ + unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES; + + float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); + float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); + float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1); + float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index); + + /* perform bilinear interpolation on the four grid corners */ + + float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; + float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; + + float declination_ret = ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min; + + return declination_ret / 180.0f * M_PI; +} + +float get_lookup_table_val(unsigned lat_index, unsigned lon_index) +{ + return declination_table[lat_index][lon_index]; +} diff --git a/rotors_gazebo_plugins/src/liftdrag_plugin/liftdrag_plugin.cpp b/rotors_gazebo_plugins/src/liftdrag_plugin/liftdrag_plugin.cpp new file mode 100644 index 0000000..e462712 --- /dev/null +++ b/rotors_gazebo_plugins/src/liftdrag_plugin/liftdrag_plugin.cpp @@ -0,0 +1,396 @@ +/* + * Copyright (C) 2014-2016 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include "gazebo/common/Assert.hh" +#include "gazebo/physics/physics.hh" +#include "gazebo/sensors/SensorManager.hh" +#include "gazebo/transport/transport.hh" +#include "gazebo/msgs/msgs.hh" +#include "liftdrag_plugin/liftdrag_plugin.h" + +using namespace gazebo; + +GZ_REGISTER_MODEL_PLUGIN(LiftDragPlugin) + +///////////////////////////////////////////////// +LiftDragPlugin::LiftDragPlugin() : cla(1.0), cda(0.01), cma(0.01), rho(1.2041) +{ + this->cp = ignition::math::Vector3d (0, 0, 0); + this->forward = ignition::math::Vector3d (1, 0, 0); + this->upward = ignition::math::Vector3d (0, 0, 1); + this->area = 1.0; + this->alpha0 = 0.0; + this->alpha = 0.0; + this->sweep = 0.0; + this->velocityStall = 0.0; + + // 90 deg stall + this->alphaStall = 0.5*M_PI; + this->claStall = 0.0; + + this->radialSymmetry = false; + + /// \TODO: what's flat plate drag? + this->cdaStall = 1.0; + this->cmaStall = 0.0; + + /// how much to change CL per every radian of the control joint value + this->controlJointRadToCL = 4.0; +} + +///////////////////////////////////////////////// +LiftDragPlugin::~LiftDragPlugin() +{ +} + +///////////////////////////////////////////////// +void LiftDragPlugin::Load(physics::ModelPtr _model, + sdf::ElementPtr _sdf) +{ + GZ_ASSERT(_model, "LiftDragPlugin _model pointer is NULL"); + GZ_ASSERT(_sdf, "LiftDragPlugin _sdf pointer is NULL"); + this->model = _model; + this->sdf = _sdf; + + this->world = this->model->GetWorld(); + GZ_ASSERT(this->world, "LiftDragPlugin world pointer is NULL"); + + this->physics = this->world->Physics(); + GZ_ASSERT(this->physics, "LiftDragPlugin physics pointer is NULL"); + + GZ_ASSERT(_sdf, "LiftDragPlugin _sdf pointer is NULL"); + + if (_sdf->HasElement("radial_symmetry")) + this->radialSymmetry = _sdf->Get("radial_symmetry"); + + if (_sdf->HasElement("a0")) + this->alpha0 = _sdf->Get("a0"); + + if (_sdf->HasElement("cla")) + this->cla = _sdf->Get("cla"); + + if (_sdf->HasElement("cda")) + this->cda = _sdf->Get("cda"); + + if (_sdf->HasElement("cma")) + this->cma = _sdf->Get("cma"); + + if (_sdf->HasElement("alpha_stall")) + this->alphaStall = _sdf->Get("alpha_stall"); + + if (_sdf->HasElement("cla_stall")) + this->claStall = _sdf->Get("cla_stall"); + + if (_sdf->HasElement("cda_stall")) + this->cdaStall = _sdf->Get("cda_stall"); + + if (_sdf->HasElement("cma_stall")) + this->cmaStall = _sdf->Get("cma_stall"); + + if (_sdf->HasElement("cp")) + this->cp = _sdf->Get("cp"); + + // blade forward (-drag) direction in link frame + if (_sdf->HasElement("forward")) + this->forward = _sdf->Get("forward"); + this->forward.Normalize(); + + // blade upward (+lift) direction in link frame + if (_sdf->HasElement("upward")) + this->upward = _sdf->Get("upward"); + this->upward.Normalize(); + + if (_sdf->HasElement("area")) + this->area = _sdf->Get("area"); + + if (_sdf->HasElement("air_density")) + this->rho = _sdf->Get("air_density"); + + if (_sdf->HasElement("link_name")) + { + sdf::ElementPtr elem = _sdf->GetElement("link_name"); + // GZ_ASSERT(elem, "Element link_name doesn't exist!"); + std::string linkName = elem->Get(); + this->link = this->model->GetLink(linkName); + // GZ_ASSERT(this->link, "Link was NULL"); + + if (!this->link) + { + gzerr << "Link with name[" << linkName << "] not found. " + << "The LiftDragPlugin will not generate forces\n"; + } + else + { + this->updateConnection = event::Events::ConnectWorldUpdateBegin( + boost::bind(&LiftDragPlugin::OnUpdate, this)); + } + } + + if (_sdf->HasElement("control_joint_name")) + { + std::string controlJointName = _sdf->Get("control_joint_name"); + this->controlJoint = this->model->GetJoint(controlJointName); + if (!this->controlJoint) + { + gzerr << "Joint with name[" << controlJointName << "] does not exist.\n"; + } + } + + if (_sdf->HasElement("control_joint_rad_to_cl")) + this->controlJointRadToCL = _sdf->Get("control_joint_rad_to_cl"); +} + +///////////////////////////////////////////////// +void LiftDragPlugin::OnUpdate() +{ + GZ_ASSERT(this->link, "Link was NULL"); + // get linear velocity at cp in inertial frame + ignition::math::Vector3d vel = this->link->WorldLinearVel(this->cp); + ignition::math::Vector3d velI = vel; + velI.Normalize(); + + // smoothing + // double e = 0.8; + // this->velSmooth = e*vel + (1.0 - e)*velSmooth; + // vel = this->velSmooth; + + if (vel.Length() <= 0.01) + return; + + // pose of body + ignition::math::Pose3d pose = this->link->WorldPose(); + + // rotate forward and upward vectors into inertial frame + ignition::math::Vector3d forwardI = pose.Rot().RotateVector(this->forward); + + ignition::math::Vector3d upwardI; + if (this->radialSymmetry) + { + // use inflow velocity to determine upward direction + // which is the component of inflow perpendicular to forward direction. + ignition::math::Vector3d tmp = forwardI.Cross(velI); + upwardI = forwardI.Cross(tmp).Normalize(); + } + else + { + upwardI = pose.Rot().RotateVector(this->upward); + } + + // spanwiseI: a vector normal to lift-drag-plane described in inertial frame + ignition::math::Vector3d spanwiseI = forwardI.Cross(upwardI).Normalize(); + + const double minRatio = -1.0; + const double maxRatio = 1.0; + // check sweep (angle between velI and lift-drag-plane) + double sinSweepAngle = ignition::math::clamp( + spanwiseI.Dot(velI), minRatio, maxRatio); + + // get cos from trig identity + double cosSweepAngle = 1.0 - sinSweepAngle * sinSweepAngle; + this->sweep = asin(sinSweepAngle); + + // truncate sweep to within +/-90 deg + while (fabs(this->sweep) > 0.5 * M_PI) + this->sweep = this->sweep > 0 ? this->sweep - M_PI + : this->sweep + M_PI; + + // angle of attack is the angle between + // velI projected into lift-drag plane + // and + // forward vector + // + // projected = spanwiseI Xcross ( vector Xcross spanwiseI) + // + // so, + // removing spanwise velocity from vel + ignition::math::Vector3d velInLDPlane = vel - vel.Dot(spanwiseI)*velI; + + // get direction of drag + ignition::math::Vector3d dragDirection = -velInLDPlane; + dragDirection.Normalize(); + + // get direction of lift + ignition::math::Vector3d liftI = spanwiseI.Cross(velInLDPlane); + liftI.Normalize(); + + // get direction of moment + ignition::math::Vector3d momentDirection = spanwiseI; + + // compute angle between upwardI and liftI + // in general, given vectors a and b: + // cos(theta) = a.Dot(b)/(a.Length()*b.Lenghth()) + // given upwardI and liftI are both unit vectors, we can drop the denominator + // cos(theta) = a.Dot(b) + double cosAlpha = ignition::math::clamp(liftI.Dot(upwardI), minRatio, maxRatio); + + // Is alpha positive or negative? Test: + // forwardI points toward zero alpha + // if forwardI is in the same direction as lift, alpha is positive. + // liftI is in the same direction as forwardI? + if (liftI.Dot(forwardI) >= 0.0) + this->alpha = this->alpha0 + acos(cosAlpha); + else + this->alpha = this->alpha0 - acos(cosAlpha); + + // normalize to within +/-90 deg + while (fabs(this->alpha) > 0.5 * M_PI) + this->alpha = this->alpha > 0 ? this->alpha - M_PI + : this->alpha + M_PI; + + // compute dynamic pressure + double speedInLDPlane = velInLDPlane.Length(); + double q = 0.5 * this->rho * speedInLDPlane * speedInLDPlane; + + // compute cl at cp, check for stall, correct for sweep + double cl; + if (this->alpha > this->alphaStall) + { + cl = (this->cla * this->alphaStall + + this->claStall * (this->alpha - this->alphaStall)) + * cosSweepAngle; + // make sure cl is still great than 0 + cl = std::max(0.0, cl); + } + else if (this->alpha < -this->alphaStall) + { + cl = (-this->cla * this->alphaStall + + this->claStall * (this->alpha + this->alphaStall)) + * cosSweepAngle; + // make sure cl is still less than 0 + cl = std::min(0.0, cl); + } + else + cl = this->cla * this->alpha * cosSweepAngle; + + // modify cl per control joint value + if (this->controlJoint) + { + double controlAngle = this->controlJoint->Position(0); + cl = cl + this->controlJointRadToCL * controlAngle; + /// \TODO: also change cm and cd + } + + // compute lift force at cp + ignition::math::Vector3d lift = cl * q * this->area * liftI; + + // compute cd at cp, check for stall, correct for sweep + double cd; + if (this->alpha > this->alphaStall) + { + cd = (this->cda * this->alphaStall + + this->cdaStall * (this->alpha - this->alphaStall)) + * cosSweepAngle; + } + else if (this->alpha < -this->alphaStall) + { + cd = (-this->cda * this->alphaStall + + this->cdaStall * (this->alpha + this->alphaStall)) + * cosSweepAngle; + } + else + cd = (this->cda * this->alpha) * cosSweepAngle; + + // make sure drag is positive + cd = fabs(cd); + + // drag at cp + ignition::math::Vector3d drag = cd * q * this->area * dragDirection; + + // compute cm at cp, check for stall, correct for sweep + double cm; + if (this->alpha > this->alphaStall) + { + cm = (this->cma * this->alphaStall + + this->cmaStall * (this->alpha - this->alphaStall)) + * cosSweepAngle; + // make sure cm is still great than 0 + cm = std::max(0.0, cm); + } + else if (this->alpha < -this->alphaStall) + { + cm = (-this->cma * this->alphaStall + + this->cmaStall * (this->alpha + this->alphaStall)) + * cosSweepAngle; + // make sure cm is still less than 0 + cm = std::min(0.0, cm); + } + else + cm = this->cma * this->alpha * cosSweepAngle; + + /// \TODO: implement cm + /// for now, reset cm to zero, as cm needs testing + cm = 0.0; + + // compute moment (torque) at cp + ignition::math::Vector3d moment = cm * q * this->area * momentDirection; + + // moment arm from cg to cp in inertial plane + ignition::math::Vector3d momentArm = pose.Rot().RotateVector( + this->cp - this->link->GetInertial()->CoG()); + // gzerr << this->cp << " : " << this->link->GetInertial()->GetCoG() << "\n"; + + // force and torque about cg in inertial frame + ignition::math::Vector3d force = lift + drag; + // + moment.Cross(momentArm); + + ignition::math::Vector3d torque = moment; + // - lift.Cross(momentArm) - drag.Cross(momentArm); + + // debug + // + // if ((this->link->GetName() == "wing_1" || + // this->link->GetName() == "wing_2") && + // (vel.GetLength() > 50.0 && + // vel.GetLength() < 50.0)) + if (0) + { + gzdbg << "=============================\n"; + gzdbg << "sensor: [" << this->GetHandle() << "]\n"; + gzdbg << "Link: [" << this->link->GetName() + << "] pose: [" << pose + << "] dynamic pressure: [" << q << "]\n"; + gzdbg << "spd: [" << vel.Length() + << "] vel: [" << vel << "]\n"; + gzdbg << "LD plane spd: [" << velInLDPlane.Length() + << "] vel : [" << velInLDPlane << "]\n"; + gzdbg << "forward (inertial): " << forwardI << "\n"; + gzdbg << "upward (inertial): " << upwardI << "\n"; + gzdbg << "lift dir (inertial): " << liftI << "\n"; + gzdbg << "Span direction (normal to LD plane): " << spanwiseI << "\n"; + gzdbg << "sweep: " << this->sweep << "\n"; + gzdbg << "alpha: " << this->alpha << "\n"; + gzdbg << "lift: " << lift << "\n"; + gzdbg << "drag: " << drag << " cd: " + << cd << " cda: " << this->cda << "\n"; + gzdbg << "moment: " << moment << "\n"; + gzdbg << "cp momentArm: " << momentArm << "\n"; + gzdbg << "force: " << force << "\n"; + gzdbg << "torque: " << torque << "\n"; + } + + // Correct for nan or inf + force.Correct(); + this->cp.Correct(); + torque.Correct(); + + // apply forces at cg (with torques for position shift) + this->link->AddForceAtRelativePosition(force, this->cp); + this->link->AddTorque(torque); +}