diff --git a/gazebo_ros_pkgs/CONTRIBUTING.md b/gazebo_ros_pkgs/CONTRIBUTING.md new file mode 100755 index 0000000..fb9f8ee --- /dev/null +++ b/gazebo_ros_pkgs/CONTRIBUTING.md @@ -0,0 +1,35 @@ +# How to Contribute + +We welcome contributions to this repo and encourage you to fork the project, thanks! + +## Implementations of simulations + +It is highly recommended to place the code that performs simulation inside +[upstream gazebo code](https://bitbucket.org/osrf/gazebo/). The Gazebo project +provides documentation about [how to create and code +plugins](http://gazebosim.org/tutorials?cat=write_plugin) Ideally +gazebo_ros_pkgs should implement the ROS wrapper over an existing gazebo +plugin. + +## Issues and Pull Requests + +There are several maintainers of different packages in this repository. If you +are submitting an issue or a pull request, please prefix the title of the issue +or pull resquest with the package name. This way the appropriate maintainers +can more easily recognize contributions which require their attention. + +## Style + +We follow the [C++ ROS style guidelines](http://ros.org/wiki/CppStyleGuide) and +conventions as closely as possible. However, because the plugins inherit from Gazebo +classes and Gazebo follows a very different formatting standard, there are a few +exceptions where Gazebo's function names do not comply to the ROS guidelines. + +## Tests + +We encourage developers to include tests in their pull requests when possible, +both for bug fixes and new features. +Examples of tests are in the `gazebo_plugins/test*` folders. +Currently the tests must not be run in parallel (see issue +[issue 409](https://github.com/ros-simulation/gazebo_ros_pkgs/issues/409)) +so it is recommented to use the `-j1` argument to `catkin run_tests`. diff --git a/gazebo_ros_pkgs/README.md b/gazebo_ros_pkgs/README.md new file mode 100755 index 0000000..00720c3 --- /dev/null +++ b/gazebo_ros_pkgs/README.md @@ -0,0 +1,17 @@ +# gazebo_ros_pkgs + +[![Build Status](http://build.ros.org/buildStatus/icon?job=Kpr__gazebo_ros_pkgs__ubuntu_xenial_amd64)](http://build.ros.org/job/Kpr__gazebo_ros_pkgs__ubuntu_xenial_amd64) + +Wrappers, tools and additional API's for using ROS with the Gazebo simulator. Formally simulator_gazebo stack, gazebo_pkgs is a meta package. Now Catkinized and works with the standalone Gazebo debian. + +### Installation +[Installing gazebo_ros_pkgs](http://gazebosim.org/tutorials?tut=ros_installing&cat=connect_ros) + +### Documentation and Tutorials +[On gazebosim.org](http://gazebosim.org/tutorials?cat=connect_ros) + +### Develop and Contribute + +See [Contribute](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/hydro-devel/CONTRIBUTING.md) page. + + diff --git a/gazebo_ros_pkgs/SENSORS.md b/gazebo_ros_pkgs/SENSORS.md new file mode 100755 index 0000000..f2a8658 --- /dev/null +++ b/gazebo_ros_pkgs/SENSORS.md @@ -0,0 +1,241 @@ +# Plugins supported in Gazebo ROS packages + +## Initial note + +As detailed in the CONTRIBUITON.md guide, Gazebo ROS packages are a wrapper that +connects upstream Gazebo simulations with the ROS framework. Ideally all the +plugins in this repository should implement the ROS wrapper over a gazebo plugin +(plugin code in the [upstream gazebo repository](https://bitbucket.org/osrf/gazebo/)). + +## Sensors + +### Template + + - ***description:*** short description of the wrapper. + - ***status:*** + - **maintained**: no big issues, should work fine + - **autotest**: the wrapper has automatic tests + - **doxygen**: the wrapper implements doxygen comments + - **needs-cleanup**: the code of the wrapper should be improved + - **not-just-a-wrapper**: the code in this repo is not just a wrapper and + contains simulation code. The simulation code should be migrated to + the Gazebo repository. + - ***gazebo plugin:*** gazebo upstream plugin used by the wrapper + - ***example:*** example files that use the wrapper + +### ROS wrappers for Gazebo upstream sensors + + * ***gazebo_ros_block_laser*** + - ***description:*** implements ray based sensors (lasers). Publishes + sensors_msgs::PointCloud. + - ***status:*** maintained + - ***gazebo plugin:*** RayPlugin + - ***example:*** gazebo_plugins/test/test_worlds/gazebo_ros_block_laser.world + gazebo_plugins/test/test_worlds/gazebo_ros_trimesh_collision.world + + * ***gazebo_ros_bumper*** + - ***description:*** implements a contact sensor. Publishes + gazebo_msgs::ContactsState. + - ***status:*** maintained, needs-cleanup + - ***gazebo plugin:*** + - ***example:*** gazebo_plugins/test/bumper_test/gazebo_ros_bumper.world: + - test: gazebo_plugins/test/test_worlds/bumper_test.world: + + * ***gazebo_ros_camera*** + gazebo_ros_camera_utils + - ***description:*** implements a camera. Publishes: sensor_msgs::Image, sensor_msgs::CameraInfo + - ***status:*** maintained, dynamic-reconfigure, autotest + - ***gazebo plugin:*** CameraPlugin, GazeboRosCameraUtils + - ***example:*** gazebo_plugins/test/test_worlds/gazebo_ros_block_laser.world: + gazebo_plugins/test/test_worlds/gazebo_ros_depth_camera.world: + gazebo_plugins/test/test_worlds/gazebo_ros_camera.world: + gazebo_plugins/test/test_worlds/gazebo_ros_trimesh_collision.world: + gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro: + + * ***gazebo_ros_f3d*** + - ***description:*** controller that fake (empty publisher) a 6 dof force sensor + Publishes geometry_msgs::WrenchStamped + - ***status:*** stub + - ***gazebo plugin:*** ModelPlugin (generic) + - ***example:*** -- + + * ***gazebo_ros_force*** + - ***description:*** collects data from a ROS topic and applies wrench to a body accordingly. + - ***status:*** maintained, doxygen + - ***gazebo plugin:*** ModelPlugin (generic) + - ***example:*** -- + + * ***gazebo_ros_ft_sensor*** + - ***description:*** implements Force/Torque sensor. + Publishes: geometry_msgs/WrenchStamped messages + - ***status:*** maintained, doxygen + - ***gazebo plugin:*** ModelPlugin (generic) + - ***example:*** -- + + * ***gazebo_ros_gpu_laser*** + - ***description:*** implements GPU laser based sensors. + Publishes: sensor_msgs::LaserScan + - ***status:*** maintained + - ***gazebo plugin:*** GpuRayPlugin + - ***example:*** gazebo_plugins/test/test_worlds/gazebo_ros_gpu_laser.world + gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo_gpu.xacro + + * ***gazebo_ros_laser*** + - ***description:*** ROS wrapper for ray based sensors (lasers). + Publishes sensor_msgs::LaserScan. + - ***status:*** maintained + - ***gazebo plugin:*** RayPlugin + - ***example:*** test/test_worlds/gazebo_ros_laser.world + test/test_worlds/test_lasers.world + test/multi_robot_scenario/xacro/laser/hokuyo.xacro + + * ***gazebo_ros_multicamera*** + MultiCameraPlugin + gazebo_ros_camera_utils + - ***description:*** ROS wrapper for one or more synchronized cameras. + Publishes: sensor_msgs::Image, sensor_msgs::CameraInfo + - ***status:*** maintained, autotest + - ***gazebo plugin:*** MultiCameraPlugin + - ***example:*** -- + + * ***gazebo_ros_openni_kinect*** + - ***description:*** ROS wrapper for depth camera sensors (Kinect like) + Publishes: sensor_msgs::PointCloud2, sensor_msgs::Image + - ***status:*** maintained + - ***gazebo plugin:*** DepthCameraPlugin + - ***example:*** -- + + * ***gazebo_ros_prosilica*** + - ***description:*** plugin for simulating prosilica cameras in gazebo + Publishes: sensor_msgs::Image, sensor_msgs::CameraInfo + - ***status:*** maintained + - ***gazebo plugin:*** > CameraPlugin > ModelPlugin (generic) + - ***example:*** -- + + * ***gazebo_ros_range*** + - ***description:*** simulate range sensors like infrared or ultrasounds + Publishes: sensor_msgs/Range + - ***status:*** maintained + - ***gazebo plugin:*** RayPlugin + - ***example:*** gazebo_plugins/test/test_worlds/gazebo_ros_range.world + gazebo_ros/launch/range_world.launch + + + +### ROS implementations for sensors (not recommended) + +Although there is some code in this repo that implements simulations, this +should be an exception. Please checkout the CONTRIBUTION.md guide for more +details about which code should be submitted to this repository. + + * ***gazebo_ros_depth_camera*** + - ***description:*** implements depth camera based sensors. Publishes: sensor_msgs::Image, + sensor_msgs::CameraInfo, sensors_msgs::PointCloud2 + - ***status:*** maintained, not-just-a-wrapper, autotest, needs-cleanup + - ***gazebo plugin:*** DepthCameraPlugin + - ***example:*** gazebo_plugins/test/test_worlds/gazebo_ros_depth_camera.world + gazebo_plugins/test/test_worlds/gazebo_ros_trimesh_collision.world + + * ***gazebo_ros_diff_drive*** + - ***description:*** implements a diff drive base. Publishes: sensor_msgs::JointState, + nav_msgs::Odometry + - ***status:*** maintained, not-just-a-wrapper, rosparams, needs-cleanup + - ***gazebo plugin:*** ModelPlugin (generic) + - ***example:*** gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro + gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.gazebo.launch + + +## Other plugins (not related to sensors) + + * ***gazebo_ros_elevator*** + - ***description:*** implements an elevator + - ***status:*** maintained + - ***gazebo plugin:*** ElevatorPlugin + - ***example:*** gazebo_plugins/test/test_worlds/elevator.world + + * ***gazebo_ros_hand_of_god*** + - ***description:*** Drives floating object around based on the location of a TF frame + - ***status:*** maintained, doxygen + - ***gazebo plugin:*** ModelPlugin (generic) + - ***example:*** -- + + * ***gazebo_ros_joint_pose_trajectory*** + - ***description:*** get a pose trajectory (trajectory_msgs::JointTrajectory) and execute it + - ***status:*** maintained + - ***gazebo_plugin:*** -- + - ***example:*** gazebo_plugins/test/pub_joint_trajectory_test.cpp + + * ***gazebo_ros_joint_state_publisher*** + - ***description:*** ROS plugin that publishes the state of a given set of joints at a given rate + Publishes: sensors_msgs::JointState + - ***status:*** maintained + - ***gazebo_plugin:*** -- + - ***example:*** multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro + + * ***gazebo_ros_p3d*** + - ***description:*** publish 3D position interface for ground truth + Publishes: nav_msgs::Odometry + - ***status:*** maintained + - ***gazebo plugin:*** ModelPlugin (generic) + - ***example:*** test/p3d_test/worlds/single_pendulum.world + test/p3d_test/worlds/3_double_pendulums.world + test2/large_models/smaller_large_model.urdf.xacro + test2/large_models/large_model.urdf.xacro + + * ***gazebo_ros_planar_move*** + - ***description:*** simple model controller that uses a twist message to move a robot on the xy plane + Publishes: nav_msgs::Odometry, tf + - ***status:*** maintained + - ***gazebo plugin:*** ModelPlugin (generic) + - ***example:*** + + * ***gazebo_ros_projector*** + - ***description:*** controller that controls texture projection into the world from a given body. + Publish: msgs::Projector + - ***status:*** maintained + - ***gazebo plugin:*** ModelPlugin (generic) + - ***example:*** -- (some information in the header file) + + * ***gazebo_ros_skid_steer_drive*** + - ***description:*** A skid steering drive plugin + Publish: nav_msgs::Odometry + - ***status:*** maintained + - ***gazebo plugin:*** ModelPlugin (generic) + - ***example:*** -- + + * ***gazebo_ros_tricycle_drive*** + - ***description:*** a tricycle drive plugin for gazebo + Publish: nav_msgs::Odometry, sensor_msgs::JointState + - ***status:*** maintained, rosparams + - ***gazebo plugin:*** ModelPlugin (generic) + - ***example:*** gazebo_plugins/test/tricycle_drive/ + xacro, launch, world, rviz, etc. + + * ***gazebo_ros_video*** + - ***description:*** Video plugin for displaying ROS image topics on Ogre textures + - ***status:*** maintained + - ***gazebo plugin:*** VisualPlugin + - ***example:*** -- + + +## List of deprecated plugins (to be removed) + + * ***gazebo_ros_joint_trajectory*** + - ***description:*** migrate to gazebo_ros_joint_pose_trajectory + - ***status:*** deprecated. To be removed in Lunar. + - ***gazebo plugin:*** -- + - ***example:*** -- + + * ***camera_synchronizer*** + * ***vision_reconfigure*** + - ***description:*** + - ***status:*** needs-work, dead? + - ***gazebo plugin:*** CameraSynchronizerConfig + - ***example:*** + + * ***gazebo_ros_imu*** + - ***description:*** implements an IMU sensor. Publishes: sensor_msgs::Imu + - ***status:*** maintained, not-just-a-wrapper + - ***gazebo plugin:*** ModelPlugin (generic) + - ***example:*** -- + diff --git a/gazebo_ros_pkgs/gazebo_dev/CHANGELOG.rst b/gazebo_ros_pkgs/gazebo_dev/CHANGELOG.rst new file mode 100755 index 0000000..3bd922b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_dev/CHANGELOG.rst @@ -0,0 +1,16 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gazebo_dev +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.5.15 (2018-02-12) +------------------- + +2.5.14 (2017-12-11) +------------------- + +2.5.13 (2017-06-24) +------------------- +* Add catkin package(s) to provide the default version of Gazebo - take II (kinetic-devel) (`#571 `_) + * Added catkin package gazebo_dev which provides the cmake config of the installed Gazebo version + * gazebo_dev: added execution dependency gazebo +* Contributors: Jose Luis Rivero diff --git a/gazebo_ros_pkgs/gazebo_dev/CMakeLists.txt b/gazebo_ros_pkgs/gazebo_dev/CMakeLists.txt new file mode 100755 index 0000000..563a8e7 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_dev/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gazebo_dev) + +find_package(catkin REQUIRED) + +catkin_package( + CFG_EXTRAS gazebo_dev-extras.cmake +) diff --git a/gazebo_ros_pkgs/gazebo_dev/cmake/gazebo_dev-extras.cmake b/gazebo_ros_pkgs/gazebo_dev/cmake/gazebo_dev-extras.cmake new file mode 100755 index 0000000..d4bdd3b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_dev/cmake/gazebo_dev-extras.cmake @@ -0,0 +1,13 @@ +# Depend on system install of Gazebo +find_package(gazebo REQUIRED) + +message(STATUS "Gazebo version: ${GAZEBO_VERSION}") + +# The following lines will tell catkin to add the Gazebo directories and libraries to the +# respective catkin_* cmake variables. +set(gazebo_dev_INCLUDE_DIRS ${GAZEBO_INCLUDE_DIRS}) +set(gazebo_dev_LIBRARY_DIRS ${GAZEBO_LIBRARY_DIRS}) +set(gazebo_dev_LIBRARIES ${GAZEBO_LIBRARIES}) + +# Append gazebo CXX_FLAGS to CMAKE_CXX_FLAGS (c++11) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") diff --git a/gazebo_ros_pkgs/gazebo_dev/package.xml b/gazebo_ros_pkgs/gazebo_dev/package.xml new file mode 100755 index 0000000..cc682a7 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_dev/package.xml @@ -0,0 +1,25 @@ + + + gazebo_dev + 2.5.15 + + Provides a cmake config for the default version of Gazebo for the ROS distribution. + + + John Hsu + Dave Coleman + + Apache 2.0 + + http://gazebosim.org/tutorials?cat=connect_ros + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs + + Johannes Meyer + + catkin + + libgazebo7-dev + gazebo + + diff --git a/gazebo_ros_pkgs/gazebo_msgs/CHANGELOG.rst b/gazebo_ros_pkgs/gazebo_msgs/CHANGELOG.rst new file mode 100755 index 0000000..bcdf004 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/CHANGELOG.rst @@ -0,0 +1,164 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gazebo_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.5.15 (2018-02-12) +------------------- + +2.5.14 (2017-12-11) +------------------- + +2.5.13 (2017-06-24) +------------------- +* Add catkin package(s) to provide the default version of Gazebo (`#571 `_) + * Added catkin package gazebo_dev which provides the cmake config of the installed Gazebo version +* Contributors: Jose Luis Rivero + +2.5.12 (2017-04-25) +------------------- + +2.5.11 (2017-04-18) +------------------- +* Changed the spawn model methods to spawn also lights. (`#511 `_) +* Contributors: Alessandro Ambrosano + +2.5.10 (2017-03-03) +------------------- + +2.5.9 (2017-02-20) +------------------ +* Removed all trailing whitespace +* Contributors: Dave Coleman + +2.5.8 (2016-12-06) +------------------ + +2.5.7 (2016-06-10) +------------------ + +2.5.6 (2016-04-28) +------------------ + +2.5.5 (2016-04-27) +------------------ +* merge indigo, jade to kinetic-devel +* Update maintainer for Kinetic release +* Contributors: Jose Luis Rivero, Steven Peters + +2.5.3 (2016-04-11) +------------------ + +2.5.2 (2016-02-25) +------------------ +* merging from indigo-devel +* 2.4.9 +* Generate changelog +* GetModelState modification for jade +* Contributors: John Hsu, Jose Luis Rivero, Markus Bader + +2.5.1 (2015-08-16) +------------------ + +2.5.0 (2015-04-30) +------------------ + +2.4.10 (2016-02-25) +------------------- + +2.4.9 (2015-08-16) +------------------ + +2.4.8 (2015-03-17) +------------------ + +2.4.7 (2014-12-15) +------------------ +* Update Gazebo/ROS tutorial URL +* Contributors: Jose Luis Rivero + +2.4.6 (2014-09-01) +------------------ + +2.4.5 (2014-08-18) +------------------ + +2.4.4 (2014-07-18) +------------------ +* Fix repo names in package.xml's +* Contributors: Jon Binney + +2.4.3 (2014-05-12) +------------------ + +2.4.2 (2014-03-27) +------------------ +* merging from hydro-devel +* bump patch version for indigo-devel to 2.4.1 +* merging from indigo-devel after 2.3.4 release +* "2.4.0" +* catkin_generate_changelog +* Contributors: John Hsu + +2.4.1 (2013-11-13) +------------------ +* rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. + +2.4.0 (2013-10-14) +------------------ + +2.3.5 (2014-03-26) +------------------ + +2.3.4 (2013-11-13) +------------------ + +2.3.3 (2013-10-10) +------------------ + +2.3.2 (2013-09-19) +------------------ + +2.3.1 (2013-08-27) +------------------ + +2.3.0 (2013-08-12) +------------------ + +2.2.1 (2013-07-29) +------------------ + +2.2.0 (2013-07-29) +------------------ + +2.1.5 (2013-07-18) +------------------ + +2.1.4 (2013-07-14) +------------------ + +2.1.3 (2013-07-13) +------------------ + +2.1.2 (2013-07-12) +------------------ +* Cleaned up CMakeLists.txt for all gazebo_ros_pkgs +* 2.1.1 + +2.1.1 (2013-07-10 19:11) +------------------------ + +2.1.0 (2013-06-27) +------------------ + +2.0.2 (2013-06-20) +------------------ + +2.0.1 (2013-06-19) +------------------ +* Incremented version to 2.0.1 + +2.0.0 (2013-06-18) +------------------ +* Changed version to 2.0.0 based on gazebo_simulator being 1.0.0 +* Updated package.xml files for ros.org documentation purposes +* Imported from bitbucket.org diff --git a/gazebo_ros_pkgs/gazebo_msgs/CMakeLists.txt b/gazebo_ros_pkgs/gazebo_msgs/CMakeLists.txt new file mode 100755 index 0000000..e15014c --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gazebo_msgs) + +find_package(catkin REQUIRED COMPONENTS + std_msgs + trajectory_msgs + geometry_msgs + sensor_msgs + std_srvs + message_generation + ) + +add_message_files( + DIRECTORY msg + FILES + ContactsState.msg + ContactState.msg + LinkState.msg + LinkStates.msg + ModelState.msg + ModelStates.msg + ODEJointProperties.msg + ODEPhysics.msg + WorldState.msg + ) + +add_service_files(DIRECTORY srv FILES + ApplyBodyWrench.srv + DeleteModel.srv + DeleteLight.srv + GetLinkState.srv + GetPhysicsProperties.srv + SetJointProperties.srv + SetModelConfiguration.srv + SpawnModel.srv + ApplyJointEffort.srv + GetJointProperties.srv + GetModelProperties.srv + GetWorldProperties.srv + SetLinkProperties.srv + SetModelState.srv + BodyRequest.srv + GetLinkProperties.srv + GetModelState.srv + JointRequest.srv + SetLinkState.srv + SetPhysicsProperties.srv + SetJointTrajectory.srv + GetLightProperties.srv + SetLightProperties.srv + ) + +generate_messages(DEPENDENCIES + std_msgs + geometry_msgs + sensor_msgs + trajectory_msgs + ) + +catkin_package( + CATKIN_DEPENDS + message_runtime + std_msgs + trajectory_msgs + geometry_msgs + sensor_msgs + std_srvs + ) diff --git a/gazebo_ros_pkgs/gazebo_msgs/msg/ContactState.msg b/gazebo_ros_pkgs/gazebo_msgs/msg/ContactState.msg new file mode 100755 index 0000000..f8240c2 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/msg/ContactState.msg @@ -0,0 +1,8 @@ +string info # text info on this contact +string collision1_name # name of contact collision1 +string collision2_name # name of contact collision2 +geometry_msgs/Wrench[] wrenches # list of forces/torques +geometry_msgs/Wrench total_wrench # sum of forces/torques in every DOF +geometry_msgs/Vector3[] contact_positions # list of contact position +geometry_msgs/Vector3[] contact_normals # list of contact normals +float64[] depths # list of penetration depths diff --git a/gazebo_ros_pkgs/gazebo_msgs/msg/ContactsState.msg b/gazebo_ros_pkgs/gazebo_msgs/msg/ContactsState.msg new file mode 100755 index 0000000..6682350 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/msg/ContactsState.msg @@ -0,0 +1,2 @@ +Header header # stamp +gazebo_msgs/ContactState[] states # array of geom pairs in contact diff --git a/gazebo_ros_pkgs/gazebo_msgs/msg/LinkState.msg b/gazebo_ros_pkgs/gazebo_msgs/msg/LinkState.msg new file mode 100755 index 0000000..6dcf278 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/msg/LinkState.msg @@ -0,0 +1,6 @@ +# @todo: FIXME: sets pose and twist of a link. All children link poses/twists of the URDF tree are not updated accordingly, but should be. +string link_name # link name, link_names are in gazebo scoped name notation, [model_name::body_name] +geometry_msgs/Pose pose # desired pose in reference frame +geometry_msgs/Twist twist # desired twist in reference frame +string reference_frame # set pose/twist relative to the frame of this link/body + # leave empty or "world" or "map" defaults to world-frame diff --git a/gazebo_ros_pkgs/gazebo_msgs/msg/LinkStates.msg b/gazebo_ros_pkgs/gazebo_msgs/msg/LinkStates.msg new file mode 100755 index 0000000..275dd1d --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/msg/LinkStates.msg @@ -0,0 +1,4 @@ +# broadcast all link states in world frame +string[] name # link names +geometry_msgs/Pose[] pose # desired pose in world frame +geometry_msgs/Twist[] twist # desired twist in world frame diff --git a/gazebo_ros_pkgs/gazebo_msgs/msg/ModelState.msg b/gazebo_ros_pkgs/gazebo_msgs/msg/ModelState.msg new file mode 100755 index 0000000..10b5ad8 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/msg/ModelState.msg @@ -0,0 +1,7 @@ +# Set Gazebo Model pose and twist +string model_name # model to set state (pose and twist) +geometry_msgs/Pose pose # desired pose in reference frame +geometry_msgs/Twist twist # desired twist in reference frame +string reference_frame # set pose/twist relative to the frame of this entity (Body/Model) + # leave empty or "world" or "map" defaults to world-frame + diff --git a/gazebo_ros_pkgs/gazebo_msgs/msg/ModelStates.msg b/gazebo_ros_pkgs/gazebo_msgs/msg/ModelStates.msg new file mode 100755 index 0000000..1eac239 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/msg/ModelStates.msg @@ -0,0 +1,4 @@ +# broadcast all model states in world frame +string[] name # model names +geometry_msgs/Pose[] pose # desired pose in world frame +geometry_msgs/Twist[] twist # desired twist in world frame diff --git a/gazebo_ros_pkgs/gazebo_msgs/msg/ODEJointProperties.msg b/gazebo_ros_pkgs/gazebo_msgs/msg/ODEJointProperties.msg new file mode 100755 index 0000000..40ea605 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/msg/ODEJointProperties.msg @@ -0,0 +1,11 @@ +# access to low level joint properties, change these at your own risk +float64[] damping # joint damping +float64[] hiStop # joint limit +float64[] loStop # joint limit +float64[] erp # set joint erp +float64[] cfm # set joint cfm +float64[] stop_erp # set joint erp for joint limit "contact" joint +float64[] stop_cfm # set joint cfm for joint limit "contact" joint +float64[] fudge_factor # joint fudge_factor applied at limits, see ODE manual for info. +float64[] fmax # ode joint param fmax +float64[] vel # ode joint param vel diff --git a/gazebo_ros_pkgs/gazebo_msgs/msg/ODEPhysics.msg b/gazebo_ros_pkgs/gazebo_msgs/msg/ODEPhysics.msg new file mode 100755 index 0000000..c0e6b7e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/msg/ODEPhysics.msg @@ -0,0 +1,10 @@ +bool auto_disable_bodies # enable auto disabling of bodies, default false +uint32 sor_pgs_precon_iters # preconditioning inner iterations when uisng projected Gauss Seidel +uint32 sor_pgs_iters # inner iterations when uisng projected Gauss Seidel +float64 sor_pgs_w # relaxation parameter when using projected Gauss Seidel, 1 = no relaxation +float64 sor_pgs_rms_error_tol # rms error tolerance before stopping inner iterations +float64 contact_surface_layer # contact "dead-band" width +float64 contact_max_correcting_vel # contact maximum correction velocity +float64 cfm # global constraint force mixing +float64 erp # global error reduction parameter +uint32 max_contacts # maximum contact joints between two geoms diff --git a/gazebo_ros_pkgs/gazebo_msgs/msg/WorldState.msg b/gazebo_ros_pkgs/gazebo_msgs/msg/WorldState.msg new file mode 100755 index 0000000..e523e77 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/msg/WorldState.msg @@ -0,0 +1,42 @@ +# This is a message that holds data necessary to reconstruct a snapshot of the world +# +# = Approach to Message Passing = +# The state of the world is defined by either +# 1. Inertial Model pose, twist +# * kinematic data - connectivity graph from Model to each Link +# * joint angles +# * joint velocities +# * Applied forces - Body wrench +# * relative transform from Body to each collision Geom +# Or +# 2. Inertial (absolute) Body pose, twist, wrench +# * relative transform from Body to each collision Geom - constant, so not sent over wire +# * back compute from canonical body info to get Model pose and twist. +# +# Chooing (2.) because it matches most physics engines out there +# and is simpler. +# +# = Future = +# Consider impacts on using reduced coordinates / graph (parent/child links) approach +# constraint and physics solvers. +# +# = Application = +# This message is used to do the following: +# * reconstruct the world and objects for sensor generation +# * stop / start simulation - need pose, twist, wrench of each body +# * collision detection - need pose of each collision geometry. velocity/acceleration if +# +# = Assumptions = +# Assuming that each (physics) processor node locally already has +# * collision information - Trimesh for Geoms, etc +# * relative transforms from Body to Geom - this is assumed to be fixed, do not send oved wire +# * inertial information - does not vary in time +# * visual information - does not vary in time +# + +Header header + +string[] name +geometry_msgs/Pose[] pose +geometry_msgs/Twist[] twist +geometry_msgs/Wrench[] wrench diff --git a/gazebo_ros_pkgs/gazebo_msgs/package.xml b/gazebo_ros_pkgs/gazebo_msgs/package.xml new file mode 100755 index 0000000..56c7814 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/package.xml @@ -0,0 +1,36 @@ + + + gazebo_msgs + 2.5.15 + + Message and service data structures for interacting with Gazebo from ROS. + + + Jose Luis Rivero + + BSD + + http://gazebosim.org/tutorials?cat=connect_ros + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs + + John Hsu + + catkin + + geometry_msgs + sensor_msgs + trajectory_msgs + std_msgs + std_srvs + message_generation + + geometry_msgs + sensor_msgs + trajectory_msgs + std_msgs + message_runtime + std_srvs + + + diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/ApplyBodyWrench.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/ApplyBodyWrench.srv new file mode 100755 index 0000000..ca27ab5 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/ApplyBodyWrench.srv @@ -0,0 +1,22 @@ +# Apply Wrench to Gazebo Body. +# via the callback mechanism +# all Gazebo operations are made in world frame +string body_name # Gazebo body to apply wrench (linear force and torque) + # wrench is applied in the gazebo world by default + # body names are prefixed by model name, e.g. pr2::base_link +string reference_frame # wrench is defined in the reference frame of this entity + # use inertial frame if left empty + # frame names are bodies prefixed by model name, e.g. pr2::base_link +geometry_msgs/Point reference_point # wrench is defined at this location in the reference frame +geometry_msgs/Wrench wrench # wrench applied to the origin of the body +time start_time # (optional) wrench application start time (seconds) + # if start_time is not specified, or + # start_time < current time, start as soon as possible +duration duration # optional duration of wrench application time (seconds) + # if duration < 0, apply wrench continuously without end + # if duration = 0, do nothing + # if duration < step size, apply wrench + # for one step size +--- +bool success # return true if set wrench successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/ApplyJointEffort.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/ApplyJointEffort.srv new file mode 100755 index 0000000..5ba9523 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/ApplyJointEffort.srv @@ -0,0 +1,13 @@ +# set urdf joint effort +string joint_name # joint to apply wrench (linear force and torque) +float64 effort # effort to apply +time start_time # optional wrench application start time (seconds) + # if start_time < current time, start as soon as possible +duration duration # optional duration of wrench application time (seconds) + # if duration < 0, apply wrench continuously without end + # if duration = 0, do nothing + # if duration < step size, assume step size and + # display warning in status_message +--- +bool success # return true if effort application is successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/BodyRequest.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/BodyRequest.srv new file mode 100755 index 0000000..d219112 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/BodyRequest.srv @@ -0,0 +1,2 @@ +string body_name # name of the body requested. body names are prefixed by model name, e.g. pr2::base_link +--- diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/DeleteLight.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/DeleteLight.srv new file mode 100755 index 0000000..0743f76 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/DeleteLight.srv @@ -0,0 +1,4 @@ +string light_name # name of the light to be deleted +--- +bool success # return true if deletion is successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/DeleteModel.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/DeleteModel.srv new file mode 100755 index 0000000..fa895aa --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/DeleteModel.srv @@ -0,0 +1,4 @@ +string model_name # name of the Gazebo Model to be deleted +--- +bool success # return true if deletion is successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/GetJointProperties.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/GetJointProperties.srv new file mode 100755 index 0000000..dd207bd --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/GetJointProperties.srv @@ -0,0 +1,18 @@ +string joint_name # name of joint +--- +# joint type +uint8 type +uint8 REVOLUTE = 0 # single DOF +uint8 CONTINUOUS = 1 # single DOF (revolute w/o joints) +uint8 PRISMATIC = 2 # single DOF +uint8 FIXED = 3 # 0 DOF +uint8 BALL = 4 # 3 DOF +uint8 UNIVERSAL = 5 # 2 DOF +# dynamics properties +float64[] damping +# joint state +float64[] position +float64[] rate +# service return status +bool success # return true if get successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/GetLightProperties.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/GetLightProperties.srv new file mode 100755 index 0000000..bd85949 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/GetLightProperties.srv @@ -0,0 +1,8 @@ +string light_name # name of Gazebo Light +--- +std_msgs/ColorRGBA diffuse # diffuse color as red, green, blue, alpha +float64 attenuation_constant +float64 attenuation_linear +float64 attenuation_quadratic +bool success # return true if get successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/GetLinkProperties.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/GetLinkProperties.srv new file mode 100755 index 0000000..ad83b54 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/GetLinkProperties.srv @@ -0,0 +1,16 @@ +string link_name # name of link + # link names are prefixed by model name, e.g. pr2::base_link +--- +geometry_msgs/Pose com # center of mass location in link frame + # and orientation of the moment of inertias + # relative to the link frame +bool gravity_mode # set gravity mode on/off +float64 mass # linear mass of link +float64 ixx # moment of inertia +float64 ixy # moment of inertia +float64 ixz # moment of inertia +float64 iyy # moment of inertia +float64 iyz # moment of inertia +float64 izz # moment of inertia +bool success # return true if get info is successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/GetLinkState.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/GetLinkState.srv new file mode 100755 index 0000000..63bdcf3 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/GetLinkState.srv @@ -0,0 +1,9 @@ +string link_name # name of link + # link names are prefixed by model name, e.g. pr2::base_link +string reference_frame # reference frame of returned information, must be a valid link + # if empty, use inertial (gazebo world) frame + # reference_frame names are prefixed by model name, e.g. pr2::base_link +--- +gazebo_msgs/LinkState link_state +bool success # return true if get info is successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/GetModelProperties.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/GetModelProperties.srv new file mode 100755 index 0000000..c83783d --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/GetModelProperties.srv @@ -0,0 +1,11 @@ +string model_name # name of Gazebo Model +--- +string parent_model_name # parent model +string canonical_body_name # name of canonical body, body names are prefixed by model name, e.g. pr2::base_link +string[] body_names # list of bodies, body names are prefixed by model name, e.g. pr2::base_link +string[] geom_names # list of geoms +string[] joint_names # list of joints attached to the model +string[] child_model_names # list of child models +bool is_static # returns true if model is static +bool success # return true if get successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/GetModelState.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/GetModelState.srv new file mode 100755 index 0000000..9c6b6ee --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/GetModelState.srv @@ -0,0 +1,14 @@ +string model_name # name of Gazebo Model +string relative_entity_name # return pose and twist relative to this entity + # an entity can be a model, body, or geom + # be sure to use gazebo scoped naming notation (e.g. [model_name::body_name]) + # leave empty or "world" will use inertial world frame +--- +Header header # Standard metadata for higher-level stamped data types. + # * header.seq holds the number of requests since the plugin started + # * header.stamp timestamp related to the pose + # * header.frame_id not used but currently filled with the relative_entity_name +geometry_msgs/Pose pose # pose of model in relative entity frame +geometry_msgs/Twist twist # twist of model in relative entity frame +bool success # return true if get successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/GetPhysicsProperties.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/GetPhysicsProperties.srv new file mode 100755 index 0000000..4c276fa --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/GetPhysicsProperties.srv @@ -0,0 +1,9 @@ +--- +# sets pose and twist of a link. All children link poses/twists of the URDF tree will be updated accordingly +float64 time_step # dt in seconds +bool pause # true if physics engine is paused +float64 max_update_rate # throttle maximum physics update rate +geometry_msgs/Vector3 gravity # gravity vector (e.g. earth ~[0,0,-9.81]) +gazebo_msgs/ODEPhysics ode_config # contains physics configurations pertaining to ODE +bool success # return true if set wrench successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/GetWorldProperties.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/GetWorldProperties.srv new file mode 100755 index 0000000..422167b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/GetWorldProperties.srv @@ -0,0 +1,6 @@ +--- +float64 sim_time # current sim time +string[] model_names # list of models in the world +bool rendering_enabled # if X is used +bool success # return true if get successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/JointRequest.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/JointRequest.srv new file mode 100755 index 0000000..448941c --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/JointRequest.srv @@ -0,0 +1,3 @@ +string joint_name # name of the joint requested +--- + diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/SetJointProperties.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/SetJointProperties.srv new file mode 100755 index 0000000..e2781ed --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/SetJointProperties.srv @@ -0,0 +1,5 @@ +string joint_name # name of joint +gazebo_msgs/ODEJointProperties ode_joint_config # access to ODE joint dynamics properties +--- +bool success # return true if get successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/SetJointTrajectory.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/SetJointTrajectory.srv new file mode 100755 index 0000000..62169e6 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/SetJointTrajectory.srv @@ -0,0 +1,8 @@ +string model_name +trajectory_msgs/JointTrajectory joint_trajectory +geometry_msgs/Pose model_pose +bool set_model_pose +bool disable_physics_updates # defaults to false +--- +bool success # return true if set wrench successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/SetLightProperties.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/SetLightProperties.srv new file mode 100755 index 0000000..5e33866 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/SetLightProperties.srv @@ -0,0 +1,8 @@ +string light_name # name of Gazebo Light +std_msgs/ColorRGBA diffuse # diffuse color as red, green, blue, alpha +float64 attenuation_constant +float64 attenuation_linear +float64 attenuation_quadratic +--- +bool success # return true if get successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/SetLinkProperties.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/SetLinkProperties.srv new file mode 100755 index 0000000..65e8dd3 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/SetLinkProperties.srv @@ -0,0 +1,16 @@ +string link_name # name of link + # link names are prefixed by model name, e.g. pr2::base_link +geometry_msgs/Pose com # center of mass location in link frame + # and orientation of the moment of inertias + # relative to the link frame +bool gravity_mode # set gravity mode on/off +float64 mass # linear mass of link +float64 ixx # moment of inertia +float64 ixy # moment of inertia +float64 ixz # moment of inertia +float64 iyy # moment of inertia +float64 iyz # moment of inertia +float64 izz # moment of inertia +--- +bool success # return true if get info is successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/SetLinkState.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/SetLinkState.srv new file mode 100755 index 0000000..6530a15 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/SetLinkState.srv @@ -0,0 +1,4 @@ +gazebo_msgs/LinkState link_state +--- +bool success # return true if set wrench successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/SetModelConfiguration.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/SetModelConfiguration.srv new file mode 100755 index 0000000..e65a928 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/SetModelConfiguration.srv @@ -0,0 +1,9 @@ +# Set Gazebo Model pose and twist +string model_name # model to set state (pose and twist) +string urdf_param_name # parameter name that contains the urdf XML. + +string[] joint_names # list of joints to set positions. if joint is not listed here, preserve current position. +float64[] joint_positions # set to this position. +--- +bool success # return true if setting state successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/SetModelState.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/SetModelState.srv new file mode 100755 index 0000000..2095e2a --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/SetModelState.srv @@ -0,0 +1,4 @@ +gazebo_msgs/ModelState model_state +--- +bool success # return true if setting state successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/SetPhysicsProperties.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/SetPhysicsProperties.srv new file mode 100755 index 0000000..3ac6524 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/SetPhysicsProperties.srv @@ -0,0 +1,8 @@ +# sets pose and twist of a link. All children link poses/twists of the URDF tree will be updated accordingly +float64 time_step # dt in seconds +float64 max_update_rate # throttle maximum physics update rate +geometry_msgs/Vector3 gravity # gravity vector (e.g. earth ~[0,0,-9.81]) +gazebo_msgs/ODEPhysics ode_config # configurations for ODE +--- +bool success # return true if set wrench successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_msgs/srv/SpawnModel.srv b/gazebo_ros_pkgs/gazebo_msgs/srv/SpawnModel.srv new file mode 100755 index 0000000..7dadd64 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_msgs/srv/SpawnModel.srv @@ -0,0 +1,11 @@ +string model_name # name of the model to be spawn +string model_xml # this should be an urdf or gazebo xml +string robot_namespace # spawn robot and all ROS interfaces under this namespace +geometry_msgs/Pose initial_pose # only applied to canonical body +string reference_frame # initial_pose is defined relative to the frame of this model/body + # if left empty or "world", then gazebo world frame is used + # if non-existent model/body is specified, an error is returned + # and the model is not spawned +--- +bool success # return true if spawn successful +string status_message # comments if available diff --git a/gazebo_ros_pkgs/gazebo_plugins/CHANGELOG.rst b/gazebo_ros_pkgs/gazebo_plugins/CHANGELOG.rst new file mode 100755 index 0000000..45a5665 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/CHANGELOG.rst @@ -0,0 +1,563 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gazebo_plugins +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.5.15 (2018-02-12) +------------------- +* Merge pull request `#669 `_ from ahcorde/kinetic-devel +* Adding velocity to joint state publisher gazebo plugin +* Fix last gazebo8 warnings! (`#658 `_) +* Fix gazebo8 warnings part 10: ifdefs for GetModel, GetEntity, Light (`#656 `_) +* gazebo8 warnings: ifdefs for Get.*Vel() (`#653 `_) +* Fix gazebo8 warnings part 8: ifdef's for GetWorldPose (`#650 `_) +* Fix gazebo8 warnings part 7: ifdef's for Joint::GetAngle and some cleanup (`#642 `_) +* Contributors: Alejandro Hernández Cordero, Steven Peters + +2.5.14 (2017-12-11) +------------------- +* for gazebo8+, call functions without Get (`#639 `_) +* Fix gazebo8 warnings part 4: convert remaining local variables in plugins to ign-math (`#633 `_) +* Fix gazebo8 warnings part 3: more ign-math in plugins (`#631 `_) +* Fix gazebo8 warnings part 2: replace private member gazebo::math types with ignition (`#628 `_) +* Replace Events::Disconnect* with pointer reset (`#623 `_) +* Merge pull request `#542 `_ from davetcoleman/kinetic-gazebo7-only + Remove compiler directive flags for < GAZEBO 7 +* Contributors: Dave Coleman, Steven Peters + +2.5.13 (2017-06-24) +------------------- +* Fix inverted height in block laser plugin (`#582 `_) +* Allow disabling distorted camera border crop (and associated tests) (`#572 `_) +* Add an IMU sensor plugin that inherits from SensorPlugin (`#363 `_) + * now the plugin works with multiple robots + * using GetParentName name instead of GetScopedName + * added comments to highlight the differents between GazeboRosImuSensor and GazeboRosIMU + * now the message header is properly handled, using bodyName parameter as frame_id + * added check on gazebo version + * added check for sensor null pointer + * changed deprecated functions for gazebo version >= 6 + * fixed version check + * added missing sensor variable for LastUpdateTime() function call + * considering '/' included in the robotNamespace + * replaced "bodyFrame" with "frameName" +* Less exciting console output (`#561 `_) +* Add catkin package(s) to provide the default version of Gazebo (`#571 `_) + * gazebo_dev: added execution dependency gazebo +* Contributors: Adam Allevato, Alessandro Settimi, Dave Coleman, Jose Luis Rivero, Shohei Fujii + +2.5.12 (2017-04-25) +------------------- +* Revert catkin warning fix (`#567 `_) + Many regressions in third party software (see https://github.com/yujinrobot/kobuki_desktop/issues/50) +* Contributors: Jose Luis Rivero + +2.5.11 (2017-04-18) +------------------- +* Change build system to set DEPEND on Gazebo/SDFormat (fix catkin warning) + Added missing DEPEND clauses to catkin_package to fix gazebo catkin warning. + Note that after the change problems could appear related to -lpthreads + errors. This is an known issue related to catkin: + https://github.com/ros/catkin/issues/856 + +* Fix: add gazebo_ros_range to catkin package libraries (`#558 `_) +* Contributors: Christoph Rist, Dave Coleman + +2.5.10 (2017-03-03) +------------------- +* Revert catkin warnings to fix regressions (problems with catkin -lpthreads errors) + For reference and reasons, please check: + https://discourse.ros.org/t/need-to-sync-new-release-of-rqt-topic-indigo-jade-kinetic/1410/4 + * Revert "Fix gazebo catkin warning, cleanup CMakeLists (`#537 `_)" + This reverts commit 5a0305fcb97864b66bc2e587fc0564435b4f2034. + * Revert "Fix gazebo and sdformat catkin warnings" + This reverts commit 11f95d25dcd32faccd2401d45c722f7794c7542c. +* Fix destructor of GazeboRosVideo (`#547 `_) +* Less exciting console output (`#549 `_) +* Fix SDF namespacing for Video Plugin (`#546 `_) +* Contributors: Dave Coleman, Jose Luis Rivero + +2.5.9 (2017-02-20) +------------------ +* Fix gazebo catkin warning, cleanup CMakeLists (`#537 `_) +* Fix timestamp issues for rendering sensors (kinetic-devel) +* Namespace console output (`#543 `_) +* Adding depth camera world to use in test to make depth camera have right timestamp `#408 `_- appears to be working (though only looking at horizon) but getting these sdf errors: +* `#408 `_ Make the multi camera timestamps current rather than outdated, also reuse the same update code +* Fix merge with kinetic branch +* `#408 `_ Making a test for multicamra that shows the timestamps are currently outdated, will fix them similar to how the regular camera was fixed. +* Fix for issue `#408 `_. The last measurement time is the time that gazebo generated the sensor data, so ought to be used. updateRate doesn't seem that useful. + The other cameras need similar fixes to have the proper timestamps. +* Bugfix: duplicated tf prefix resolution +* fill in child_frame_id of odom topic +* Fix gazebo and sdformat catkin warnings +* Contributors: Dave Coleman, Jose Luis Rivero, Kei Okada, Lucas Walter, Yuki Furuta + +2.5.8 (2016-12-06) +------------------ +* Fix camera distortion coefficients order. Now {k1, k2, p1, p2, k3} +* Added an interface to gazebo's harness plugin +* Contributors: Enrique Fernandez, Steven Peters, Nate Koenig + +2.5.7 (2016-06-10) +------------------ + +2.5.6 (2016-04-28) +------------------ +* fix gazebo7 deprecation warnings on kinetic +* Contributors: Steven Peters + +2.5.5 (2016-04-27) +------------------ +* merge indigo, jade to kinetic-devel +* Accept /world for the frameName parameter in gazebo_ros_p3d +* Upgrade to gazebo 7 and remove deprecated driver_base dependency + * Upgrade to gazebo 7 and remove deprecated driver_base dependency + * disable gazebo_ros_control until dependencies are met + * Remove stray backslash +* Update maintainer for Kinetic release +* use HasElement in if condition +* Contributors: Hugo Boyer, Jackie Kay, Jose Luis Rivero, Steven Peters, William Woodall, Yuki Furuta + +2.5.3 (2016-04-11) +------------------ + +2.5.2 (2016-02-25) +------------------ +* Fix row_step of openni_kinect plugin +* remove duplicated code during merge +* merging from indigo-devel +* Merge pull request `#368 `_ from l0g1x/jade-devel + Covariance for published twist in skid steer plugin +* gazebo_ros_utils.h: include gazebo_config.h + Make sure to include gazebo_config.h, + which defines the GAZEBO_MAJOR_VERSION macro +* Fix compiler error with SetHFOV + In gazebo7, the rendering::Camera::SetHFOV function + is overloaded with a potential for ambiguity, + as reported in the following issue: + https://bitbucket.org/osrf/gazebo/issues/1830 + This fixes the build by explicitly defining the + Angle type. +* Add missing boost header + Some boost headers were remove from gazebo7 header files + and gazebo_ros_joint_state_publisher.cpp was using it + implicitly. +* Fix gazebo7 build errors + The SensorPtr types have changed from boost:: pointers + to std:: pointers, + which requires boost::dynamic_pointer_cast to change to + std::dynamic_pointer_cast. + A helper macro is added that adds a `using` statement + corresponding to the correct type of dynamic_pointer_cast. + This macro should be narrowly scoped to protect + other code. +* gazebo_ros_utils.h: include gazebo_config.h + Make sure to include gazebo_config.h, + which defines the GAZEBO_MAJOR_VERSION macro +* Use Joint::SetParam for joint velocity motors + Before gazebo5, Joint::SetVelocity and SetMaxForce + were used to set joint velocity motors. + The API has changed in gazebo5, to use Joint::SetParam + instead. + The functionality is still available through the SetParam API. + cherry-picked from indigo-devel + Add ifdefs to fix build with gazebo2 + It was broken by `#315 `_. + Fixes `#321 `_. +* Fix gazebo6 deprecation warnings + Several RaySensor functions are deprecated in gazebo6 + and are removed in gazebo7. + The return type is changed to use ignition math + and the function name is changed. + This adds ifdef's to handle the changes. +* Fix compiler error with SetHFOV + In gazebo7, the rendering::Camera::SetHFOV function + is overloaded with a potential for ambiguity, + as reported in the following issue: + https://bitbucket.org/osrf/gazebo/issues/1830 + This fixes the build by explicitly defining the + Angle type. +* Add missing boost header + Some boost headers were remove from gazebo7 header files + and gazebo_ros_joint_state_publisher.cpp was using it + implicitly. +* Fix gazebo7 build errors + The SensorPtr types have changed from boost:: pointers + to std:: pointers, + which requires boost::dynamic_pointer_cast to change to + std::dynamic_pointer_cast. + A helper macro is added that adds a `using` statement + corresponding to the correct type of dynamic_pointer_cast. + This macro should be narrowly scoped to protect + other code. +* Fix gazebo6 deprecation warnings + Several RaySensor functions are deprecated in gazebo6 + and are removed in gazebo7. + The return type is changed to use ignition math + and the function name is changed. + This adds ifdef's to handle the changes. +* Publish organized point cloud from openni_kinect plugin +* Added covariance matrix for published twist message in the skid steer plugin, as packages such as robot_localization require an associated non-zero covariance matrix +* Added a missing initialization inside Differential Drive +* 2.4.9 +* Generate changelog +* Merge pull request `#335 `_ from pal-robotics-forks/add_range_sensor_plugin + Adds range plugin for infrared and ultrasound sensors from PAL Robotics +* Import changes from jade-branch +* Add range world and launch file +* Adds range plugin for infrared and ultrasound sensors from PAL Robotics +* Add ifdefs to fix build with gazebo2 + It was broken by `#315 `_. + Fixes `#321 `_. +* Use Joint::SetParam for joint velocity motors + Before gazebo5, Joint::SetVelocity and SetMaxForce + were used to set joint velocity motors. + The API has changed in gazebo5, to use Joint::SetParam + instead. + The functionality is still available through the SetParam API. +* Set GAZEBO_CXX_FLAGS to fix c++11 compilation errors +* Contributors: Bence Magyar, John Hsu, Jose Luis Rivero, Kentaro Wada, Krystian, Mirko Ferrati, Steven Peters, hsu + +2.5.1 (2015-08-16) +------------------ +* Port of Pal Robotics range sensor plugin to Jade +* Added a comment about the need of libgazebo5-dev in runtime +* Added gazebo version check +* Added missing files +* Added elevator plugin +* Use c++11 +* run_depend on libgazebo5-dev (`#323 `_) + Declare the dependency. + It can be fixed later if we don't want it. +* Contributors: Jose Luis Rivero, Nate Koenig, Steven Peters + +* Port of Pal Robotics range sensor plugin to Jade +* Added a comment about the need of libgazebo5-dev in runtime +* Added gazebo version check +* Added missing files +* Added elevator plugin +* Use c++11 +* run_depend on libgazebo5-dev +* Contributors: Jose Luis Rivero, Nate Koenig, Steven Peters + +2.5.0 (2015-04-30) +------------------ +* run_depend on libgazebo5-dev instead of gazebo5 +* Changed the rosdep key for gazebo to gazebo5, for Jade Gazebo5 will be used. +* Contributors: Steven Peters, William Woodall + +2.4.9 (2015-08-16) +------------------ +* Adds range plugin for infrared and ultrasound sensors from PAL Robotics +* Import changes from jade-branch +* Add range world and launch file +* Add ifdefs to fix build with gazebo2 +* Use Joint::SetParam for joint velocity motors +* Set GAZEBO_CXX_FLAGS to fix c++11 compilation errors +* Contributors: Bence Magyar, Jose Luis Rivero, Steven Peters + +2.4.8 (2015-03-17) +------------------ +* fixed mistake at calculation of joint velocity +* [gazebo_ros_diff_drive] force call SetMaxForce since this Joint::Reset in gazebo/physics/Joint.cc reset MaxForce to zero and ModelPlugin::Reset is called after Joint::Reset +* add PointCloudCutoffMax +* Contributors: Kei Okada, Michael Ferguson, Sabrina Heerklotz + +2.4.7 (2014-12-15) +------------------ +* fix missing ogre flags: removed from gazebo default (5.x.x candidate) cmake config +* Fixing handling of non-world frame velocities in setModelState. +* fix missing ogre flags (removed from gazebo cmake config) +* change header to use opencv2/opencv.hpp issue `#274 `_ +* Update Gazebo/ROS tutorial URL +* Merge pull request `#237 `_ from ros-simulation/update_header_license + Update header license for Indigo +* Contributors: John Hsu, Jose Luis Rivero, Robert Codd-Downey, Tom Moore, hsu + +2.4.6 (2014-09-01) +------------------ +* Update gazebo_ros_openni_kinect.cpp +* merging from hydro-devel into indigo-devel +* Merge pull request `#204 `_ from fsuarez6/hydro-devel + gazebo_plugins: Adding ForceTorqueSensor Plugin +* Updated to Apache 2.0 license +* Merge pull request `#180 `_ from vrabaud/indigo-devel + remove PCL dependency +* merging +* check deprecation of gazebo::Joint::SetAngle by SetPosition +* compatibility with gazebo 4.x +* Update changelogs for the upcoming release +* Fix build with gazebo4 and indigo +* Added Gaussian Noise generator +* publish organized pointcloud from openni plugin +* Changed measurement direction to "parent to child" +* gazebo_plugin: Added updateRate parameter to the gazebo_ros_imu plugin +* gazebo_plugins: Adding ForceTorqueSensor Plugin +* remove PCL dependency +* ros_camera_utils: Adding CameraInfoManager to satisfy full ROS camera API (relies on https://github.com/ros-perception/image_common/pull/20 ) + ros_camera_utils: Adding CameraInfoManager to satisfy full ROS camera API (relies on https://github.com/ros-perception/image_common/pull/20 ) +* Contributors: John Hsu, Jonathan Bohren, Jose Luis Rivero, Nate Koenig, Ryohei Ueda, Vincent Rabaud, fsuarez6, gborque, John Binney + +2.4.5 (2014-08-18) +------------------ +* Replace SetAngle with SetPosition for gazebo 4 and up +* Port fix_build branch for indigo-devel + See pull request `#221 `_ +* Contributors: Jose Luis Rivero, Steven Peters + +2.4.4 (2014-07-18) +------------------ +* Merge branch 'hydro-devel' into indigo-devel +* gazebo_ros_diff_drive gazebo_ros_tricycle_drive encoderSource option names updated +* gazebo_ros_diff_drive is now able to use the wheels rotation of the optometry or the gazebo ground truth based on the 'odometrySource' parameter +* simple linear controller for the tricycle_drive added +* second robot for testing in tricycle_drive_scenario.launch added +* Merge remote-tracking branch 'upstream/hydro-devel' into hydro-devel +* BDS licenses header fixed and tricycle drive plugin added +* format patch of hsu applied +* Updated package.xml +* Fix repo names in package.xml's +* ros diff drive supports now an acceleration limit +* Pioneer model: Diff_drive torque reduced +* GPU Laser test example added +* fixed gpu_laser to work with workspaces +* hand_of_god: Adding hand-of-god plugin + ros_force: Fixing error messages to refer to the right plugin +* Remove unneeded dependency on pcl_ros +* minor fixes on relative paths in xacro for pioneer robot +* gazebo test model pionneer 3dx updated with xacro path variables +* pioneer model update for the multi_robot_scenario +* Merge remote-tracking branch 'upstream/hydro-devel' into hydro-devel +* fixed camera to work with workspaces +* fixed links related to changed name +* diff drive name changed to multi robot scenario +* working camera added +* Merge remote-tracking branch 'upstream/hydro-devel' into hydro-devel +* fix in pioneer xacro model for diff_drive +* Laser colour in rviz changed +* A test model for the ros_diff_drive ros_laser and joint_state_publisher added +* the ros_laser checkes now for the model name and adds it als prefix +* joint velocity fixed using radius instead of diameter +* ROS_INFO on laser plugin added to see if it starts +* fetched with upstream +* gazebo_ros_diff_drive was enhanced to publish the wheels tf or the wheels joint state depending on two additinal xml options +* Gazebo ROS joint state publisher added +* Contributors: Dave Coleman, John Hsu, Jon Binney, Jonathan Bohren, Markus Bader, Steven Peters + +2.4.3 (2014-05-12) +------------------ +* gazebo_plugins: add run-time dependency on gazebo_ros +* Merge pull request `#176 `_ from ros-simulation/issue_175 + Fix `#175 `_: dynamic reconfigure dependency error +* Remove unneeded dependency on pcl_ros +* Fix `#175 `_: dynamic reconfigure dependency error +* Contributors: Steven Peters + +2.4.2 (2014-03-27) +------------------ +* merging from hydro-devel +* bump patch version for indigo-devel to 2.4.1 +* merging from indigo-devel after 2.3.4 release +* "2.4.0" +* catkin_generate_changelog +* Contributors: John Hsu + +2.4.1 (2013-11-13) +------------------ + +2.3.5 (2014-03-26) +------------------ +* update test world for block laser +* this corrects the right orientation of the laser scan and improves on comparison between 2 double numbers +* Initialize ``depth_image_connect_count_`` in openni_kinect plugin +* multicamera bad namespace. Fixes `#161 `_ + There was a race condition between GazeboRosCameraUtils::LoadThread + creating the ros::NodeHandle and GazeboRosCameraUtils::Load + suffixing the camera name in the namespace +* Use function for accessing scene node in gazebo_ros_video +* readded the trailing whitespace for cleaner diff +* the parent sensor in gazebo seems not to be active +* Contributors: Dejan Pangercic, Ian Chen, John Hsu, Jordi Pages, Toni Oliver, Ugo Cupcic + +2.3.4 (2013-11-13) +------------------ +* rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. +* Simplify ``gazebo_plugins/CMakeLists.txt`` + Replace ``cxx_flags`` and ``ld_flags`` variables with simpler cmake macros + and eliminate unnecessary references to ``SDFormat_LIBRARIES``, since + they are already part of ``GAZEBO_LIBRARIES``. +* Put some cmake lists on multiple lines to improve readability. +* Add dependencies on dynamic reconfigure files + Occasionally the build can fail due to some targets having an + undeclared dependency on automatically generated dynamic + reconfigure files (GazeboRosCameraConfig.h for example). This + commit declares several of those dependencies. + +2.4.0 (2013-10-14) +------------------ + +2.3.3 (2013-10-10) +------------------ +* gazebo_plugins: use shared pointers for variables shared among cameras + It is not allowed to construct a shared_ptr from a pointer to a member + variable. +* gazebo_plugins: moved initialization of shared_ptr members of + GazeboRosCameraUtils to `GazeboRosCameraUtils::Load()` + This fixes segfaults in gazebo_ros_depth_camera and + gazebo_ros_openni_kinect as the pointers have not been initialized + there. +* Use `RenderingIFace.hh` + +2.3.2 (2013-09-19) +------------------ +* Make gazebo includes use full path + In the next release of gazebo, it will be required to use the + full path for include files. For example, + `include ` will not be valid + `include ` must be done instead. +* Merge branch 'hydro-devel' of `gazebo_ros_pkgs `_ into synchronize_with_drcsim_plugins +* change includes to use brackets in headers for export +* per pull request comments +* Changed resolution for searchParam. +* Don't forget to delete the node! +* Removed info message on robot namespace. +* Retreive the tf prefix from the robot node. +* synchronize with drcsim plugins + +2.3.1 (2013-08-27) +------------------ +* Remove direct dependency on pcl, rely on the transitive dependency from pcl_ros +* Cleaned up template, fixes for header files + +2.3.0 (2013-08-12) +------------------ +* enable image generation when pointcloud is requested, as the generated image is used by the pointcloud +* gazebo_plugins: replace deprecated boost function + This is related to this `gazebo issue #581 `_ +* gazebo_plugins: fix linkedit issues + Note: other linkedit errors were fixed upstream + in gazebo +* gazebo_ros_openni_kinect plugin: adds publishing of the camera info + again (fixes `#95 `_) +* Merge pull request `#90 `_ from piyushk/add_model_controller + added a simple model controller plugin that uses a twist message +* renamed plugin from model controller to planar move +* prevents dynamic_reconfigure from overwritting update rate param on start-up +* removed anonymizer from include guard +* fixed odometry publication for model controller plugin +* added a simple model controller plugin that uses a twist message to control models + +2.2.1 (2013-07-29) +------------------ +* Added prosilica plugin to install TARGETS + +2.2.0 (2013-07-29) +------------------ +* Switched to pcl_conversions instead of using compiler flags for Hydro/Groovy PCL support +* fixed node intialization conflict between gzserver and gzclient. better adherance to gazebo style guidelines +* Fixed template +* removed ros initialization from plugins +* Standardized the way ROS nodes are initialized in gazebo plugins +* Remove find_package(SDF) from CMakeLists.txt + It is sufficient to find gazebo, which will export the information about the SDFormat package. +* ROS Video Plugin for Gazebo - allows displaying an image stream in an OGRE texture inside gazebo. Also provides a fix for `#85 `_. +* patch a fix for prosilica plugin (startup race condition where `rosnode_` might still be NULL). +* Added explanation of new dependency in gazebo_ros_pkgs +* switch Prosilica camera from type depth to regular camera (as depth data were not used). +* migrating prosilica plugin from pr2_gazebo_plugins +* Removed tbb because it was a temporary dependency for a Gazebo bug +* SDF.hh --> sdf.hh +* Added PCL to package.xml + +2.1.5 (2013-07-18) +------------------ +* Include instead of + The sdformat package recently changed the name of an sdf header + file from SDF.hh to SDFImpl.hh; this change will use the lower-case + header file which should work with old and new versions of sdformat + or gazebo. + +2.1.4 (2013-07-14) +------------------ + +2.1.3 (2013-07-13) +------------------ +* temporarily add tbb as a work around for `#74 `_ + +2.1.2 (2013-07-12) +------------------ +* Fixed compatibility with new PCL 1.7.0 +* Tweak to make SDFConfig.cmake +* Re-enabled dynamic reconfigure for camera utils - had been removed for Atlas +* Cleaned up CMakeLists.txt for all gazebo_ros_pkgs +* Removed SVN references +* 2.1.1 + +2.1.1 (2013-07-10 19:11) +------------------------ +* Small deprecated warning +* Fixed errors and deprecation warnings from Gazebo 1.9 and the sdformat split +* Source code formatting. +* Merge pull request `#59 `_ from ros-simulation/CMake_Tweak + Added dependency to prevent missing msg header, cleaned up CMakeLists +* export diff drive and skid steer for other catkin packages +* install diff_drive and skid_steer plugins +* Added dependency to prevent missing msg header, cleaned up CMakeLists +* Added ability to switch off publishing TF. + +2.1.0 (2013-06-27) +------------------ +* gazebo_plugins: always use gazebo/ path prefix in include directives +* gazebo_plugins: call Advertise() directly after initialization has + completed in gazebo_ros_openni_kinect and gazebo_ros_depth_camera + plugins, as the sensor will never be activated otherwise +* Merge pull request `#41 `_ from ZdenekM/hydro-devel + Added skid steering plugin (modified diff drive plugin). +* Merge pull request `#35 `_ from meyerj/fix_include_directory_installation_target + Header files of packages gazebo_ros and gazebo_plugins are installed to the wrong location +* Rotation fixed. +* Skid steering drive plugin. +* gazebo_plugins: added missing initialization of `GazeboRosDepthCamera::advertised_` +* gazebo_plugins: fixed depth and openni kinect camera plugin segfaults +* gazebo_plugins: terminate the service thread properly on destruction of a PubMutliQueue object without shuting down ros +* gazebo_plugins/gazebo_ros: fixed install directories for include files and gazebo scripts +* fix for terminating the `service_thread_` in PubQueue.h +* added differential drive plugin to gazebo plugins + +2.0.2 (2013-06-20) +------------------ +* Added Gazebo dependency + +2.0.1 (2013-06-19) +------------------ +* Incremented version to 2.0.1 +* Fixed circular dependency, removed deprecated pkgs since its a stand alone pkg +* Check camera util is initialized before publishing - fix from Atlas + +2.0.0 (2013-06-18) +------------------ +* Changed version to 2.0.0 based on gazebo_simulator being 1.0.0 +* Updated package.xml files for ros.org documentation purposes +* Combined updateSDFModelPose and updateSDFName, added ability to spawn SDFs from model database, updates SDF version to lastest in parts of code, updated the tests +* Created tests for various spawning methods +* Added debug info to shutdown +* Fixed gazebo includes to be in format +* Cleaned up file, addded debug info +* Merge branch 'groovy-devel' into plugin_updates +* Merged changes from Atlas ROS plugins, cleaned up headers +* Merged changes from Atlas ROS plugins, cleaned up headers +* fix curved laser issue +* Combining Atlas code with old gazebo_plugins +* Combining Atlas code with old gazebo_plugins +* Small fixes per ffurrer's code review +* Added the robot namespace to the tf prefix. + The tf_prefix param is published under the robot namespace and not the + robotnamespace/camera node which makes it non-local we have to use the + robot namespace to get it otherwise it is empty. +* findreplace ConnectWorldUpdateStart ConnectWorldUpdateBegin +* Fixed deprecated function calls in gazebo_plugins +* Deprecated warnings fixes +* Removed the two plugin tests that are deprecated +* Removed abandoned plugin tests +* All packages building in Groovy/Catkin +* Imported from bitbucket.org diff --git a/gazebo_ros_pkgs/gazebo_plugins/CMakeLists.txt b/gazebo_ros_pkgs/gazebo_plugins/CMakeLists.txt new file mode 100755 index 0000000..5ed3b31 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/CMakeLists.txt @@ -0,0 +1,415 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gazebo_plugins) + +option(ENABLE_DISPLAY_TESTS "Enable the building of tests that requires a display" OFF) + +find_package(catkin REQUIRED COMPONENTS + gazebo_dev + message_generation + gazebo_msgs + roscpp + rospy + nodelet + angles + std_srvs + geometry_msgs + sensor_msgs + nav_msgs + urdf + tf + tf2_ros + dynamic_reconfigure + rosgraph_msgs + trajectory_msgs + image_transport + rosconsole + cv_bridge + polled_camera + diagnostic_updater + camera_info_manager + std_msgs +) + +include (FindPkgConfig) +if (PKG_CONFIG_FOUND) + pkg_check_modules(XML libxml-2.0) + pkg_check_modules(OGRE OGRE) + pkg_check_modules(OGRE-Terrain OGRE-Terrain) + pkg_check_modules(OGRE-Paging OGRE-Paging) +else() + message(FATAL_ERROR "pkg-config is required; please install it") +endif() + +find_package(Boost REQUIRED COMPONENTS thread) + +execute_process(COMMAND + pkg-config --variable=plugindir OGRE + OUTPUT_VARIABLE OGRE_PLUGIN_PATH + OUTPUT_STRIP_TRAILING_WHITESPACE +) + +catkin_python_setup() + +generate_dynamic_reconfigure_options( + cfg/CameraSynchronizer.cfg + cfg/GazeboRosCamera.cfg + cfg/GazeboRosOpenniKinect.cfg + cfg/Hokuyo.cfg +) + +include_directories(include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${OGRE_INCLUDE_DIRS} + ${OGRE-Terrain_INCLUDE_DIRS} + ${OGRE-Paging_INCLUDE_DIRS} +) + +link_directories( + ${catkin_LIBRARY_DIRS} + ${OGRE_LIBRARY_DIRS} + ${OGRE-Terrain_LIBRARY_DIRS} + ${OGRE-Paging_LIBRARY_DIRS} +) + +if (NOT GAZEBO_VERSION VERSION_LESS 6.0) + catkin_package( INCLUDE_DIRS include LIBRARIES gazebo_ros_elevator) +endif() + +if (NOT GAZEBO_VERSION VERSION_LESS 7.3) + catkin_package(INCLUDE_DIRS include LIBRARIES gazebo_ros_harness) +endif() + +catkin_package( + INCLUDE_DIRS include + LIBRARIES + vision_reconfigure + gazebo_ros_utils + gazebo_ros_camera_utils + gazebo_ros_camera + gazebo_ros_triggered_camera + gazebo_ros_multicamera + gazebo_ros_triggered_multicamera + gazebo_ros_depth_camera + gazebo_ros_openni_kinect + gazebo_ros_gpu_laser + gazebo_ros_laser + gazebo_ros_block_laser + gazebo_ros_p3d + gazebo_ros_imu + gazebo_ros_imu_sensor + gazebo_ros_f3d + gazebo_ros_ft_sensor + gazebo_ros_bumper + gazebo_ros_template + gazebo_ros_projector + gazebo_ros_prosilica + gazebo_ros_force + gazebo_ros_joint_trajectory + gazebo_ros_joint_state_publisher + gazebo_ros_joint_pose_trajectory + gazebo_ros_diff_drive + gazebo_ros_tricycle_drive + gazebo_ros_skid_steer_drive + gazebo_ros_video + gazebo_ros_planar_move + gazebo_ros_range + gazebo_ros_vacuum_gripper + + CATKIN_DEPENDS + message_runtime + gazebo_msgs + roscpp + rospy + nodelet + angles + std_srvs + geometry_msgs + sensor_msgs + nav_msgs + urdf + tf + tf2_ros + dynamic_reconfigure + rosgraph_msgs + trajectory_msgs + image_transport + rosconsole + camera_info_manager + std_msgs +) +add_dependencies(${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS}) + +## Executables +add_executable(hokuyo_node src/hokuyo_node.cpp) +add_dependencies(hokuyo_node ${PROJECT_NAME}_gencfg) +target_link_libraries(hokuyo_node + ${catkin_LIBRARIES} +) + +add_library(gazebo_ros_utils src/gazebo_ros_utils.cpp) +target_link_libraries(gazebo_ros_utils ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(vision_reconfigure src/vision_reconfigure.cpp) +add_dependencies(vision_reconfigure ${PROJECT_NAME}_gencfg) +target_link_libraries(vision_reconfigure ${catkin_LIBRARIES}) + +add_executable(camera_synchronizer src/camera_synchronizer.cpp) +add_dependencies(camera_synchronizer ${PROJECT_NAME}_gencfg) +target_link_libraries(camera_synchronizer vision_reconfigure ${catkin_LIBRARIES}) + +add_executable(pub_joint_trajectory_test test/pub_joint_trajectory_test.cpp) +target_link_libraries(pub_joint_trajectory_test ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +add_dependencies(pub_joint_trajectory_test ${catkin_EXPORTED_TARGETS}) # don't build until gazebo_msgs is done + +add_definitions(-fPIC) # what is this for? + +## Plugins +add_library(gazebo_ros_camera_utils src/gazebo_ros_camera_utils.cpp) +add_dependencies(gazebo_ros_camera_utils ${PROJECT_NAME}_gencfg) +target_link_libraries(gazebo_ros_camera_utils ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(MultiCameraPlugin src/MultiCameraPlugin.cpp) +target_link_libraries(MultiCameraPlugin ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_camera src/gazebo_ros_camera.cpp) +add_dependencies(gazebo_ros_camera ${PROJECT_NAME}_gencfg) +target_link_libraries(gazebo_ros_camera gazebo_ros_camera_utils CameraPlugin ${catkin_LIBRARIES}) + +add_library(gazebo_ros_triggered_camera src/gazebo_ros_triggered_camera.cpp) +add_dependencies(gazebo_ros_triggered_camera ${PROJECT_NAME}_gencfg) +target_link_libraries(gazebo_ros_triggered_camera gazebo_ros_camera_utils ${GAZEBO_LIBRARIES} CameraPlugin ${catkin_LIBRARIES}) + + +if (NOT GAZEBO_VERSION VERSION_LESS 6.0) + add_library(gazebo_ros_elevator src/gazebo_ros_elevator.cpp) + add_dependencies(gazebo_ros_elevator ${PROJECT_NAME}_gencfg) + target_link_libraries(gazebo_ros_elevator ElevatorPlugin ${catkin_LIBRARIES}) +endif() + +add_library(gazebo_ros_multicamera src/gazebo_ros_multicamera.cpp) +add_dependencies(gazebo_ros_multicamera ${PROJECT_NAME}_gencfg) +target_link_libraries(gazebo_ros_multicamera gazebo_ros_camera_utils MultiCameraPlugin ${catkin_LIBRARIES}) + +add_library(gazebo_ros_triggered_multicamera src/gazebo_ros_triggered_multicamera.cpp) +add_dependencies(gazebo_ros_triggered_multicamera ${PROJECT_NAME}_gencfg) +target_link_libraries(gazebo_ros_triggered_multicamera gazebo_ros_camera_utils gazebo_ros_triggered_camera ${GAZEBO_LIBRARIES} MultiCameraPlugin ${catkin_LIBRARIES}) + +add_library(gazebo_ros_depth_camera src/gazebo_ros_depth_camera.cpp) +add_dependencies(gazebo_ros_depth_camera ${PROJECT_NAME}_gencfg) +target_link_libraries(gazebo_ros_depth_camera gazebo_ros_camera_utils DepthCameraPlugin ${catkin_LIBRARIES}) + +add_library(gazebo_ros_openni_kinect src/gazebo_ros_openni_kinect.cpp) +add_dependencies(gazebo_ros_openni_kinect ${PROJECT_NAME}_gencfg) +target_link_libraries(gazebo_ros_openni_kinect gazebo_ros_camera_utils DepthCameraPlugin ${catkin_LIBRARIES}) + +add_library(gazebo_ros_gpu_laser src/gazebo_ros_gpu_laser.cpp) +target_link_libraries(gazebo_ros_gpu_laser ${catkin_LIBRARIES} GpuRayPlugin) + +if (NOT GAZEBO_VERSION VERSION_LESS 7.3) + add_library(gazebo_ros_harness src/gazebo_ros_harness.cpp) + add_dependencies(gazebo_ros_harness ${catkin_EXPORTED_TARGETS}) + target_link_libraries(gazebo_ros_harness + ${Boost_LIBRARIES} HarnessPlugin ${catkin_LIBRARIES}) +endif() + +add_library(gazebo_ros_laser src/gazebo_ros_laser.cpp) +target_link_libraries(gazebo_ros_laser RayPlugin ${catkin_LIBRARIES}) + +add_library(gazebo_ros_block_laser src/gazebo_ros_block_laser.cpp) +target_link_libraries(gazebo_ros_block_laser RayPlugin ${catkin_LIBRARIES}) + +add_library(gazebo_ros_p3d src/gazebo_ros_p3d.cpp) +target_link_libraries(gazebo_ros_p3d ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_imu src/gazebo_ros_imu.cpp) +target_link_libraries(gazebo_ros_imu ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_imu_sensor src/gazebo_ros_imu_sensor.cpp) +target_link_libraries(gazebo_ros_imu_sensor ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_f3d src/gazebo_ros_f3d.cpp) +target_link_libraries(gazebo_ros_f3d ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_bumper src/gazebo_ros_bumper.cpp) +add_dependencies(gazebo_ros_bumper ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_ros_bumper ${Boost_LIBRARIES} ContactPlugin ${catkin_LIBRARIES}) + +add_library(gazebo_ros_projector src/gazebo_ros_projector.cpp) +target_link_libraries(gazebo_ros_projector ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + +add_library(gazebo_ros_prosilica src/gazebo_ros_prosilica.cpp) +add_dependencies(gazebo_ros_prosilica ${PROJECT_NAME}_gencfg) +target_link_libraries(gazebo_ros_prosilica gazebo_ros_camera_utils CameraPlugin ${catkin_LIBRARIES}) + +add_library(gazebo_ros_force src/gazebo_ros_force.cpp) +target_link_libraries(gazebo_ros_force ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_joint_trajectory src/gazebo_ros_joint_trajectory.cpp) +add_dependencies(gazebo_ros_joint_trajectory ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_ros_joint_trajectory ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + + +add_library(gazebo_ros_joint_state_publisher src/gazebo_ros_joint_state_publisher.cpp) +set_target_properties(gazebo_ros_joint_state_publisher PROPERTIES LINK_FLAGS "${ld_flags}") +set_target_properties(gazebo_ros_joint_state_publisher PROPERTIES COMPILE_FLAGS "${cxx_flags}") +add_dependencies(gazebo_ros_joint_state_publisher ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_ros_joint_state_publisher ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_joint_pose_trajectory src/gazebo_ros_joint_pose_trajectory.cpp) +add_dependencies(gazebo_ros_joint_pose_trajectory ${catkin_EXPORTED_TARGETS}) +target_link_libraries(gazebo_ros_joint_pose_trajectory ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_diff_drive src/gazebo_ros_diff_drive.cpp) +target_link_libraries(gazebo_ros_diff_drive gazebo_ros_utils ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_tricycle_drive src/gazebo_ros_tricycle_drive.cpp) +target_link_libraries(gazebo_ros_tricycle_drive gazebo_ros_utils ${Boost_LIBRARIES} ${catkin_LIBRARIES}) + +add_library(gazebo_ros_skid_steer_drive src/gazebo_ros_skid_steer_drive.cpp) +target_link_libraries(gazebo_ros_skid_steer_drive ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_video src/gazebo_ros_video.cpp) +target_link_libraries(gazebo_ros_video ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OGRE_LIBRARIES}) + +add_library(gazebo_ros_planar_move src/gazebo_ros_planar_move.cpp) +target_link_libraries(gazebo_ros_planar_move ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_hand_of_god src/gazebo_ros_hand_of_god.cpp) +set_target_properties(gazebo_ros_hand_of_god PROPERTIES LINK_FLAGS "${ld_flags}") +set_target_properties(gazebo_ros_hand_of_god PROPERTIES COMPILE_FLAGS "${cxx_flags}") +target_link_libraries(gazebo_ros_hand_of_god ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_ft_sensor src/gazebo_ros_ft_sensor.cpp) +target_link_libraries(gazebo_ros_ft_sensor ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +add_library(gazebo_ros_range src/gazebo_ros_range.cpp) +target_link_libraries(gazebo_ros_range ${catkin_LIBRARIES} ${Boost_LIBRARIES} RayPlugin) + +add_library(gazebo_ros_vacuum_gripper src/gazebo_ros_vacuum_gripper.cpp) +target_link_libraries(gazebo_ros_vacuum_gripper ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +## +## Add your new plugin here +## + +## Template +add_library(gazebo_ros_template src/gazebo_ros_template.cpp) +target_link_libraries(gazebo_ros_template ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +install(TARGETS + hokuyo_node + vision_reconfigure + camera_synchronizer + gazebo_ros_utils + gazebo_ros_camera_utils + gazebo_ros_camera + gazebo_ros_multicamera + MultiCameraPlugin + gazebo_ros_depth_camera + gazebo_ros_openni_kinect + gazebo_ros_openni_kinect + gazebo_ros_laser + gazebo_ros_block_laser + gazebo_ros_p3d + gazebo_ros_imu + gazebo_ros_imu_sensor + gazebo_ros_f3d + gazebo_ros_ft_sensor + gazebo_ros_bumper + gazebo_ros_hand_of_god + gazebo_ros_template + gazebo_ros_projector + gazebo_ros_prosilica + gazebo_ros_force + gazebo_ros_joint_trajectory + gazebo_ros_joint_state_publisher + gazebo_ros_joint_pose_trajectory + gazebo_ros_diff_drive + gazebo_ros_tricycle_drive + gazebo_ros_skid_steer_drive + gazebo_ros_video + gazebo_ros_planar_move + gazebo_ros_vacuum_gripper + pub_joint_trajectory_test + gazebo_ros_gpu_laser + gazebo_ros_range + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ) + +if (NOT GAZEBO_VERSION VERSION_LESS 6.0) + install(TARGETS gazebo_ros_elevator + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ) +endif() + +if (NOT GAZEBO_VERSION VERSION_LESS 7.3) + install(TARGETS gazebo_ros_harness + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ) +endif() + +install(DIRECTORY include/ + DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +install(PROGRAMS scripts/set_wrench.py scripts/set_pose.py scripts/gazebo_model + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) + +install(DIRECTORY Media + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +install(DIRECTORY test + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + +# Tests +# These need to be run with -j1 flag because gazebo can't be run +# in parallel. +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + + add_rostest_gtest(set_model_state-test + test/set_model_state_test/set_model_state_test.test + test/set_model_state_test/set_model_state_test.cpp) + target_link_libraries(set_model_state-test ${catkin_LIBRARIES}) + + add_rostest(test/range/range_plugin.test) + + if (ENABLE_DISPLAY_TESTS) + add_rostest_gtest(depth_camera-test + test/camera/depth_camera.test + test/camera/depth_camera.cpp) + target_link_libraries(depth_camera-test ${catkin_LIBRARIES}) + add_rostest_gtest(multicamera-test + test/camera/multicamera.test + test/camera/multicamera.cpp) + target_link_libraries(multicamera-test ${catkin_LIBRARIES}) + add_rostest_gtest(camera-test + test/camera/camera.test + test/camera/camera.cpp) + target_link_libraries(camera-test ${catkin_LIBRARIES}) + add_rostest_gtest(camera16bit-test + test/camera/camera16bit.test + test/camera/camera16bit.cpp) + target_link_libraries(camera16bit-test ${catkin_LIBRARIES}) + add_rostest_gtest(distortion_barrel_test + test/camera/distortion_barrel.test + test/camera/distortion_barrel.cpp) + target_link_libraries(distortion_barrel_test ${catkin_LIBRARIES}) + add_rostest_gtest(distortion_pincushion_test + test/camera/distortion_pincushion.test + test/camera/distortion_pincushion.cpp) + target_link_libraries(distortion_pincushion_test ${catkin_LIBRARIES}) + add_rostest_gtest(triggered-camera-test + test/camera/triggered_camera.test + test/camera/triggered_camera.cpp) + target_link_libraries(triggered-camera-test ${catkin_LIBRARIES}) + endif() +endif() diff --git a/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/doc.kml b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/doc.kml new file mode 100755 index 0000000..d539086 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/doc.kml @@ -0,0 +1,58 @@ + + + + Chair + Google SketchUp 7.0.8657]]> + SketchUp +1 + +320.223 +64.7271 +40.01698901172049 +-105.2829857471169 +2.891488298910991 +1.320883614486943 + + +Tour + + +1 + +320.223 +64.7271 +40.01698901172049 +-105.2829857471169 +2.891488298910991 +1.320883614486943 + + + + + Model + + + + relativeToGround + + -105.283000000000 + 40.017000000000 + 0.000000000000 + + + 0 + 0 + 0 + + + 1.0 + 1.0 + 1.0 + + + models/Chair.dae + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/images/texture0.jpg b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/images/texture0.jpg new file mode 100755 index 0000000..716f434 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/images/texture0.jpg differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/images/texture1.jpg b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/images/texture1.jpg new file mode 100755 index 0000000..2ac91cb Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/images/texture1.jpg differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/models/Chair.dae b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/models/Chair.dae new file mode 100755 index 0000000..7c2ee39 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/models/Chair.dae @@ -0,0 +1,1382 @@ + + + + + Google SketchUp 7.0.8657 + + 2008-11-26T21:19:56Z + 2008-11-26T21:19:56Z + + Z_UP + + + + ../images/texture0.jpg + + + ../images/texture1.jpg + + + + + + + + + + + + + + + + + + + material_0_1_0-image + + + + + material_0_1_0-image-surface + + + + + + 0.000000 0.000000 0.000000 1 + + + 0.000000 0.000000 0.000000 1 + + + + + + 1 1 1 1 + + + 0.000000 + + + + + + 1 + + + + + + + + + material_1_2_0-image + + + + + material_1_2_0-image-surface + + + + + + 0.000000 0.000000 0.000000 1 + + + 0.000000 0.000000 0.000000 1 + + + + + + 1 1 1 1 + + + 0.000000 + + + + + + 1 + + + + + + + + + + 0.000000 0.000000 0.000000 1 + + + 0.000000 0.000000 0.000000 1 + + + 0.086275 0.086275 0.086275 1 + + + 1 1 1 1 + + + 0.000000 + + + + + + 1 + + + + + + + + + + 6.631565 14.812500 17.125000 8.187500 0.000000 17.625000 8.187500 0.000000 17.125000 6.631565 14.812500 17.625000 -8.187500 0.000000 17.125000 5.166495 14.812500 17.125000 5.166495 14.812500 17.625000 -8.187500 0.000000 17.625000 -6.631565 14.812500 17.125000 -5.166495 14.812500 17.125000 -5.166495 16.250000 17.625000 -6.631565 14.812500 17.625000 -6.554290 14.812500 17.125000 5.166495 16.250000 17.125000 -5.166495 14.812500 17.625000 5.166495 16.250000 17.625000 -5.166495 16.250000 17.125000 + + + + + + + + + + 0.994528 0.104467 0.000000 -0.000000 -1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 1.000000 0.000000 0.000000 0.000000 1.000000 -0.994528 0.104467 -0.000000 1.000000 0.000000 0.000000 -1.000000 -0.000000 0.000000 + + + + + + + + + + 0.823959 0.000000 -0.106916 0.046143 -0.106916 0.000000 0.823959 0.046143 1.023438 0.046143 0.000000 0.000000 1.023438 0.000000 -1.364583 0.000000 -1.112833 0.627648 -1.234922 0.627648 -0.926192 0.000000 -0.834625 0.046143 -0.926192 0.046143 0.834625 1.366974 0.926192 1.366974 0.000000 0.046143 -0.129661 0.627648 -0.251750 0.627648 -0.834625 0.000000 0.188813 1.499634 0.097246 1.366974 -0.930875 0.046143 -0.136101 0.627648 -0.930875 0.000000 -1.112833 0.688559 1.015625 0.000000 0.925781 0.046143 0.925781 0.000000 0.188813 1.366974 0.834625 1.499634 -0.097246 0.046143 -0.102076 0.000000 -0.097246 0.000000 -0.188813 0.000000 -0.251750 0.688559 1.015625 0.046143 -1.015625 0.046143 -0.925781 0.000000 -0.925781 0.046143 -0.188813 0.046143 -1.015625 0.000000 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 1 1 4 4 1 5 2 1 6 0 3 10 6 3 11 3 3 12 6 4 13 1 4 6 3 4 14 4 1 5 1 1 4 7 1 15 6 3 11 0 3 10 5 3 18 10 4 19 1 4 6 6 4 13 1 4 6 11 4 20 7 4 5 11 5 21 4 5 5 7 5 15 4 5 5 11 5 21 8 5 23 13 6 25 6 6 26 5 6 27 14 4 28 1 4 6 10 4 19 15 4 29 10 4 19 6 4 13 11 4 20 1 4 6 14 4 28 11 3 30 12 3 31 8 3 32 12 3 31 11 3 30 9 3 33 6 6 26 13 6 25 15 6 35 10 7 36 9 7 37 14 7 38 13 3 18 10 3 39 15 3 11 9 3 33 11 3 30 14 3 39 9 7 37 10 7 36 16 7 40 10 3 39 13 3 18 16 3 33

+ + + + + +

2 2 7 5 2 8 0 2 9 8 2 16 2 2 7 4 2 5 2 2 7 9 2 17 5 2 8 2 2 7 8 2 16 12 2 22 2 2 7 12 2 22 9 2 17 5 2 8 9 2 17 13 2 24 13 2 24 9 2 17 16 2 34

+ + + + + + + -5.386791 15.288982 4.125000 -6.852535 1.500000 2.937500 -5.386791 15.288982 2.937500 -6.852535 1.500000 4.125000 -6.199291 15.288982 2.937500 -6.199291 15.288982 4.125000 -7.665035 1.500000 2.937500 -7.665035 1.500000 4.125000 + + + + + + + + + + 0.994398 -0.105703 0.000000 0.000000 0.000000 -1.000000 0.000000 1.000000 0.000000 0.000000 0.000000 1.000000 -0.994398 0.105703 0.000000 + + + + + + + + + + -0.074219 1.265389 0.000000 -0.014298 -0.000000 1.265389 -0.074219 -0.014298 0.091609 -0.000000 0.050781 1.272518 0.000000 1.272518 0.050781 -0.000000 -0.000000 0.109589 -0.000000 -0.000000 -0.050781 1.272518 -0.091609 -0.000000 0.142390 0.000000 0.091609 0.109589 0.091609 0.000000 0.142390 -0.000000 0.050781 0.109589 -0.142390 -0.000000 0.142390 0.109589 0.074219 -0.022224 -0.000000 1.257464 -0.000000 -0.022224 0.074219 1.257464 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 1 1 4 4 1 5 2 1 6 4 2 7 0 2 8 2 2 9 5 3 10 3 3 11 0 3 6 6 2 12 3 2 13 1 2 14 4 1 5 1 1 4 6 1 15 0 2 8 4 2 7 5 2 16 3 3 11 5 3 10 7 3 17 3 2 13 6 2 12 7 2 18 7 4 19 4 4 20 6 4 21 4 4 20 7 4 19 5 4 22

+ + + + + + + -5.420971 14.924370 17.125000 -6.847958 1.500000 15.125000 -5.420971 14.924370 15.125000 -6.847958 1.500000 17.125000 -6.238048 14.924370 15.125000 -7.665035 1.500000 17.125000 -7.665035 1.500000 15.125000 -6.238048 14.924370 17.125000 + + + + + + + + + + 0.994398 -0.105703 0.000000 0.000000 0.000000 -1.000000 -0.000000 1.000000 -0.000000 0.000000 0.000000 1.000000 0.000000 1.000000 0.000000 -0.994398 0.105703 -0.000000 + + + + + + + + + + -0.125000 1.231930 0.000000 -0.013920 0.000000 1.231930 -0.125000 -0.013920 0.089187 0.000000 0.051067 1.238870 0.000000 1.238870 0.051067 -0.000000 -0.000000 0.184570 -0.000000 -0.000000 -0.140254 0.000000 -0.089187 0.000000 0.140254 0.000000 0.089187 0.184570 0.089187 0.000000 0.140254 0.000000 0.051067 0.184570 -0.051067 1.238870 0.140254 0.184570 0.125000 -0.021890 0.000000 1.223959 0.000000 -0.021890 0.125000 1.223959 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 1 1 4 4 1 5 2 1 6 4 2 7 0 2 8 2 2 9 0 3 6 5 3 10 3 3 11 6 4 12 3 4 13 1 4 14 4 1 5 1 1 4 6 1 15 0 2 8 4 2 7 7 2 16 5 3 10 0 3 6 7 3 17 3 4 13 6 4 12 5 4 18 5 5 19 4 5 20 6 5 21 4 5 20 5 5 19 7 5 22

+ + + + + + + -7.677584 0.843750 16.074436 -7.677584 0.843750 16.175564 -7.677584 0.937500 16.027561 -7.771334 0.843750 16.074436 -7.677584 0.937500 16.222439 -7.771334 0.843750 16.175564 -7.677584 1.031250 16.074436 -7.771334 0.937500 16.222439 -7.771334 0.937500 16.027561 -7.771334 0.828260 16.314210 -7.677584 1.031250 16.175564 -7.771334 1.031250 16.175564 -7.771334 0.880953 15.913964 -7.771334 0.880953 16.336036 -7.771334 0.828260 15.935790 -7.771334 1.031250 16.074436 -7.771334 0.937500 16.343480 -7.771334 0.937500 15.906520 -7.771334 0.783011 16.279489 -7.771334 0.994047 15.913964 -7.771334 0.994047 16.336036 -7.771334 0.783011 15.970511 -7.771334 1.046740 15.935790 -7.771334 1.046740 16.314210 -7.771334 0.748290 16.234240 -7.771334 0.748290 16.015760 -7.771334 1.091989 15.970511 -7.771334 0.726464 16.181547 -7.771334 1.091989 16.279489 -7.771334 0.726464 16.068453 -7.771334 1.126710 16.015760 -7.771334 0.719020 16.125000 -7.771334 1.126710 16.234240 -7.771334 1.148536 16.068453 -7.771334 1.148536 16.181547 -7.771334 1.155980 16.125000 + + + + + + + + + + -1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.447214 0.894427 0.000000 0.447214 -0.894427 0.000000 -0.447214 0.894427 -0.000000 -0.447214 -0.894427 0.000000 -1.000000 0.000000 + + + + + + + + + + 0.156250 0.199436 0.156250 0.300564 0.250000 0.152561 0.199436 -0.093750 0.199436 0.000000 0.300564 -0.093750 0.250000 0.347439 -0.050564 -0.093750 -0.155379 -0.093750 -0.050564 0.000000 0.300564 0.000000 0.343750 0.199436 0.378986 -0.093750 0.274170 -0.093750 0.378986 0.000000 -0.155379 0.000000 0.274170 0.000000 0.140760 0.439210 -0.291834 -0.093750 -0.396650 -0.093750 -0.291834 0.000000 0.343750 0.300564 0.068228 0.000000 0.173043 0.000000 0.068228 -0.093750 0.193453 0.038964 0.193453 0.461036 0.140760 0.060790 -0.396650 0.000000 -0.300564 -0.093750 -0.300564 0.000000 -0.199436 -0.093750 0.173043 -0.093750 0.250000 0.468480 0.250000 0.031520 0.095511 0.404489 0.306547 0.038964 -0.199436 0.000000 0.306547 0.461036 0.095511 0.095511 0.359240 0.060790 0.359240 0.439210 0.060790 0.359240 0.060790 0.140760 0.404489 0.095511 0.038964 0.306547 0.404489 0.404489 0.038964 0.193453 0.439210 0.140760 0.031520 0.250000 0.439210 0.359240 0.461036 0.193453 0.461036 0.306547 0.468480 0.250000 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 0 1 3 3 1 4 1 1 5 4 0 6 2 0 2 1 0 1 0 2 7 2 2 8 3 2 9 5 1 10 1 1 5 3 1 4 6 0 11 2 0 2 4 0 6 4 3 12 1 3 13 7 3 14 8 2 15 3 2 9 2 2 8 5 3 16 7 3 14 1 3 13 3 0 0 9 0 17 5 0 1 2 4 18 6 4 19 8 4 20 10 0 21 6 0 11 4 0 6 7 5 22 11 5 23 4 5 24 8 0 2 12 0 25 3 0 0 7 0 6 5 0 1 13 0 26 3 0 0 14 0 27 9 0 17 13 0 26 5 0 1 9 0 17 15 4 28 8 4 20 6 4 19 10 6 29 11 6 30 6 6 31 10 5 32 4 5 24 11 5 23 11 0 21 7 0 6 16 0 33 17 0 34 12 0 25 8 0 2 12 0 25 14 0 27 3 0 0 16 0 33 7 0 6 13 0 26 9 0 17 14 0 27 18 0 35 15 0 11 19 0 36 8 0 2 15 6 37 6 6 31 11 6 30 20 0 38 11 0 21 16 0 33 19 0 36 17 0 34 8 0 2 14 0 27 21 0 39 18 0 35 22 0 40 19 0 36 15 0 11 11 0 21 22 0 40 15 0 11 23 0 41 11 0 21 20 0 38 18 0 35 21 0 39 24 0 42 22 0 40 11 0 21 23 0 41 21 0 39 25 0 43 24 0 42 26 0 44 22 0 40 23 0 41 24 0 42 25 0 43 27 0 45 28 0 46 26 0 44 23 0 41 25 0 43 29 0 47 27 0 45 30 0 48 26 0 44 28 0 46 31 0 49 27 0 45 29 0 47 32 0 50 30 0 48 28 0 46 33 0 51 30 0 48 32 0 50 34 0 52 33 0 51 32 0 50 35 0 53 33 0 51 34 0 52

+ + + + + + + -6.708834 1.437500 0.000000 -7.708834 0.437500 0.000000 -7.708834 1.437500 0.000000 -6.708834 0.437500 0.000000 -7.771334 1.500000 0.125000 -6.646334 1.500000 0.125000 -6.646334 0.375000 0.125000 -7.771334 0.375000 0.125000 -7.771334 0.719020 16.125000 -7.771334 1.500000 17.125000 -6.646334 0.375000 17.125000 -7.771334 0.726464 16.068453 -7.771334 0.375000 17.125000 -6.646334 1.500000 17.125000 -7.771334 1.155980 16.125000 -7.771334 0.748290 16.015760 -7.771334 0.726464 16.181547 -7.771334 1.148536 16.068453 -7.771334 1.148536 16.181547 -7.771334 0.783011 15.970511 -7.771334 0.748290 16.234240 -7.771334 1.126710 16.015760 -7.771334 1.126710 16.234240 -7.771334 0.783011 16.279489 -7.771334 0.828260 15.935790 -7.771334 1.091989 15.970511 -7.771334 1.091989 16.279489 -7.771334 0.828260 16.314210 -7.771334 0.880953 15.913964 -7.771334 1.046740 15.935790 -7.771334 1.046740 16.314210 -7.771334 0.880953 16.336036 -7.771334 0.937500 15.906520 -7.771334 0.994047 15.913964 -7.771334 0.994047 16.336036 -7.771334 0.937500 16.343480 + + + + + + + + + + 0.000000 0.000000 -1.000000 -0.894427 0.000000 -0.447214 0.000000 0.894427 -0.447214 0.894427 0.000000 -0.447214 0.000000 -0.894427 -0.447214 -1.000000 0.000000 0.000000 0.000000 1.000000 -0.000000 1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 0.000000 1.000000 + + + + + + + + + + -0.066406 0.098053 -0.003906 0.005768 -0.003906 0.098053 -0.066406 0.005768 -0.003906 -0.002579 -0.070313 0.010318 -0.066406 -0.002579 0.000000 0.056748 -0.066406 0.043851 -0.003906 0.043851 0.070313 0.056748 0.003906 0.043851 0.066406 0.043851 0.070313 0.010318 0.003906 -0.002579 0.066406 -0.002579 0.000000 0.010318 -0.070313 0.056748 -0.021501 1.488098 -0.070313 0.011536 0.000000 0.011536 0.000000 1.580383 -0.070312 0.011536 0.000000 0.011536 0.070313 0.011536 0.000000 1.580383 0.070313 1.580383 -0.021967 1.482880 -0.070312 1.580383 -0.048811 1.488098 -0.070313 1.580383 -0.023331 1.478017 -0.021967 1.493317 0.000000 0.103821 0.070313 0.000000 0.070313 0.103821 -0.048346 1.482880 -0.048346 1.493317 0.000000 0.000000 -0.025501 1.473841 -0.023331 1.498179 -0.046982 1.478017 -0.046982 1.498179 -0.025501 1.502355 -0.028329 1.470637 -0.044812 1.473841 -0.044812 1.502355 -0.028329 1.505559 -0.031622 1.468623 -0.041984 1.470637 -0.041984 1.505559 -0.031622 1.507574 -0.035156 1.467936 -0.038690 1.468623 -0.038690 1.507574 -0.035156 1.508261 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 1 1 4 4 1 5 2 1 6 4 2 7 0 2 8 2 2 9 5 3 10 3 3 11 0 3 12 6 4 13 1 4 14 3 4 15 4 1 5 1 1 4 7 1 16 0 2 8 4 2 7 5 2 17 3 3 11 5 3 10 6 3 7 1 4 14 6 4 13 7 4 16 8 5 18 4 5 19 7 5 20 9 6 21 5 6 22 4 6 23 5 7 24 10 7 25 6 7 20 10 8 26 7 8 20 6 8 24 4 5 19 8 5 18 11 5 27 12 5 25 8 5 18 7 5 20 5 6 22 9 6 21 13 6 28 4 5 19 14 5 29 9 5 30 10 7 25 5 7 24 13 7 26 7 8 20 10 8 26 12 8 25 4 5 19 11 5 27 15 5 31 8 5 18 12 5 25 16 5 32 9 9 33 10 9 34 13 9 35 4 5 19 17 5 36 14 5 29 18 5 37 9 5 30 14 5 29 10 9 34 9 9 33 12 9 38 4 5 19 15 5 31 19 5 39 16 5 32 12 5 25 20 5 40 4 5 19 21 5 41 17 5 36 22 5 42 9 5 30 18 5 37 23 5 43 12 5 25 9 5 30 4 5 19 19 5 39 24 5 44 20 5 40 12 5 25 23 5 43 4 5 19 25 5 45 21 5 41 26 5 46 9 5 30 22 5 42 23 5 43 9 5 30 27 5 47 4 5 19 24 5 44 28 5 48 4 5 19 29 5 49 25 5 45 30 5 50 9 5 30 26 5 46 27 5 47 9 5 30 31 5 51 4 5 19 28 5 48 32 5 52 4 5 19 33 5 53 29 5 49 34 5 54 9 5 30 30 5 50 31 5 51 9 5 30 35 5 55 4 5 19 32 5 52 33 5 53 35 5 55 9 5 30 34 5 54

+ + + + + + + -6.197745 18.159422 34.379686 -6.197745 18.159422 34.480813 -6.197745 18.253172 34.332811 -6.291495 18.159422 34.379686 -6.197745 18.253172 34.527688 -6.291495 18.159422 34.480813 -6.197745 18.346922 34.379686 -6.291495 18.253172 34.527688 -6.291495 18.253172 34.332811 -6.291495 18.143932 34.619459 -6.197745 18.346922 34.480813 -6.291495 18.346922 34.480813 -6.291495 18.196625 34.219214 -6.291495 18.196625 34.641286 -6.291495 18.143932 34.241040 -6.291495 18.346922 34.379686 -6.291495 18.253172 34.648730 -6.291495 18.253172 34.211769 -6.291495 18.098683 34.584739 -6.291495 18.309719 34.219214 -6.291495 18.309719 34.641286 -6.291495 18.098683 34.275761 -6.291495 18.362412 34.241040 -6.291495 18.362412 34.619459 -6.291495 18.063962 34.539490 -6.291495 18.063962 34.321010 -6.291495 18.407661 34.275761 -6.291495 18.042136 34.486797 -6.291495 18.407661 34.584739 -6.291495 18.042136 34.373703 -6.291495 18.442381 34.321010 -6.291495 18.034692 34.430250 -6.291495 18.442381 34.539490 -6.291495 18.464208 34.373703 -6.291495 18.464208 34.486797 -6.291495 18.471652 34.430250 + + + + + + + + + + -1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.447214 0.894427 0.000000 0.447214 -0.894427 0.000000 -0.447214 0.894427 -0.000000 -0.447214 -0.894427 0.000000 -1.000000 0.000000 + + + + + + + + + + 0.156250 0.199436 0.156250 0.300564 0.250000 0.152561 0.199436 -0.093750 0.199436 0.000000 0.300564 -0.093750 0.250000 0.347439 -0.050564 -0.093750 -0.155379 -0.093750 -0.050564 0.000000 0.300564 0.000000 0.343750 0.199436 0.378986 -0.093750 0.274170 -0.093750 0.378986 0.000000 -0.155379 0.000000 0.274170 0.000000 0.140760 0.439210 -0.291834 -0.093750 -0.396650 -0.093750 -0.291834 0.000000 0.343750 0.300564 0.068228 0.000000 0.173043 0.000000 0.068228 -0.093750 0.193453 0.038964 0.193453 0.461036 0.140760 0.060790 -0.396650 0.000000 -0.300564 -0.093750 -0.300564 0.000000 -0.199436 -0.093750 0.173043 -0.093750 0.250000 0.468480 0.250000 0.031520 0.095511 0.404489 0.306547 0.038964 -0.199436 0.000000 0.306547 0.461036 0.095511 0.095511 0.359240 0.060790 0.359240 0.439210 0.060790 0.359240 0.060790 0.140760 0.404489 0.095511 0.038964 0.306547 0.404489 0.404489 0.038964 0.193453 0.439210 0.140760 0.031520 0.250000 0.439210 0.359240 0.461036 0.193453 0.461036 0.306547 0.468480 0.250000 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 0 1 3 3 1 4 1 1 5 4 0 6 2 0 2 1 0 1 0 2 7 2 2 8 3 2 9 5 1 10 1 1 5 3 1 4 6 0 11 2 0 2 4 0 6 4 3 12 1 3 13 7 3 14 8 2 15 3 2 9 2 2 8 5 3 16 7 3 14 1 3 13 3 0 0 9 0 17 5 0 1 2 4 18 6 4 19 8 4 20 10 0 21 6 0 11 4 0 6 7 5 22 11 5 23 4 5 24 8 0 2 12 0 25 3 0 0 7 0 6 5 0 1 13 0 26 3 0 0 14 0 27 9 0 17 13 0 26 5 0 1 9 0 17 15 4 28 8 4 20 6 4 19 10 6 29 11 6 30 6 6 31 10 5 32 4 5 24 11 5 23 11 0 21 7 0 6 16 0 33 17 0 34 12 0 25 8 0 2 12 0 25 14 0 27 3 0 0 16 0 33 7 0 6 13 0 26 9 0 17 14 0 27 18 0 35 15 0 11 19 0 36 8 0 2 15 6 37 6 6 31 11 6 30 20 0 38 11 0 21 16 0 33 19 0 36 17 0 34 8 0 2 14 0 27 21 0 39 18 0 35 22 0 40 19 0 36 15 0 11 11 0 21 22 0 40 15 0 11 23 0 41 11 0 21 20 0 38 18 0 35 21 0 39 24 0 42 22 0 40 11 0 21 23 0 41 21 0 39 25 0 43 24 0 42 26 0 44 22 0 40 23 0 41 24 0 42 25 0 43 27 0 45 28 0 46 26 0 44 23 0 41 25 0 43 29 0 47 27 0 45 30 0 48 26 0 44 28 0 46 31 0 49 27 0 45 29 0 47 32 0 50 30 0 48 28 0 46 33 0 51 30 0 48 32 0 50 34 0 52 33 0 51 32 0 50 35 0 53 33 0 51 34 0 52

+ + + + + + + -6.197745 15.525999 16.074436 -6.197745 15.525999 16.175564 -6.197745 15.619749 16.027561 -6.291495 15.525999 16.074436 -6.197745 15.619749 16.222439 -6.291495 15.525999 16.175564 -6.197745 15.713499 16.074436 -6.291495 15.619749 16.222439 -6.291495 15.619749 16.027561 -6.291495 15.510509 16.314210 -6.197745 15.713499 16.175564 -6.291495 15.713499 16.175564 -6.291495 15.563202 15.913964 -6.291495 15.563202 16.336036 -6.291495 15.510509 15.935790 -6.291495 15.713499 16.074436 -6.291495 15.619749 16.343480 -6.291495 15.619749 15.906520 -6.291495 15.465260 16.279489 -6.291495 15.676296 15.913964 -6.291495 15.676296 16.336036 -6.291495 15.465260 15.970511 -6.291495 15.728989 15.935790 -6.291495 15.728989 16.314210 -6.291495 15.430540 16.234240 -6.291495 15.430540 16.015760 -6.291495 15.774238 15.970511 -6.291495 15.408713 16.181547 -6.291495 15.774238 16.279489 -6.291495 15.408713 16.068453 -6.291495 15.808959 16.015760 -6.291495 15.401269 16.125000 -6.291495 15.808959 16.234240 -6.291495 15.830785 16.068453 -6.291495 15.830785 16.181547 -6.291495 15.838230 16.125000 + + + + + + + + + + -1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.447214 0.894427 0.000000 0.447214 -0.894427 0.000000 -0.447214 0.894427 -0.000000 -0.447214 -0.894427 0.000000 -1.000000 0.000000 + + + + + + + + + + 0.156250 0.199436 0.156250 0.300564 0.250000 0.152561 0.199436 -0.093750 0.199436 0.000000 0.300564 -0.093750 0.250000 0.347439 -0.050564 -0.093750 -0.155379 -0.093750 -0.050564 0.000000 0.300564 0.000000 0.343750 0.199436 0.378986 -0.093750 0.274170 -0.093750 0.378986 0.000000 -0.155379 0.000000 0.274170 0.000000 0.140760 0.439210 -0.291834 -0.093750 -0.396650 -0.093750 -0.291834 0.000000 0.343750 0.300564 0.068228 0.000000 0.173043 0.000000 0.068228 -0.093750 0.193453 0.038964 0.193453 0.461036 0.140760 0.060790 -0.396650 0.000000 -0.300564 -0.093750 -0.300564 0.000000 -0.199436 -0.093750 0.173043 -0.093750 0.250000 0.468480 0.250000 0.031520 0.095511 0.404489 0.306547 0.038964 -0.199436 0.000000 0.306547 0.461036 0.095511 0.095511 0.359240 0.060790 0.359240 0.439210 0.060790 0.359240 0.060790 0.140760 0.404489 0.095511 0.038964 0.306547 0.404489 0.404489 0.038964 0.193453 0.439210 0.140760 0.031520 0.250000 0.439210 0.359240 0.461036 0.193453 0.461036 0.306547 0.468480 0.250000 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 0 1 3 3 1 4 1 1 5 4 0 6 2 0 2 1 0 1 0 2 7 2 2 8 3 2 9 5 1 10 1 1 5 3 1 4 6 0 11 2 0 2 4 0 6 4 3 12 1 3 13 7 3 14 8 2 15 3 2 9 2 2 8 5 3 16 7 3 14 1 3 13 3 0 0 9 0 17 5 0 1 2 4 18 6 4 19 8 4 20 10 0 21 6 0 11 4 0 6 7 5 22 11 5 23 4 5 24 8 0 2 12 0 25 3 0 0 7 0 6 5 0 1 13 0 26 3 0 0 14 0 27 9 0 17 13 0 26 5 0 1 9 0 17 15 4 28 8 4 20 6 4 19 10 6 29 11 6 30 6 6 31 10 5 32 4 5 24 11 5 23 11 0 21 7 0 6 16 0 33 17 0 34 12 0 25 8 0 2 12 0 25 14 0 27 3 0 0 16 0 33 7 0 6 13 0 26 9 0 17 14 0 27 18 0 35 15 0 11 19 0 36 8 0 2 15 6 37 6 6 31 11 6 30 20 0 38 11 0 21 16 0 33 19 0 36 17 0 34 8 0 2 14 0 27 21 0 39 18 0 35 22 0 40 19 0 36 15 0 11 11 0 21 22 0 40 15 0 11 23 0 41 11 0 21 20 0 38 18 0 35 21 0 39 24 0 42 22 0 40 11 0 21 23 0 41 21 0 39 25 0 43 24 0 42 26 0 44 22 0 40 23 0 41 24 0 42 25 0 43 27 0 45 28 0 46 26 0 44 23 0 41 25 0 43 29 0 47 27 0 45 30 0 48 26 0 44 28 0 46 31 0 49 27 0 45 29 0 47 32 0 50 30 0 48 28 0 46 33 0 51 30 0 48 32 0 50 34 0 52 33 0 51 32 0 50 35 0 53 33 0 51 34 0 52

+ + + + + + + -5.228995 15.484069 0.000000 -5.166495 16.546569 0.125000 -5.166495 15.421569 0.125000 -5.228995 16.484069 0.000000 -5.166495 16.546569 17.625000 -6.228995 15.484069 0.000000 -6.291495 16.546569 0.125000 -5.166495 14.924370 17.625000 -6.291495 16.546569 17.625000 -6.291495 15.421569 0.125000 -6.228995 16.484069 0.000000 -5.166495 14.924370 15.125000 -5.166495 17.883552 35.380091 -6.291495 18.993245 35.195142 -6.291495 15.838230 16.125000 -6.291495 14.924370 15.125000 -5.166495 18.993245 35.195142 -6.291495 14.924370 17.625000 -6.291495 18.442381 34.321010 -6.291495 15.430540 16.015760 -6.291495 15.830785 16.068453 -6.291495 17.883552 35.380091 -5.228995 17.962441 35.493621 -6.228995 18.948835 35.329222 -6.291495 18.407661 34.275761 -6.291495 18.464208 34.373703 -6.291495 15.401269 16.125000 -6.291495 15.408713 16.068453 -6.291495 15.465260 15.970511 -6.291495 15.808959 16.015760 -6.291495 15.830785 16.181547 -6.291495 18.042136 34.373703 -5.228995 18.948835 35.329222 -6.228995 17.962441 35.493621 -6.291495 18.362412 34.241040 -6.291495 18.471652 34.430250 -6.291495 15.510509 15.935790 -6.291495 15.774238 15.970511 -6.291495 15.808959 16.234240 -6.291495 18.034692 34.430250 -6.291495 18.063962 34.321010 -6.291495 18.143932 34.619459 -6.291495 18.309719 34.219214 -6.291495 18.464208 34.486797 -6.291495 15.408713 16.181547 -6.291495 15.563202 15.913964 -6.291495 15.728989 15.935790 -6.291495 15.774238 16.279489 -6.291495 18.042136 34.486797 -6.291495 18.098683 34.275761 -6.291495 18.098683 34.584739 -6.291495 18.196625 34.641286 -6.291495 18.253172 34.211769 -6.291495 18.442381 34.539490 -6.291495 15.430540 16.234240 -6.291495 15.619749 15.906520 -6.291495 15.676296 15.913964 -6.291495 15.728989 16.314210 -6.291495 18.063962 34.539490 -6.291495 18.143932 34.241040 -6.291495 18.253172 34.648730 -6.291495 18.196625 34.219214 -6.291495 18.407661 34.584739 -6.291495 15.465260 16.279489 -6.291495 15.676296 16.336036 -6.291495 18.309719 34.641286 -6.291495 18.362412 34.619459 -6.291495 15.510509 16.314210 -6.291495 15.619749 16.343480 -6.291495 15.563202 16.336036 + + + + + + + + + + 0.894427 0.000000 -0.447214 1.000000 -0.000000 -0.000000 0.000000 -0.894427 -0.447214 0.000000 0.000000 -1.000000 0.000000 0.894427 -0.447214 0.000000 1.000000 0.000000 -0.000000 0.990443 -0.137921 0.000000 -0.999451 -0.033128 -0.894427 0.000000 -0.447214 -1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 -0.986394 0.164399 0.000000 0.164399 0.986394 0.894363 0.073543 0.441255 0.000000 0.949285 0.314418 0.000000 -0.821203 0.570636 -0.894363 0.073543 0.441255 0.000000 0.164399 0.986394 + + + + + + + + + + 0.034981 0.043851 0.101387 0.056748 0.031075 0.056748 0.097481 0.043851 0.101387 1.626526 0.031075 0.011536 0.101387 0.011536 0.070313 -0.010202 0.003906 -0.023099 0.066406 -0.023099 -0.003906 0.051652 -0.066406 0.143937 -0.066406 0.051652 -0.066406 0.064371 0.000000 0.077268 -0.070313 0.077268 0.000000 1.626526 -0.070313 0.011536 0.000000 1.626526 -0.070313 1.626526 0.000000 -0.010202 -0.003906 0.143937 -0.003906 0.064371 0.000000 0.011536 0.000000 1.395813 0.184949 3.265057 -0.000000 3.268738 -0.070313 1.631629 -0.000000 1.631629 0.070313 1.395047 0.000000 0.010009 0.070313 0.010009 -0.101387 0.010318 -0.034981 -0.002579 -0.031075 0.010318 -0.097481 -0.002579 -0.101387 0.011536 -0.057116 1.488098 -0.101387 1.626526 0.070313 1.626526 0.000000 1.395813 0.070313 1.395813 0.254305 3.247989 0.070313 3.265528 -0.000000 1.604395 0.070313 1.604395 -0.070313 3.268738 -0.219876 3.167320 -0.254305 3.247989 0.000000 1.395047 -0.031075 0.011536 -0.031636 1.478017 -0.056651 1.482880 -0.184949 3.265057 0.000000 0.163578 -0.070313 0.267399 -0.070313 0.163578 0.247687 2.855450 0.183799 2.886145 0.178564 2.874469 0.000000 3.265528 -0.003906 2.978238 -0.070313 2.965204 0.000000 2.965204 -0.217706 3.163144 -0.221240 3.172183 -0.029806 1.488098 -0.030271 1.482880 -0.033806 1.473841 -0.055287 1.478017 -0.056651 1.493317 -0.194860 3.172183 0.000000 0.267399 0.245241 2.869238 0.066406 2.849868 0.000000 2.837110 0.070313 2.837110 -0.066406 2.978238 -0.253449 2.901107 -0.189561 2.931802 -0.251003 2.914895 -0.214878 3.159940 -0.221705 3.177401 -0.036634 1.470637 -0.053117 1.473841 -0.055287 1.498179 -0.194395 3.177401 -0.196225 3.167320 -0.201223 3.194862 -0.184327 2.920127 -0.003906 0.169655 -0.066406 0.261940 -0.066406 0.169655 0.003906 2.849868 -0.003906 0.261940 -0.211584 3.157926 -0.221240 3.182619 -0.030271 1.493317 -0.039927 1.468623 -0.050289 1.470637 -0.053117 1.502355 -0.194860 3.182619 -0.198395 3.163144 -0.198395 3.191658 -0.204516 3.196876 -0.208050 3.157238 -0.219876 3.187482 -0.031636 1.498179 -0.043461 1.467936 -0.046995 1.468623 -0.050289 1.505559 -0.196225 3.187482 -0.201223 3.159940 -0.208050 3.197563 -0.204516 3.157926 -0.217706 3.191658 -0.033806 1.502355 -0.046995 1.507574 -0.211584 3.196876 -0.214878 3.194862 -0.036634 1.505559 -0.043461 1.508261 -0.039927 1.507574 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 4 1 4 2 1 5 1 1 6 2 2 7 5 2 8 0 2 9 5 3 10 3 3 11 0 3 12 3 4 13 6 4 14 1 4 15 7 1 16 2 1 5 4 1 4 1 5 17 8 5 18 4 5 19 5 2 8 2 2 7 9 2 20 3 3 11 5 3 10 10 3 21 6 4 14 3 4 13 10 4 22 8 5 18 1 5 17 6 5 23 2 1 5 7 1 16 11 1 24 7 1 16 4 1 4 12 1 25 13 6 26 4 6 27 8 6 28 11 7 29 9 7 30 2 7 31 6 8 32 5 8 33 9 8 34 5 8 33 6 8 32 10 8 35 6 9 36 14 9 37 8 9 38 7 10 39 15 10 40 11 10 41 12 1 25 4 1 4 16 1 42 12 11 43 17 11 44 7 11 45 4 6 27 13 6 26 16 6 46 8 9 38 18 9 47 13 9 48 9 7 30 11 7 29 15 7 49 9 9 50 19 9 51 6 9 36 6 9 36 20 9 52 14 9 37 14 9 37 21 9 53 8 9 38 15 10 40 7 10 39 17 10 18 13 12 54 12 12 55 16 12 56 16 13 57 22 13 58 12 13 59 17 11 44 12 11 43 21 11 60 23 14 61 16 14 62 13 14 63 8 9 38 24 9 64 18 9 47 13 9 48 18 9 47 25 9 65 26 9 66 9 9 50 15 9 40 9 9 50 27 9 67 19 9 51 6 9 36 19 9 51 28 9 68 6 9 36 29 9 69 20 9 52 30 9 70 21 9 53 14 9 37 8 9 38 21 9 53 31 9 71 21 9 53 15 9 40 17 9 18 12 12 55 13 12 54 21 12 72 22 13 58 16 13 57 32 13 73 22 15 74 21 15 75 12 15 76 16 14 62 23 14 61 32 14 77 13 16 78 33 16 79 23 16 80 8 9 38 34 9 81 24 9 64 13 9 48 25 9 65 35 9 82 9 9 50 26 9 66 27 9 67 15 9 40 21 9 53 26 9 66 6 9 36 28 9 68 36 9 83 6 9 36 37 9 84 29 9 69 38 9 85 21 9 53 30 9 70 31 9 71 21 9 53 39 9 86 8 9 38 31 9 71 40 9 87 41 9 88 21 9 53 13 9 48 33 16 79 13 16 78 21 16 89 23 17 90 22 17 91 32 17 92 21 15 75 22 15 74 33 15 93 22 17 91 23 17 90 33 17 94 8 9 38 42 9 95 34 9 81 43 9 96 13 9 48 35 9 82 26 9 66 21 9 53 44 9 97 6 9 36 36 9 83 45 9 98 6 9 36 46 9 99 37 9 84 47 9 100 21 9 53 38 9 85 21 9 53 48 9 101 39 9 86 8 9 38 40 9 87 49 9 102 50 9 103 21 9 53 41 9 88 41 9 88 13 9 48 51 9 104 8 9 38 52 9 105 42 9 95 53 9 106 13 9 48 43 9 96 44 9 97 21 9 53 54 9 107 6 9 36 45 9 98 55 9 108 6 9 36 56 9 109 46 9 99 57 9 110 21 9 53 47 9 100 48 9 101 21 9 53 58 9 111 8 9 38 49 9 102 59 9 112 58 9 111 21 9 53 50 9 103 51 9 104 13 9 48 60 9 113 8 9 38 61 9 114 52 9 105 62 9 115 13 9 48 53 9 106 54 9 107 21 9 53 63 9 116 6 9 36 55 9 108 56 9 109 64 9 117 21 9 53 57 9 110 8 9 38 59 9 112 61 9 114 60 9 113 13 9 48 65 9 118 66 9 119 13 9 48 62 9 115 63 9 116 21 9 53 67 9 120 68 9 121 21 9 53 64 9 117 65 9 118 13 9 48 66 9 119 67 9 120 21 9 53 69 9 122 69 9 122 21 9 53 68 9 121

+ + + + + + + 5.386791 15.288982 2.937500 6.852535 1.500000 2.937500 5.386791 15.288982 4.125000 6.199291 15.288982 2.937500 6.852535 1.500000 4.125000 7.665035 1.500000 2.937500 6.199291 15.288982 4.125000 7.665035 1.500000 4.125000 + + + + + + + + + + -0.994398 -0.105703 0.000000 0.000000 0.000000 -1.000000 0.000000 1.000000 0.000000 0.000000 0.000000 1.000000 0.994398 0.105703 0.000000 + + + + + + + + + + -0.000000 1.265389 0.000000 -0.014298 -0.074219 1.265389 0.000000 1.272518 0.050781 1.272518 0.091609 -0.000000 -0.074219 -0.014298 -0.000000 -0.000000 -0.000000 0.109589 0.050781 -0.000000 0.142390 -0.000000 -0.091609 -0.000000 -0.050781 1.272518 0.091609 0.000000 0.091609 0.109589 0.142390 0.000000 0.050781 0.109589 -0.000000 -0.022224 -0.000000 1.257464 0.074219 -0.022224 -0.142390 -0.000000 0.142390 0.109589 0.074219 1.257464 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 0 1 3 3 1 4 1 1 5 4 0 6 2 0 2 1 0 1 0 2 7 2 2 8 3 2 9 5 1 10 1 1 5 3 1 4 2 3 3 4 3 11 6 3 12 1 2 13 4 2 14 5 2 15 6 2 16 3 2 9 2 2 8 5 4 17 3 4 18 7 4 19 7 3 20 6 3 12 4 3 11 7 2 21 5 2 15 4 2 14 6 4 22 7 4 19 3 4 18

+ + + + + + + 5.420971 14.924370 15.125000 6.847958 1.500000 15.125000 5.420971 14.924370 17.125000 6.238048 14.924370 15.125000 6.847958 1.500000 17.125000 7.665035 1.500000 15.125000 7.665035 1.500000 17.125000 6.238048 14.924370 17.125000 + + + + + + + + + + -0.994398 -0.105703 0.000000 0.000000 0.000000 -1.000000 0.000000 1.000000 -0.000000 0.000000 0.000000 1.000000 -0.000000 1.000000 0.000000 0.994398 0.105703 -0.000000 + + + + + + + + + + 0.000000 1.231930 0.000000 -0.013920 -0.125000 1.231930 0.000000 1.238870 0.051067 1.238870 0.089187 0.000000 -0.125000 -0.013920 -0.000000 -0.000000 -0.000000 0.184570 0.051067 -0.000000 0.140254 0.000000 -0.089187 0.000000 -0.140254 0.000000 0.089187 0.000000 0.089187 0.184570 0.140254 0.000000 0.051067 0.184570 0.000000 -0.021890 0.000000 1.223959 0.125000 -0.021890 0.140254 0.184570 -0.051067 1.238870 0.125000 1.223959 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 0 1 3 3 1 4 1 1 5 4 0 6 2 0 2 1 0 1 0 2 7 2 2 8 3 2 9 5 1 10 1 1 5 3 1 4 4 3 11 6 3 12 2 3 3 1 4 13 4 4 14 5 4 15 7 2 16 3 2 9 2 2 8 5 5 17 3 5 18 6 5 19 6 4 20 5 4 15 4 4 14 7 3 21 2 3 3 6 3 12 7 5 22 6 5 19 3 5 18

+ + + + + + + 7.677584 0.937500 16.027561 7.677584 0.843750 16.175564 7.677584 0.843750 16.074436 7.677584 0.937500 16.222439 7.771334 0.843750 16.074436 7.677584 1.031250 16.074436 7.771334 0.937500 16.222439 7.771334 0.843750 16.175564 7.771334 0.937500 16.027561 7.677584 1.031250 16.175564 7.771334 1.031250 16.175564 7.771334 0.828260 16.314210 7.771334 0.880953 15.913964 7.771334 1.031250 16.074436 7.771334 0.880953 16.336036 7.771334 0.937500 16.343480 7.771334 0.828260 15.935790 7.771334 0.937500 15.906520 7.771334 0.994047 15.913964 7.771334 0.994047 16.336036 7.771334 0.783011 16.279489 7.771334 1.046740 15.935790 7.771334 1.046740 16.314210 7.771334 0.783011 15.970511 7.771334 0.748290 16.234240 7.771334 1.091989 15.970511 7.771334 0.748290 16.015760 7.771334 1.091989 16.279489 7.771334 0.726464 16.181547 7.771334 1.126710 16.015760 7.771334 0.726464 16.068453 7.771334 1.126710 16.234240 7.771334 0.719020 16.125000 7.771334 1.148536 16.068453 7.771334 1.148536 16.181547 7.771334 1.155980 16.125000 + + + + + + + + + + 1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.447214 0.894427 0.000000 0.447214 -0.894427 0.000000 -0.447214 0.894427 0.000000 -0.447214 -0.894427 0.000000 -1.000000 0.000000 + + + + + + + + + + 0.250000 0.152561 0.156250 0.300564 0.156250 0.199436 0.250000 0.347439 0.300564 -0.093750 0.199436 0.000000 0.199436 -0.093750 -0.050564 0.000000 -0.155379 -0.093750 -0.050564 -0.093750 0.343750 0.199436 0.378986 0.000000 0.274170 -0.093750 0.378986 -0.093750 0.300564 0.000000 -0.155379 0.000000 -0.291834 0.000000 -0.396650 -0.093750 -0.291834 -0.093750 0.343750 0.300564 0.274170 0.000000 0.068228 -0.093750 0.173043 0.000000 0.068228 0.000000 0.140760 0.439210 0.193453 0.038964 -0.396650 0.000000 -0.199436 -0.093750 -0.300564 0.000000 -0.300564 -0.093750 0.173043 -0.093750 0.193453 0.461036 0.250000 0.468480 0.140760 0.060790 0.250000 0.031520 0.306547 0.038964 -0.199436 0.000000 0.306547 0.461036 0.095511 0.404489 0.359240 0.060790 0.359240 0.439210 0.095511 0.095511 0.060790 0.359240 0.404489 0.095511 0.060790 0.140760 0.404489 0.404489 0.038964 0.306547 0.439210 0.140760 0.038964 0.193453 0.439210 0.359240 0.031520 0.250000 0.461036 0.193453 0.461036 0.306547 0.468480 0.250000 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 1 1 4 4 1 5 2 1 6 4 2 7 0 2 8 2 2 9 3 0 3 0 0 0 5 0 10 6 3 11 1 3 12 3 3 13 4 1 5 1 1 4 7 1 14 0 2 8 4 2 7 8 2 15 8 4 16 5 4 17 0 4 18 3 0 3 5 0 10 9 0 19 1 3 12 6 3 11 7 3 20 3 5 21 10 5 22 6 5 23 7 0 1 11 0 24 4 0 2 4 0 2 12 0 25 8 0 0 5 4 17 8 4 16 13 4 26 5 6 27 10 6 28 9 6 29 10 5 22 3 5 21 9 5 30 14 0 31 7 0 1 6 0 3 15 0 32 6 0 3 10 0 19 11 0 24 7 0 1 14 0 31 11 0 24 16 0 33 4 0 2 4 0 2 16 0 33 12 0 25 8 0 0 12 0 25 17 0 34 8 0 0 18 0 35 13 0 10 10 6 28 5 6 27 13 6 36 14 0 31 6 0 3 15 0 32 15 0 32 10 0 19 19 0 37 20 0 38 16 0 33 11 0 24 8 0 0 17 0 34 18 0 35 13 0 10 18 0 35 21 0 39 13 0 10 21 0 39 10 0 19 19 0 37 10 0 19 22 0 40 20 0 38 23 0 41 16 0 33 22 0 40 10 0 19 21 0 39 24 0 42 23 0 41 20 0 38 22 0 40 21 0 39 25 0 43 24 0 42 26 0 44 23 0 41 22 0 40 25 0 43 27 0 45 28 0 46 26 0 44 24 0 42 27 0 45 25 0 43 29 0 47 28 0 46 30 0 48 26 0 44 27 0 45 29 0 47 31 0 49 30 0 48 28 0 46 32 0 50 31 0 49 29 0 47 33 0 51 31 0 49 33 0 51 34 0 52 34 0 52 33 0 51 35 0 53

+ + + + + + + 7.708834 1.437500 0.000000 7.708834 0.437500 0.000000 6.708834 1.437500 0.000000 7.771334 1.500000 0.125000 6.708834 0.437500 0.000000 7.771334 0.375000 0.125000 6.646334 1.500000 0.125000 6.646334 0.375000 0.125000 7.771334 0.719020 16.125000 7.771334 1.500000 17.125000 6.646334 0.375000 17.125000 7.771334 0.726464 16.068453 7.771334 0.375000 17.125000 6.646334 1.500000 17.125000 7.771334 1.155980 16.125000 7.771334 0.748290 16.015760 7.771334 0.726464 16.181547 7.771334 1.148536 16.181547 7.771334 1.148536 16.068453 7.771334 0.783011 15.970511 7.771334 0.748290 16.234240 7.771334 1.126710 16.234240 7.771334 1.126710 16.015760 7.771334 0.783011 16.279489 7.771334 0.828260 15.935790 7.771334 1.091989 16.279489 7.771334 1.091989 15.970511 7.771334 0.828260 16.314210 7.771334 0.880953 15.913964 7.771334 1.046740 16.314210 7.771334 1.046740 15.935790 7.771334 0.880953 16.336036 7.771334 0.937500 15.906520 7.771334 0.994047 16.336036 7.771334 0.994047 15.913964 7.771334 0.937500 16.343480 + + + + + + + + + + 0.000000 0.000000 -1.000000 0.894427 0.000000 -0.447214 0.000000 0.894427 -0.447214 -0.894427 0.000000 -0.447214 -0.000000 -0.894427 -0.447214 1.000000 0.000000 0.000000 -0.000000 1.000000 -0.000000 0.000000 -1.000000 0.000000 -1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 + + + + + + + + + + -0.003906 0.098053 -0.003906 0.005768 -0.066406 0.098053 -0.066406 -0.002579 -0.070313 0.010318 -0.003906 -0.002579 -0.066406 0.005768 -0.003906 0.043851 -0.066406 0.043851 0.000000 0.056748 0.000000 0.010318 0.066406 0.043851 0.003906 0.043851 0.070313 0.056748 0.066406 -0.002579 0.003906 -0.002579 0.070313 0.010318 -0.070313 0.056748 0.000000 0.011536 -0.070313 0.011536 -0.021501 1.488098 0.000000 0.011536 -0.070312 0.011536 0.000000 1.580383 0.070313 0.011536 0.070313 1.580383 -0.021967 1.482880 0.000000 1.580383 -0.070312 1.580383 -0.070313 1.580383 -0.048811 1.488098 -0.023331 1.478017 -0.021967 1.493317 0.070313 0.103821 0.070313 0.000000 0.000000 0.103821 -0.048346 1.493317 -0.048346 1.482880 0.000000 0.000000 -0.025501 1.473841 -0.023331 1.498179 -0.046982 1.498179 -0.046982 1.478017 -0.025501 1.502355 -0.028329 1.470637 -0.044812 1.502355 -0.044812 1.473841 -0.028329 1.505559 -0.031622 1.468623 -0.041984 1.505559 -0.041984 1.470637 -0.031622 1.507574 -0.035156 1.467936 -0.038690 1.507574 -0.038690 1.468623 -0.035156 1.508261 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 0 1 3 3 1 4 1 1 5 4 0 6 2 0 2 1 0 1 0 2 7 2 2 8 3 2 9 5 1 10 1 1 5 3 1 4 2 3 11 4 3 12 6 3 13 4 4 14 1 4 15 7 4 16 6 2 17 3 2 9 2 2 8 5 4 10 7 4 16 1 4 15 5 5 18 3 5 19 8 5 20 7 3 9 6 3 13 4 3 12 3 6 21 6 6 22 9 6 23 7 7 24 5 7 18 10 7 25 11 5 26 8 5 20 3 5 19 5 5 18 8 5 20 12 5 27 7 8 18 10 8 27 6 8 24 13 6 28 9 6 23 6 6 22 9 5 29 14 5 30 3 5 19 12 7 27 10 7 25 5 7 18 15 5 31 11 5 26 3 5 19 16 5 32 12 5 27 8 5 20 13 8 25 6 8 24 10 8 27 13 9 33 10 9 34 9 9 35 14 5 30 9 5 29 17 5 36 14 5 30 18 5 37 3 5 19 12 9 38 9 9 35 10 9 34 19 5 39 15 5 31 3 5 19 20 5 40 12 5 27 16 5 32 17 5 36 9 5 29 21 5 41 18 5 37 22 5 42 3 5 19 9 5 29 12 5 27 23 5 43 24 5 44 19 5 39 3 5 19 23 5 43 12 5 27 20 5 40 21 5 41 9 5 29 25 5 45 22 5 42 26 5 46 3 5 19 27 5 47 9 5 29 23 5 43 28 5 48 24 5 44 3 5 19 25 5 45 9 5 29 29 5 49 26 5 46 30 5 50 3 5 19 31 5 51 9 5 29 27 5 47 32 5 52 28 5 48 3 5 19 29 5 49 9 5 29 33 5 53 30 5 50 34 5 54 3 5 19 35 5 55 9 5 29 31 5 51 34 5 54 32 5 52 3 5 19 33 5 53 9 5 29 35 5 55

+ + + + + + + 6.197745 18.253172 34.332811 6.197745 18.159422 34.480813 6.197745 18.159422 34.379686 6.197745 18.253172 34.527688 6.291495 18.159422 34.379686 6.197745 18.346922 34.379686 6.291495 18.253172 34.527688 6.291495 18.159422 34.480813 6.291495 18.253172 34.332811 6.197745 18.346922 34.480813 6.291495 18.346922 34.480813 6.291495 18.143932 34.619459 6.291495 18.196625 34.219214 6.291495 18.346922 34.379686 6.291495 18.196625 34.641286 6.291495 18.253172 34.648730 6.291495 18.143932 34.241040 6.291495 18.253172 34.211769 6.291495 18.309719 34.219214 6.291495 18.309719 34.641286 6.291495 18.098683 34.584739 6.291495 18.362412 34.241040 6.291495 18.362412 34.619459 6.291495 18.098683 34.275761 6.291495 18.063962 34.539490 6.291495 18.407661 34.275761 6.291495 18.063962 34.321010 6.291495 18.407661 34.584739 6.291495 18.042136 34.486797 6.291495 18.442381 34.321010 6.291495 18.042136 34.373703 6.291495 18.442381 34.539490 6.291495 18.034692 34.430250 6.291495 18.464208 34.373703 6.291495 18.464208 34.486797 6.291495 18.471652 34.430250 + + + + + + + + + + 1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.447214 0.894427 0.000000 0.447214 -0.894427 0.000000 -0.447214 0.894427 0.000000 -0.447214 -0.894427 0.000000 -1.000000 0.000000 + + + + + + + + + + 0.250000 0.152561 0.156250 0.300564 0.156250 0.199436 0.250000 0.347439 0.300564 -0.093750 0.199436 0.000000 0.199436 -0.093750 -0.050564 0.000000 -0.155379 -0.093750 -0.050564 -0.093750 0.343750 0.199436 0.378986 0.000000 0.274170 -0.093750 0.378986 -0.093750 0.300564 0.000000 -0.155379 0.000000 -0.291834 0.000000 -0.396650 -0.093750 -0.291834 -0.093750 0.343750 0.300564 0.274170 0.000000 0.068228 -0.093750 0.173043 0.000000 0.068228 0.000000 0.140760 0.439210 0.193453 0.038964 -0.396650 0.000000 -0.199436 -0.093750 -0.300564 0.000000 -0.300564 -0.093750 0.173043 -0.093750 0.193453 0.461036 0.250000 0.468480 0.140760 0.060790 0.250000 0.031520 0.306547 0.038964 -0.199436 0.000000 0.306547 0.461036 0.095511 0.404489 0.359240 0.060790 0.359240 0.439210 0.095511 0.095511 0.060790 0.359240 0.404489 0.095511 0.060790 0.140760 0.404489 0.404489 0.038964 0.306547 0.439210 0.140760 0.038964 0.193453 0.439210 0.359240 0.031520 0.250000 0.461036 0.193453 0.461036 0.306547 0.468480 0.250000 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 1 1 4 4 1 5 2 1 6 4 2 7 0 2 8 2 2 9 3 0 3 0 0 0 5 0 10 6 3 11 1 3 12 3 3 13 4 1 5 1 1 4 7 1 14 0 2 8 4 2 7 8 2 15 8 4 16 5 4 17 0 4 18 3 0 3 5 0 10 9 0 19 1 3 12 6 3 11 7 3 20 3 5 21 10 5 22 6 5 23 7 0 1 11 0 24 4 0 2 4 0 2 12 0 25 8 0 0 5 4 17 8 4 16 13 4 26 5 6 27 10 6 28 9 6 29 10 5 22 3 5 21 9 5 30 14 0 31 7 0 1 6 0 3 15 0 32 6 0 3 10 0 19 11 0 24 7 0 1 14 0 31 11 0 24 16 0 33 4 0 2 4 0 2 16 0 33 12 0 25 8 0 0 12 0 25 17 0 34 8 0 0 18 0 35 13 0 10 10 6 28 5 6 27 13 6 36 14 0 31 6 0 3 15 0 32 15 0 32 10 0 19 19 0 37 20 0 38 16 0 33 11 0 24 8 0 0 17 0 34 18 0 35 13 0 10 18 0 35 21 0 39 13 0 10 21 0 39 10 0 19 19 0 37 10 0 19 22 0 40 20 0 38 23 0 41 16 0 33 22 0 40 10 0 19 21 0 39 24 0 42 23 0 41 20 0 38 22 0 40 21 0 39 25 0 43 24 0 42 26 0 44 23 0 41 22 0 40 25 0 43 27 0 45 28 0 46 26 0 44 24 0 42 27 0 45 25 0 43 29 0 47 28 0 46 30 0 48 26 0 44 27 0 45 29 0 47 31 0 49 30 0 48 28 0 46 32 0 50 31 0 49 29 0 47 33 0 51 31 0 49 33 0 51 34 0 52 34 0 52 33 0 51 35 0 53

+ + + + + + + 6.197745 15.619749 16.027561 6.197745 15.525999 16.175564 6.197745 15.525999 16.074436 6.197745 15.619749 16.222439 6.291495 15.525999 16.074436 6.197745 15.713499 16.074436 6.291495 15.619749 16.222439 6.291495 15.525999 16.175564 6.291495 15.619749 16.027561 6.197745 15.713499 16.175564 6.291495 15.713499 16.175564 6.291495 15.510509 16.314210 6.291495 15.563202 15.913964 6.291495 15.713499 16.074436 6.291495 15.563202 16.336036 6.291495 15.619749 16.343480 6.291495 15.510509 15.935790 6.291495 15.619749 15.906520 6.291495 15.676296 15.913964 6.291495 15.676296 16.336036 6.291495 15.465260 16.279489 6.291495 15.728989 15.935790 6.291495 15.728989 16.314210 6.291495 15.465260 15.970511 6.291495 15.430540 16.234240 6.291495 15.774238 15.970511 6.291495 15.430540 16.015760 6.291495 15.774238 16.279489 6.291495 15.408713 16.181547 6.291495 15.808959 16.015760 6.291495 15.408713 16.068453 6.291495 15.808959 16.234240 6.291495 15.401269 16.125000 6.291495 15.830785 16.068453 6.291495 15.830785 16.181547 6.291495 15.838230 16.125000 + + + + + + + + + + 1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.447214 0.894427 0.000000 0.447214 -0.894427 0.000000 -0.447214 0.894427 0.000000 -0.447214 -0.894427 0.000000 -1.000000 0.000000 + + + + + + + + + + 0.250000 0.152561 0.156250 0.300564 0.156250 0.199436 0.250000 0.347439 0.300564 -0.093750 0.199436 0.000000 0.199436 -0.093750 -0.050564 0.000000 -0.155379 -0.093750 -0.050564 -0.093750 0.343750 0.199436 0.378986 0.000000 0.274170 -0.093750 0.378986 -0.093750 0.300564 0.000000 -0.155379 0.000000 -0.291834 0.000000 -0.396650 -0.093750 -0.291834 -0.093750 0.343750 0.300564 0.274170 0.000000 0.068228 -0.093750 0.173043 0.000000 0.068228 0.000000 0.140760 0.439210 0.193453 0.038964 -0.396650 0.000000 -0.199436 -0.093750 -0.300564 0.000000 -0.300564 -0.093750 0.173043 -0.093750 0.193453 0.461036 0.250000 0.468480 0.140760 0.060790 0.250000 0.031520 0.306547 0.038964 -0.199436 0.000000 0.306547 0.461036 0.095511 0.404489 0.359240 0.060790 0.359240 0.439210 0.095511 0.095511 0.060790 0.359240 0.404489 0.095511 0.060790 0.140760 0.404489 0.404489 0.038964 0.306547 0.439210 0.140760 0.038964 0.193453 0.439210 0.359240 0.031520 0.250000 0.461036 0.193453 0.461036 0.306547 0.468480 0.250000 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 1 1 4 4 1 5 2 1 6 4 2 7 0 2 8 2 2 9 3 0 3 0 0 0 5 0 10 6 3 11 1 3 12 3 3 13 4 1 5 1 1 4 7 1 14 0 2 8 4 2 7 8 2 15 8 4 16 5 4 17 0 4 18 3 0 3 5 0 10 9 0 19 1 3 12 6 3 11 7 3 20 3 5 21 10 5 22 6 5 23 7 0 1 11 0 24 4 0 2 4 0 2 12 0 25 8 0 0 5 4 17 8 4 16 13 4 26 5 6 27 10 6 28 9 6 29 10 5 22 3 5 21 9 5 30 14 0 31 7 0 1 6 0 3 15 0 32 6 0 3 10 0 19 11 0 24 7 0 1 14 0 31 11 0 24 16 0 33 4 0 2 4 0 2 16 0 33 12 0 25 8 0 0 12 0 25 17 0 34 8 0 0 18 0 35 13 0 10 10 6 28 5 6 27 13 6 36 14 0 31 6 0 3 15 0 32 15 0 32 10 0 19 19 0 37 20 0 38 16 0 33 11 0 24 8 0 0 17 0 34 18 0 35 13 0 10 18 0 35 21 0 39 13 0 10 21 0 39 10 0 19 19 0 37 10 0 19 22 0 40 20 0 38 23 0 41 16 0 33 22 0 40 10 0 19 21 0 39 24 0 42 23 0 41 20 0 38 22 0 40 21 0 39 25 0 43 24 0 42 26 0 44 23 0 41 22 0 40 25 0 43 27 0 45 28 0 46 26 0 44 24 0 42 27 0 45 25 0 43 29 0 47 28 0 46 30 0 48 26 0 44 27 0 45 29 0 47 31 0 49 30 0 48 28 0 46 32 0 50 31 0 49 29 0 47 33 0 51 31 0 49 33 0 51 34 0 52 34 0 52 33 0 51 35 0 53

+ + + + + + + 5.166495 15.421569 0.125000 5.166495 16.546569 0.125000 5.228995 15.484069 0.000000 5.166495 16.546569 17.625000 5.228995 16.484069 0.000000 6.228995 15.484069 0.000000 5.166495 14.924370 17.625000 6.291495 16.546569 17.625000 6.291495 16.546569 0.125000 6.291495 15.421569 0.125000 5.166495 14.924370 15.125000 5.166495 17.883552 35.380091 6.291495 18.993245 35.195142 6.228995 16.484069 0.000000 6.291495 14.924370 15.125000 5.166495 18.993245 35.195142 6.291495 14.924370 17.625000 6.291495 18.442381 34.321010 6.291495 15.838230 16.125000 6.291495 15.430540 16.015760 5.228995 17.962441 35.493621 6.291495 17.883552 35.380091 6.228995 18.948835 35.329222 6.291495 18.464208 34.373703 6.291495 18.407661 34.275761 6.291495 15.830785 16.068453 6.291495 15.401269 16.125000 6.291495 15.465260 15.970511 6.291495 15.408713 16.068453 5.228995 18.948835 35.329222 6.228995 17.962441 35.493621 6.291495 18.471652 34.430250 6.291495 18.362412 34.241040 6.291495 18.042136 34.373703 6.291495 15.830785 16.181547 6.291495 15.808959 16.015760 6.291495 15.510509 15.935790 6.291495 18.143932 34.619459 6.291495 18.464208 34.486797 6.291495 18.309719 34.219214 6.291495 18.034692 34.430250 6.291495 18.063962 34.321010 6.291495 15.808959 16.234240 6.291495 15.774238 15.970511 6.291495 15.408713 16.181547 6.291495 15.563202 15.913964 6.291495 18.098683 34.584739 6.291495 18.196625 34.641286 6.291495 18.442381 34.539490 6.291495 18.253172 34.211769 6.291495 18.042136 34.486797 6.291495 18.098683 34.275761 6.291495 15.774238 16.279489 6.291495 15.728989 15.935790 6.291495 15.430540 16.234240 6.291495 15.619749 15.906520 6.291495 18.063962 34.539490 6.291495 18.253172 34.648730 6.291495 18.407661 34.584739 6.291495 18.196625 34.219214 6.291495 18.143932 34.241040 6.291495 15.728989 16.314210 6.291495 15.676296 15.913964 6.291495 15.465260 16.279489 6.291495 18.309719 34.641286 6.291495 18.362412 34.619459 6.291495 15.676296 16.336036 6.291495 15.510509 16.314210 6.291495 15.619749 16.343480 6.291495 15.563202 16.336036 + + + + + + + + + + -0.894427 0.000000 -0.447214 -1.000000 -0.000000 -0.000000 -0.000000 -0.894427 -0.447214 0.000000 1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 0.894427 -0.447214 0.000000 0.990443 -0.137921 -0.000000 -0.999451 -0.033128 0.894427 0.000000 -0.447214 0.000000 -1.000000 0.000000 -0.000000 -0.986394 0.164399 1.000000 0.000000 0.000000 0.000000 0.164399 0.986394 -0.894363 0.073543 0.441255 0.000000 0.949285 0.314418 -0.000000 -0.821203 0.570636 0.894363 0.073543 0.441255 0.000000 0.164399 0.986394 + + + + + + + + + + 0.031075 0.056748 0.101387 0.056748 0.034981 0.043851 0.101387 0.011536 0.031075 0.011536 0.101387 1.626526 0.097481 0.043851 0.066406 -0.023099 0.003906 -0.023099 0.070313 -0.010202 0.000000 1.626526 -0.070313 1.626526 0.000000 1.626526 -0.070313 0.011536 -0.066406 0.051652 -0.066406 0.143937 -0.003906 0.051652 -0.070313 0.077268 0.000000 0.077268 -0.066406 0.064371 0.000000 -0.010202 0.000000 1.395813 0.184949 3.265057 -0.000000 1.631629 -0.070313 1.631629 -0.000000 3.268738 0.000000 0.011536 -0.003906 0.143937 -0.003906 0.064371 0.070313 0.010009 0.000000 0.010009 0.070313 1.395047 -0.031075 0.010318 -0.034981 -0.002579 -0.101387 0.010318 0.070313 1.395813 0.000000 1.395813 0.070313 1.626526 0.254305 3.247989 0.070313 1.604395 -0.000000 1.604395 0.070313 3.265528 -0.070313 3.268738 -0.254305 3.247989 -0.219876 3.167320 -0.101387 1.626526 -0.057116 1.488098 -0.101387 0.011536 -0.097481 -0.002579 0.000000 1.395047 -0.031636 1.478017 -0.031075 0.011536 -0.070313 0.163578 -0.070313 0.267399 0.000000 0.163578 0.178564 2.874469 0.183799 2.886145 0.247687 2.855450 0.000000 3.265528 0.000000 2.965204 -0.070313 2.965204 -0.003906 2.978238 -0.221240 3.172183 -0.217706 3.163144 -0.184949 3.265057 -0.056651 1.482880 -0.029806 1.488098 -0.033806 1.473841 -0.030271 1.482880 0.000000 0.267399 0.070313 2.837110 0.000000 2.837110 0.066406 2.849868 0.245241 2.869238 -0.066406 2.978238 -0.251003 2.914895 -0.189561 2.931802 -0.253449 2.901107 -0.221705 3.177401 -0.214878 3.159940 -0.194860 3.172183 -0.056651 1.493317 -0.055287 1.478017 -0.036634 1.470637 -0.201223 3.194862 -0.184327 2.920127 0.003906 2.849868 -0.066406 0.169655 -0.066406 0.261940 -0.003906 0.169655 -0.003906 0.261940 -0.221240 3.182619 -0.211584 3.157926 -0.194395 3.177401 -0.196225 3.167320 -0.055287 1.498179 -0.053117 1.473841 -0.030271 1.493317 -0.039927 1.468623 -0.198395 3.191658 -0.204516 3.196876 -0.219876 3.187482 -0.208050 3.157238 -0.194860 3.182619 -0.198395 3.163144 -0.053117 1.502355 -0.050289 1.470637 -0.031636 1.498179 -0.043461 1.467936 -0.196225 3.187482 -0.208050 3.197563 -0.217706 3.191658 -0.204516 3.157926 -0.201223 3.159940 -0.050289 1.505559 -0.046995 1.468623 -0.033806 1.502355 -0.211584 3.196876 -0.214878 3.194862 -0.046995 1.507574 -0.036634 1.505559 -0.043461 1.508261 -0.039927 1.507574 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 1 3 0 1 4 3 1 5 4 0 6 2 0 2 1 0 1 2 2 7 5 2 8 0 2 9 3 1 5 0 1 4 6 1 10 3 3 11 7 3 12 1 3 13 2 4 14 4 4 15 5 4 16 1 5 17 8 5 18 4 5 19 9 2 20 0 2 9 5 2 8 10 1 21 6 1 10 0 1 4 11 1 22 3 1 5 6 1 10 7 6 23 3 6 24 12 6 25 8 3 26 1 3 13 7 3 12 13 4 27 5 4 16 4 4 15 13 5 28 4 5 19 8 5 18 0 7 29 9 7 30 10 7 31 9 8 32 5 8 33 8 8 34 10 9 35 14 9 36 6 9 37 15 1 38 3 1 5 11 1 22 6 10 39 16 10 40 11 10 41 15 6 42 12 6 25 3 6 24 12 11 43 17 11 44 7 11 45 7 11 45 18 11 46 8 11 47 13 8 48 8 8 34 5 8 33 14 7 49 10 7 31 9 7 30 8 11 47 19 11 50 9 11 51 16 9 12 6 9 37 14 9 36 15 12 52 11 12 53 12 12 54 11 13 55 20 13 56 15 13 57 21 10 58 11 10 41 16 10 40 12 14 59 15 14 60 22 14 61 23 11 62 17 11 44 12 11 43 17 11 44 24 11 63 7 11 45 7 11 45 21 11 64 18 11 46 18 11 46 25 11 65 8 11 47 14 11 36 9 11 51 26 11 66 27 11 67 19 11 50 8 11 47 19 11 50 28 11 68 9 11 51 16 11 12 14 11 36 21 11 64 21 12 69 12 12 54 11 12 53 11 15 70 21 15 71 20 15 72 29 13 73 15 13 57 20 13 56 29 14 74 22 14 61 15 14 60 22 16 75 30 16 76 12 16 77 31 11 78 23 11 62 12 11 43 24 11 63 32 11 79 7 11 45 33 11 80 21 11 64 7 11 45 18 11 46 21 11 64 34 11 81 25 11 65 35 11 82 8 11 47 28 11 68 26 11 66 9 11 51 26 11 66 21 11 64 14 11 36 36 11 83 27 11 67 8 11 47 12 11 43 21 11 64 37 11 84 21 16 85 12 16 77 30 16 76 30 15 86 20 15 72 21 15 71 29 17 87 20 17 88 22 17 89 30 17 90 22 17 89 20 17 88 31 11 78 12 11 43 38 11 91 32 11 79 39 11 92 7 11 45 40 11 93 21 11 64 33 11 80 41 11 94 33 11 80 7 11 45 34 11 81 21 11 64 42 11 95 35 11 82 43 11 96 8 11 47 44 11 97 21 11 64 26 11 66 45 11 98 36 11 83 8 11 47 37 11 84 21 11 64 46 11 99 47 11 100 12 11 43 37 11 84 38 11 91 12 11 43 48 11 101 39 11 92 49 11 102 7 11 45 40 11 93 50 11 103 21 11 64 51 11 104 41 11 94 7 11 45 42 11 95 21 11 64 52 11 105 43 11 96 53 11 106 8 11 47 54 11 107 21 11 64 44 11 97 55 11 108 45 11 98 8 11 47 46 11 99 21 11 64 56 11 109 57 11 110 12 11 43 47 11 100 48 11 101 12 11 43 58 11 111 49 11 102 59 11 112 7 11 45 56 11 109 21 11 64 50 11 103 60 11 113 51 11 104 7 11 45 52 11 105 21 11 64 61 11 114 53 11 106 62 11 115 8 11 47 63 11 116 21 11 64 54 11 107 62 11 115 55 11 108 8 11 47 64 11 117 12 11 43 57 11 110 58 11 111 12 11 43 65 11 118 59 11 112 60 11 113 7 11 45 61 11 114 21 11 64 66 11 119 67 11 120 21 11 64 63 11 116 65 11 118 12 11 43 64 11 117 66 11 119 21 11 64 68 11 121 69 11 122 21 11 64 67 11 120 68 11 121 21 11 64 69 11 122

+ + + + + + + 6.646334 0.531250 17.125000 -6.646334 0.531250 15.125000 6.646334 0.531250 15.125000 -6.646334 0.531250 17.125000 6.646334 1.343750 15.125000 -6.646334 1.343750 17.125000 -6.646334 1.343750 15.125000 6.646334 1.343750 17.125000 + + + + + + + + + + 0.000000 -1.000000 0.000000 0.000000 0.000000 -1.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 -1.000000 0.000000 0.000000 0.000000 1.000000 0.000000 + + + + + + + + + + 0.830792 0.184570 0.000000 0.000000 0.830792 0.000000 0.000000 0.184570 -0.830792 0.074982 -0.830792 0.000000 0.050781 0.000000 0.000000 0.074982 -0.050781 0.184570 0.050781 0.184570 0.830792 0.074982 -0.050781 0.000000 -0.830792 0.184570 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 1 1 1 4 1 4 2 1 5 4 2 6 0 2 3 2 2 1 0 3 2 5 3 7 3 3 1 5 4 8 1 4 1 3 4 3 4 1 4 1 1 1 6 1 7 0 2 3 4 2 6 7 2 9 5 3 7 0 3 2 7 3 10 1 4 1 5 4 8 6 4 11 5 5 3 4 5 5 6 5 1 4 5 5 5 5 3 7 5 12

+ + + + + + + 5.166495 16.000000 15.125000 5.166495 15.250000 17.125000 5.166495 15.250000 15.125000 5.166495 16.000000 17.125000 -5.166495 15.250000 15.125000 -5.166495 16.000000 17.125000 -5.166495 15.250000 17.125000 -5.166495 16.000000 15.125000 + + + + + + + + + + 1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 0.000000 0.000000 -1.000000 0.000000 1.000000 0.000000 0.000000 0.000000 1.000000 -1.000000 0.000000 0.000000 + + + + + + + + + + 0.046875 0.000000 0.000000 0.184570 0.000000 0.000000 0.046875 0.184570 0.645812 0.184570 0.645812 0.000000 -0.645812 0.069214 -0.645812 0.000000 -0.645812 0.184570 0.000000 0.069214 0.645812 0.069214 -0.046875 0.184570 -0.046875 0.000000 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 1 1 4 4 1 2 2 1 5 4 2 2 0 2 6 2 2 7 0 3 7 5 3 1 3 3 8 5 4 9 1 4 5 3 4 10 4 1 2 1 1 4 6 1 1 0 2 6 4 2 2 7 2 9 5 3 1 0 3 7 7 3 2 1 4 5 5 4 9 6 4 2 5 5 11 4 5 2 6 5 1 4 5 2 5 5 11 7 5 12

+ + + + + + + 0.500000 16.873192 23.288377 -0.500000 16.379995 23.370576 -0.500000 16.873192 23.288377 0.500000 16.379995 23.370576 -0.500000 18.025013 33.240684 -0.500000 18.518210 33.158485 0.500000 18.025013 33.240684 0.500000 18.518210 33.158485 + + + + + + + + + + 0.000000 -0.164399 -0.986394 -1.000000 0.000000 0.000000 0.000000 0.986394 -0.164399 1.000000 0.000000 0.000000 0.000000 -0.986394 0.164399 0.000000 0.164399 0.986394 + + + + + + + + + + 0.062500 -0.044895 0.000000 0.001247 0.000000 -0.044895 0.062500 0.001247 -0.102814 0.918450 -0.030825 0.000000 0.000000 0.007586 0.000000 0.930911 -0.062500 0.007483 0.000000 0.007483 0.030825 0.000000 0.102814 0.918450 0.062500 0.930911 0.062500 0.007483 -0.133638 0.910864 0.000000 0.930911 -0.062500 0.930911 0.133638 0.910864 -0.062500 0.001247 0.000000 -0.044895 0.000000 0.001247 -0.062500 -0.044895 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 4 1 4 2 1 5 1 1 6 5 2 7 0 2 8 2 2 9 0 3 10 6 3 11 3 3 6 6 4 12 1 4 9 3 4 13 2 1 5 4 1 4 5 1 14 1 4 9 6 4 12 4 4 15 0 2 8 5 2 7 7 2 16 6 3 11 0 3 10 7 3 17 6 5 18 5 5 19 4 5 20 5 5 19 6 5 18 7 5 21

+ + + + + + + 5.166495 16.821817 22.029697 -5.166495 16.020372 22.163271 -5.166495 16.821817 22.029697 5.166495 16.020372 22.163271 -5.166495 16.225871 23.396264 -5.166495 17.027316 23.262690 5.166495 16.225871 23.396264 5.166495 17.027316 23.262690 + + + + + + + + + + 0.000000 -0.164399 -0.986394 -1.000000 0.000000 0.000000 0.000000 0.986394 -0.164399 1.000000 0.000000 0.000000 0.000000 -0.986394 0.164399 -0.000000 0.164399 0.986394 + + + + + + + + + + 0.645812 -0.072955 0.000000 0.002027 0.000000 -0.072955 0.645812 0.002027 -0.012844 0.126114 -0.050090 0.000000 0.000000 0.012327 0.000000 0.127516 -0.645812 0.012159 0.000000 0.012159 0.050090 -0.000000 0.012844 0.126114 -0.000000 0.012327 0.645812 0.127516 0.000000 0.012159 0.645812 0.012159 -0.062934 0.113787 -0.645812 0.127516 0.062934 0.113787 -0.645812 0.002027 -0.000000 -0.072955 -0.000000 0.002027 -0.645812 -0.072955 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 4 1 4 2 1 5 1 1 6 5 2 7 0 2 8 2 2 9 0 3 10 6 3 11 3 3 12 6 4 13 1 4 14 3 4 15 2 1 5 4 1 4 5 1 16 1 4 14 6 4 13 4 4 7 0 2 8 5 2 7 7 2 17 6 3 11 0 3 10 7 3 18 6 5 19 5 5 20 4 5 21 5 5 20 6 5 19 7 5 22

+ + + + + + + -1.750000 16.873192 23.288377 -2.750000 16.379995 23.370576 -2.750000 16.873192 23.288377 -1.750000 16.379995 23.370576 -2.750000 18.025013 33.240684 -2.750000 18.518210 33.158485 -1.750000 18.025013 33.240684 -1.750000 18.518210 33.158485 + + + + + + + + + + 0.000000 -0.164399 -0.986394 -1.000000 0.000000 0.000000 0.000000 0.986394 -0.164399 1.000000 0.000000 0.000000 0.000000 -0.986394 0.164399 0.000000 0.164399 0.986394 + + + + + + + + + + 0.062500 -0.044895 0.000000 0.001247 0.000000 -0.044895 0.062500 0.001247 -0.102814 0.918450 -0.030825 0.000000 0.000000 0.007586 0.000000 0.930911 -0.062500 0.007483 0.000000 0.007483 0.030825 0.000000 0.102814 0.918450 0.062500 0.930911 0.062500 0.007483 -0.133638 0.910864 0.000000 0.930911 -0.062500 0.930911 0.133638 0.910864 -0.062500 0.001247 0.000000 -0.044895 0.000000 0.001247 -0.062500 -0.044895 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 4 1 4 2 1 5 1 1 6 5 2 7 0 2 8 2 2 9 0 3 10 6 3 11 3 3 6 6 4 12 1 4 9 3 4 13 2 1 5 4 1 4 5 1 14 1 4 9 6 4 12 4 4 15 0 2 8 5 2 7 7 2 16 6 3 11 0 3 10 7 3 17 6 5 18 5 5 19 4 5 20 5 5 19 6 5 18 7 5 21

+ + + + + + + 2.750000 16.873192 23.288377 1.750000 16.379995 23.370576 1.750000 16.873192 23.288377 2.750000 16.379995 23.370576 1.750000 18.025013 33.240684 1.750000 18.518210 33.158485 2.750000 18.025013 33.240684 2.750000 18.518210 33.158485 + + + + + + + + + + 0.000000 -0.164399 -0.986394 -1.000000 0.000000 0.000000 0.000000 0.986394 -0.164399 1.000000 0.000000 0.000000 0.000000 -0.986394 0.164399 0.000000 0.164399 0.986394 + + + + + + + + + + 0.062500 -0.044895 0.000000 0.001247 0.000000 -0.044895 0.062500 0.001247 -0.102814 0.918450 -0.030825 0.000000 0.000000 0.007586 0.000000 0.930911 -0.062500 0.007483 0.000000 0.007483 0.030825 0.000000 0.102814 0.918450 0.062500 0.930911 0.062500 0.007483 -0.133638 0.910864 0.000000 0.930911 -0.062500 0.930911 0.133638 0.910864 -0.062500 0.001247 0.000000 -0.044895 0.000000 0.001247 -0.062500 -0.044895 + + + + + + + + + + + + + + +

0 0 0 1 0 1 2 0 2 1 0 1 0 0 0 3 0 3 4 1 4 2 1 5 1 1 6 5 2 7 0 2 8 2 2 9 0 3 10 6 3 11 3 3 6 6 4 12 1 4 9 3 4 13 2 1 5 4 1 4 5 1 14 1 4 9 6 4 12 4 4 15 0 2 8 5 2 7 7 2 16 6 3 11 0 3 10 7 3 17 6 5 18 5 5 19 4 5 20 5 5 19 6 5 18 7 5 21

+ + + + + + + -1.748303 18.945036 35.112942 -0.868987 18.638695 33.140154 -1.737748 18.617688 33.140154 -0.874265 18.966170 35.112942 -1.713998 17.881157 33.263454 -2.621887 18.909818 35.112942 -1.724553 18.208504 35.236242 0.000000 18.645698 33.140154 -0.857110 17.901876 33.263454 -2.570440 17.846630 33.263454 -2.606057 18.582683 33.140154 -2.586270 18.173765 35.236242 -0.862389 18.229351 35.236242 0.000000 18.973216 35.112942 -3.494789 18.860526 35.112942 -3.447315 18.125142 35.236242 0.868987 18.638695 33.140154 0.000000 17.908783 33.263454 0.000000 18.236301 35.236242 -3.426215 17.798305 33.263454 -3.473690 18.533688 33.140154 0.874265 18.966170 35.112942 0.857110 17.901876 33.263454 -4.366784 18.797172 35.112942 -4.307464 18.062649 35.236242 -4.281100 17.736194 33.263454 0.862389 18.229351 35.236242 1.737748 18.617688 33.140154 1.713998 17.881157 33.263454 -4.340420 18.470717 33.140154 1.724553 18.208504 35.236242 1.748303 18.945036 35.112942 -5.166495 18.726097 35.112942 -5.137502 17.660080 33.263454 2.606057 18.582683 33.140154 2.570440 17.846630 33.263454 2.586270 18.173765 35.236242 -5.166495 18.397299 33.140154 -5.166495 17.986301 35.236242 2.621887 18.909818 35.112942 -5.166495 17.986301 33.208654 3.473690 18.533688 33.140154 3.426215 17.798305 33.263454 3.447315 18.125142 35.236242 3.494789 18.860526 35.112942 4.340420 18.470717 33.140154 4.281100 17.736194 33.263454 4.307464 18.062649 35.236242 4.366784 18.797172 35.112942 5.166495 18.397299 33.140154 5.137502 17.660080 33.263454 5.166495 17.986301 35.236242 5.166495 18.726097 35.112942 5.166495 17.986301 33.208654 + + + + + + + + + + -0.031793 0.985985 -0.163776 -0.015899 0.986369 -0.163776 0.002660 -0.165003 -0.986290 0.005318 -0.164938 -0.986290 -0.047680 0.985345 -0.163776 -0.005318 0.164938 0.986290 -0.002660 0.165003 0.986290 0.000000 0.986498 -0.163776 0.007976 -0.164831 -0.986290 -0.007976 0.164831 0.986290 -0.000000 -0.165024 -0.986290 0.015899 -0.986369 0.163776 0.031793 -0.985985 0.163776 0.047680 -0.985345 0.163776 -0.063553 0.984448 -0.163776 -0.010631 0.164681 0.986290 0.000000 0.165024 0.986290 0.015899 0.986369 -0.163776 -0.000000 -0.986498 0.163776 0.010631 -0.164681 -0.986290 0.063553 -0.984448 0.163776 -0.002660 -0.165003 -0.986290 -0.079411 0.983296 -0.163776 -0.013284 0.164489 0.986290 0.079411 -0.983296 0.163776 0.002660 0.165003 0.986290 0.031793 0.985985 -0.163776 -0.005318 -0.164938 -0.986290 -0.015899 -0.986369 0.163776 0.013284 -0.164489 -0.986290 0.005318 0.164938 0.986290 -0.031793 -0.985985 0.163776 -0.087332 0.982625 -0.163771 -0.014610 0.164381 0.986289 0.087332 -0.982625 0.163771 0.047680 0.985345 -0.163776 -0.007976 -0.164831 -0.986290 -0.047680 -0.985345 0.163776 0.014610 -0.164381 -0.986289 0.007976 0.164831 0.986290 -1.000000 0.000000 0.000000 -0.996074 -0.088527 -0.000000 0.063553 0.984448 -0.163776 -0.010631 -0.164681 -0.986290 -0.063553 -0.984448 0.163776 0.010631 0.164681 0.986290 0.079411 0.983296 -0.163776 -0.013284 -0.164489 -0.986290 -0.079411 -0.983296 0.163776 0.013284 0.164489 0.986290 0.087332 0.982625 -0.163771 -0.014610 -0.164381 -0.986289 -0.087332 -0.982625 0.163771 0.014610 0.164381 0.986289 1.000000 0.000000 -0.000000 0.996074 -0.088527 -0.000000 + + + + + + + + + + -0.219592 0.197743 -0.274070 0.013192 -0.219757 0.013192 -0.274235 0.197743 0.274070 -0.079448 0.220128 -0.010499 0.219757 -0.079448 -0.220608 0.012334 -0.166129 0.196885 -0.220772 0.196885 -0.219963 -0.010263 -0.274235 -0.079213 -0.219592 -0.079213 -0.272991 0.198809 -0.327469 0.014259 -0.273155 0.014259 0.273699 -0.010499 0.166665 -0.005331 0.220608 -0.074280 0.220236 -0.005331 -0.166294 0.012334 -0.166500 -0.005095 -0.220772 -0.074045 -0.166129 -0.074045 -0.273864 -0.010263 -0.220401 -0.005095 -0.327634 0.198809 0.327469 -0.085871 0.273527 -0.016921 0.273155 -0.085871 0.273864 0.197831 0.220128 0.013280 0.273699 0.013280 0.166294 -0.074280 0.220401 0.196973 0.166665 0.012422 0.220236 0.012422 -0.167095 0.011684 -0.112616 0.196235 -0.167259 0.196235 -0.167259 -0.070132 -0.112987 -0.001183 -0.166888 -0.001183 0.219963 0.197831 -0.273362 -0.016686 -0.327634 -0.085635 -0.272991 -0.085635 0.166500 0.196973 -0.326311 0.200084 -0.380790 0.015533 -0.326476 0.015533 0.327098 -0.016921 0.273527 0.014347 0.327263 0.198897 0.273362 0.198897 0.113152 -0.001418 0.167095 -0.070368 0.166723 -0.001418 -0.112781 0.011684 -0.112616 -0.070132 0.113152 0.011772 0.166888 0.196323 0.112987 0.196323 -0.327263 -0.016686 0.166723 0.011772 -0.380954 0.200084 0.380418 -0.024596 0.326476 -0.093545 0.380790 -0.093545 0.326847 -0.024596 0.327098 0.014347 0.112781 -0.070368 -0.113545 0.011243 -0.059067 0.195794 -0.113710 0.195794 -0.113710 -0.067476 -0.059438 0.001474 -0.113339 0.001474 0.113339 0.195882 0.059603 0.011331 0.113174 0.011331 -0.380583 -0.024360 -0.326311 -0.093310 -0.326682 -0.024360 -0.380954 -0.093310 -0.379540 0.201566 -0.434018 0.017015 -0.379705 0.017015 0.379705 -0.102470 0.433647 -0.033521 0.380076 -0.033521 0.380583 0.200172 0.326847 0.015621 0.380418 0.015621 0.326682 0.200172 0.059603 0.001238 0.113545 -0.067711 0.113174 0.001238 -0.059232 0.011243 -0.059067 -0.067476 0.059438 0.195882 -0.433812 -0.033285 -0.379540 -0.102235 -0.379911 -0.033285 -0.434183 0.201566 0.434018 -0.102470 0.433812 0.201654 0.380076 0.017103 0.433647 0.017103 0.379911 0.201654 0.059232 -0.067711 -0.059973 0.011011 -0.009959 0.195562 -0.060138 0.195562 -0.009959 -0.066077 -0.059767 0.002873 -0.060138 -0.066077 0.059767 0.195650 0.005866 0.011099 0.059602 0.011099 -0.434183 -0.102235 -0.432663 0.203255 -0.487141 0.018704 -0.432828 0.018704 0.432828 -0.112644 0.486770 -0.043694 0.433199 -0.043694 0.433199 0.018792 0.486935 0.203343 0.433034 0.203343 0.005866 0.002637 0.059973 -0.066312 0.059602 0.002637 -0.008140 0.011011 -0.005866 0.002873 0.005866 0.195650 -0.486935 -0.043459 -0.432663 -0.112408 -0.433034 -0.043459 -0.487306 0.203255 0.487141 -0.112644 0.486770 0.018792 0.005866 -0.028007 0.008140 -0.066312 -0.020389 0.006321 -0.066626 0.182059 -0.046076 0.000000 -0.020389 0.193438 -0.019948 0.006321 0.000521 0.011379 -0.019948 0.193438 -0.487306 -0.112408 -0.485667 0.205151 -0.540145 0.020600 -0.485832 0.020600 0.485832 -0.124062 0.539774 -0.055113 0.486203 -0.055113 0.486203 0.020688 0.539939 0.205239 0.486038 0.205239 -0.539939 -0.054877 -0.485667 -0.123827 -0.486038 -0.054877 -0.540310 0.205151 0.540145 -0.124062 0.539774 0.020688 -0.540310 -0.123827 -0.538537 0.207253 -0.593015 0.022703 -0.538702 0.022703 0.538702 -0.136724 0.592644 -0.067774 0.539073 -0.067774 0.539073 0.022791 0.592809 0.207341 0.538908 0.207341 -0.592809 -0.067539 -0.538537 -0.136488 -0.538908 -0.067539 -0.593180 0.207253 0.593015 -0.136724 0.592644 0.022791 -0.593180 -0.136488 -0.591260 0.209562 -0.643258 0.025011 -0.591425 0.025011 0.591425 -0.150624 0.645532 -0.081675 0.591796 -0.081675 0.591796 0.025099 0.645532 0.209650 0.591631 0.209650 -0.645532 -0.081439 -0.591260 -0.150389 -0.591631 -0.081439 -0.641439 0.209562 0.643258 -0.150624 0.645532 0.025099 -0.641439 -0.150389 0.066626 0.182059 0.020389 0.006321 0.046076 0.000000 0.645532 -0.112319 0.077842 0.006321 0.077842 0.193438 0.057373 0.011379 0.020389 0.193438 + + + + + + + + + + + + + + +

0 0 0 1 1 1 2 0 2 1 1 1 0 0 0 3 1 3 1 2 4 4 3 5 2 3 6 2 0 7 5 4 8 0 0 9 6 5 10 3 6 11 0 5 12 3 1 13 7 7 14 1 1 15 4 3 5 1 2 4 8 2 16 9 8 17 2 3 18 4 3 19 5 4 8 2 0 7 10 4 20 11 9 21 0 5 22 5 9 23 3 6 11 6 5 10 12 6 24 0 5 22 11 9 21 6 5 25 7 7 14 3 1 13 13 7 26 7 10 27 8 2 28 1 2 29 12 11 30 4 12 31 8 11 32 2 3 18 9 8 17 10 8 33 6 12 34 9 13 35 4 12 36 10 4 37 14 14 38 5 4 39 5 9 40 15 15 41 11 9 42 4 12 31 12 11 30 6 12 43 12 6 44 13 16 45 3 6 46 9 13 35 6 12 34 11 13 47 13 7 48 16 17 49 7 7 50 8 2 28 7 10 27 17 10 51 8 11 52 18 18 53 12 11 54 19 19 55 10 8 56 9 8 57 14 14 38 10 4 37 20 14 58 15 15 41 5 9 40 14 15 59 19 20 60 11 13 61 15 20 62 13 16 45 12 6 44 18 16 63 11 13 61 19 20 60 9 13 64 16 17 49 13 7 48 21 17 65 22 21 66 7 10 67 16 21 68 7 10 67 22 21 66 17 10 69 18 18 53 8 11 52 17 18 70 10 8 56 19 19 55 20 19 71 20 14 72 23 22 73 14 14 74 14 15 75 24 23 76 15 15 77 15 20 78 25 24 79 19 20 80 26 25 81 13 16 82 18 16 83 13 16 82 26 25 81 21 25 84 21 17 85 27 26 86 16 17 87 16 21 88 28 27 89 22 21 90 26 28 91 17 18 92 22 28 93 17 18 92 26 28 91 18 18 94 25 29 95 20 19 96 19 19 97 23 22 73 20 14 72 29 22 98 24 23 76 14 15 75 23 23 99 25 24 79 15 20 78 24 24 100 30 30 101 21 25 102 26 25 103 27 26 86 21 17 85 31 26 104 28 27 89 16 21 88 27 27 105 30 31 106 22 28 107 28 31 108 22 28 107 30 31 106 26 28 109 20 19 96 25 29 95 29 29 110 29 22 111 32 32 112 23 22 113 32 33 114 24 23 115 23 23 116 24 24 117 33 34 118 25 24 119 21 25 102 30 30 101 31 30 120 31 26 121 34 35 122 27 26 123 27 27 124 35 36 125 28 27 126 28 31 127 36 37 128 30 31 129 33 38 130 29 29 131 25 29 132 32 32 112 29 22 111 37 32 133 24 23 115 32 33 114 38 33 134 33 34 118 24 24 117 38 34 135 36 39 136 31 30 137 30 30 138 34 35 122 31 26 121 39 35 139 35 36 125 27 27 124 34 36 140 36 37 128 28 31 127 35 37 141 40 38 142 29 29 131 33 38 130 29 29 131 40 38 142 37 38 143 40 40 144 32 40 145 37 40 146 32 40 145 40 40 144 38 40 147 40 41 148 33 41 149 38 41 150 31 30 137 36 39 136 39 39 151 39 35 152 41 42 153 34 35 154 34 36 155 42 43 156 35 36 157 35 37 158 43 44 159 36 37 160 43 45 161 39 39 162 36 39 163 41 42 153 39 35 152 44 42 164 42 43 156 34 36 155 41 43 165 43 44 159 35 37 158 42 44 166 39 39 162 43 45 161 44 45 167 44 42 168 45 46 169 41 42 170 41 43 171 46 47 172 42 43 173 42 44 174 47 48 175 43 44 176 47 49 177 44 45 178 43 45 179 45 46 169 44 42 168 48 46 180 46 47 172 41 43 171 45 47 181 47 48 175 42 44 174 46 48 182 44 45 178 47 49 177 48 49 183 48 46 184 49 50 185 45 46 186 45 47 187 50 51 188 46 47 189 46 48 190 51 52 191 47 48 192 51 53 193 48 49 194 47 49 195 49 50 185 48 46 184 52 50 196 50 51 188 45 47 187 49 51 197 51 52 191 46 48 190 50 52 198 48 49 194 51 53 193 52 53 199 52 54 200 53 54 201 49 54 202 50 51 188 49 51 197 53 51 203 53 55 204 51 55 205 50 55 206 53 54 201 52 54 200 51 54 207

+ + + + + + + + + + 40.000000 + 30.000000 + 1.000000 + 1000.000000 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.768539 -0.273151 0.578564 47.734226 + 0.639803 0.328112 -0.694978 -48.167746 + -0.000000 0.904285 0.426929 52.003292 + 0.000000 0.000000 0.000000 1.000000 + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/models/Chair.stl b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/models/Chair.stl new file mode 100755 index 0000000..1db2443 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/models/Chair.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/textures.txt b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/textures.txt new file mode 100755 index 0000000..6d7be71 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/Media/models/chair/textures.txt @@ -0,0 +1,2 @@ +<../images/texture0.jpg> <../images/texture0.jpg> +<../images/texture1.jpg> <../images/texture1.jpg> diff --git a/gazebo_ros_pkgs/gazebo_plugins/cfg/CameraSynchronizer.cfg b/gazebo_ros_pkgs/gazebo_plugins/cfg/CameraSynchronizer.cfg new file mode 100755 index 0000000..da4fae6 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/cfg/CameraSynchronizer.cfg @@ -0,0 +1,92 @@ +#! /usr/bin/env python +#* +#* Copyright (c) 2009, Willow Garage, Inc. +#* All rights reserved. +#* +#* 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 the Willow Garage 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: Blaise Gassend + +# WGE100 camera configuration + +PACKAGE='gazebo_plugins' +#import roslib; roslib.load_manifest(PACKAGE) + +#from dynamic_reconfigure.parameter_generator import * +#from gazebo_plugins.levels import * # Get the levels +from dynamic_reconfigure.parameter_generator_catkin import * + +#copy from levels.py. Leaving levels.py in gazebo_plugins/src results in compile error. +lvl_narrow = 1 +lvl_wide = 2 +lvl_forearm_r = 4 +lvl_forearm_l = 8 +lvl_projector = 16 +lvl_stereo = lvl_wide | lvl_narrow +lvl_all = lvl_stereo | lvl_forearm_r | lvl_forearm_l | lvl_projector + +gen = ParameterGenerator() + +p_off = gen.const("ProjectorOff", int_t, 1, "The projector is always off.") +p_auto = gen.const("ProjectorAuto", int_t, 2, "The projector is on if one of the cameras is using it.") +p_on = gen.const("ProjectorOn", int_t, 3, "The projector is always on.") +projector_mode_enum = gen.enum([ p_off, p_auto, p_on ], "The projectors operating mode.") + +t_int = gen.const("InternalTrigger", int_t, 1, "The camera does not use the trigger input.") +t_ign = gen.const("IgnoreProjector", int_t, 2, "The cameras frequency can be set independently of the projector frequency. There is no deterministic phase relation between projector firing and camera triggering.") +t_proj = gen.const("WithProjector", int_t, 3, "The camera always exposes while the projector is on.") +t_noproj = gen.const("WithoutProjector", int_t, 4, "The camera always exposes while the projector is off.") +t_alt = gen.const("AlternateProjector", int_t, 5, "The camera alternates between frames with and without the projector.") +wide_trig_mode_enum = gen.enum([ t_ign, t_proj, t_noproj ], "The triggering mode for the wide camera.") +narrow_trig_mode_enum = gen.enum([ t_ign, t_proj, t_noproj, t_alt ], "The triggering mode for the narrow camera.") +forearm_trig_mode_enum = gen.enum([ t_int, t_ign, t_proj, t_noproj ], "The triggering mode for a forearm camera.") + +# Name Type Reconf level Description Default Min Max +gen.add("projector_rate", double_t, lvl_all, "Projector pulse frequency in Hz.", 60.0, 40.0, 120.0) +gen.add("projector_pulse_length", double_t, lvl_all, "Length of the projector pulses in s. At high currents the hardware may limit the pulse length.", 0.002, 0.001,0.002) +gen.add("projector_pulse_shift", double_t, lvl_all, "How far off-center the intermediate projector pulses are. Zero is on-center, one is touching the following pulse.", 0.0, 0.0, 1.0) +gen.add("projector_mode", int_t, lvl_all, "Indicates whether the projector should be off, on when in use or on all the time.", 2, 1, 3, edit_method = projector_mode_enum) +gen.add("prosilica_projector_inhibit", bool_t, lvl_projector, "Indicates if the projector should turn off when the prosilica camera is exposing.", False) + +gen.add("stereo_rate", double_t, lvl_stereo, "Indicates the frame rate for both stereo cameras in Hz. (Gets rounded to suitable divisors of projector_rate.)", 30.0, 1.0, 60.0) + +gen.add("wide_stereo_trig_mode", int_t, lvl_stereo, "Indicates the triggering mode of the wide stereo camera.", 4, 2, 4, edit_method = wide_trig_mode_enum) +gen.add("narrow_stereo_trig_mode", int_t, lvl_stereo, "Indicates the triggering mode of the narrow stereo camera.", 4, 2, 5, edit_method = narrow_trig_mode_enum) + +gen.add("forearm_r_rate", double_t, lvl_forearm_r, "Indicates the frame rate for the right forearm camera in Hz. (Gets rounded to suitable divisors of projector_rate.)", 30.0, 1.0, 60.0) +gen.add("forearm_r_trig_mode", int_t, lvl_forearm_r, "Indicates the triggering mode of the right forearm camera.", 1, 1, 4, edit_method = forearm_trig_mode_enum) + +gen.add("forearm_l_rate", double_t, lvl_forearm_l, "Indicates the frame rate for the left forearm camera in Hz. (Gets rounded to suitable divisors of projector_rate.)", 30.0, 1.0, 60.0) +gen.add("forearm_l_trig_mode", int_t, lvl_forearm_l, "Indicates the triggering mode of the left forearm camera.", 1, 1, 4, edit_method = forearm_trig_mode_enum) + +gen.add("projector_tweak", double_t, lvl_all, "Adds a time shift in seconds to the projector timing. Useful for debugging but not in normal use.", 0, -0.1, 0.1) + +gen.add("camera_reset", bool_t, lvl_all, "Does a hard reset of all the cameras using a long pulse on the trigger line. This parameter resets itself to false after 3 to 4 seconds.", False) + +exit(gen.generate(PACKAGE, "camera_synchronizer", "CameraSynchronizer")) diff --git a/gazebo_ros_pkgs/gazebo_plugins/cfg/GazeboRosCamera.cfg b/gazebo_ros_pkgs/gazebo_plugins/cfg/GazeboRosCamera.cfg new file mode 100755 index 0000000..49ef799 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/cfg/GazeboRosCamera.cfg @@ -0,0 +1,13 @@ +#!/usr/bin/env python + +PKG = 'gazebo_plugins' +#import roslib; roslib.load_manifest(PKG) + +from dynamic_reconfigure.msg import SensorLevels +#from dynamic_reconfigure.parameter_generator import * +from dynamic_reconfigure.parameter_generator_catkin import * +gen = ParameterGenerator() +gen.add("imager_rate", double_t, SensorLevels.RECONFIGURE_CLOSE, \ + "Sets the frame rate of the imager. In externally triggered mode this must be more than trig_rate", 2, 1, 50) + +exit(gen.generate(PKG, "gazebo_ros_camera", "GazeboRosCamera")) diff --git a/gazebo_ros_pkgs/gazebo_plugins/cfg/GazeboRosOpenniKinect.cfg b/gazebo_ros_pkgs/gazebo_plugins/cfg/GazeboRosOpenniKinect.cfg new file mode 100755 index 0000000..3555ae9 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/cfg/GazeboRosOpenniKinect.cfg @@ -0,0 +1,13 @@ +#!/usr/bin/env python + +PKG = 'gazebo_plugins' +#import roslib; roslib.load_manifest(PKG) + +from dynamic_reconfigure.msg import SensorLevels +#from dynamic_reconfigure.parameter_generator import * +from dynamic_reconfigure.parameter_generator_catkin import * +gen = ParameterGenerator() +gen.add("imager_rate", double_t, SensorLevels.RECONFIGURE_CLOSE, \ + "Sets the frame rate of the imager. In externally triggered mode this must be more than trig_rate", 2, 1, 50) + +exit(gen.generate(PKG, "gazebo_ros_openni_kinect", "GazeboRosOpenniKinect")) diff --git a/gazebo_ros_pkgs/gazebo_plugins/cfg/Hokuyo.cfg b/gazebo_ros_pkgs/gazebo_plugins/cfg/Hokuyo.cfg new file mode 100755 index 0000000..07375d0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/cfg/Hokuyo.cfg @@ -0,0 +1,58 @@ +#! /usr/bin/env python +#* All rights reserved. +#* +#* 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 the Willow Garage 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: Blaise Gassend + +# Forearm camera configuration + +PACKAGE='gazebo_plugins' +#import roslib; roslib.load_manifest(PACKAGE) + +from math import pi + +from dynamic_reconfigure.msg import SensorLevels +#from dynamic_reconfigure.parameter_generator import * +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() +# Name Type Reconfiguration level Description Default Min Max +gen.add("min_ang", double_t, SensorLevels.RECONFIGURE_STOP, "The angle of the first range measurement. The unit depends on ~ang_radians.", -pi/2, -pi, pi) +gen.add("max_ang", double_t, SensorLevels.RECONFIGURE_STOP, "The angle of the first range measurement. The unit depends on ~ang_radians.", pi/2, -pi, pi) +gen.add("intensity", bool_t, SensorLevels.RECONFIGURE_STOP, "Whether or not the hokuyo returns intensity values.", False) +gen.add("cluster", int_t, SensorLevels.RECONFIGURE_STOP, "The number of adjacent range measurements to cluster into a single reading", 1, 0, 99) +gen.add("skip", int_t, SensorLevels.RECONFIGURE_STOP, "The number of scans to skip between each measured scan", 0, 0, 9) +gen.add("port", str_t, SensorLevels.RECONFIGURE_CLOSE, "The serial port where the hokuyo device can be found", "/dev/ttyACM0") +gen.add("calibrate_time", bool_t, SensorLevels.RECONFIGURE_CLOSE, "Whether the node should calibrate the hokuyo's time offset", True) +gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_RUNNING, "The frame in which laser scans will be returned", "laser") +gen.add("time_offset", double_t, SensorLevels.RECONFIGURE_RUNNING, "An offet to add to the timestamp before publication of a scan", 0, -0.25, 0.25) +gen.add("allow_unsafe_settings",bool_t, SensorLevels.RECONFIGURE_CLOSE, "Turn this on if you wish to use the UTM-30LX with an unsafe angular range. Turning this option on may cause occasional crashes or bad data. This option is a tempory workaround that will hopefully be removed in an upcoming driver version.", False) + +exit(gen.generate(PACKAGE, "hokuyo_node", "Hokuyo")) diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h new file mode 100755 index 0000000..b3092d0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/MultiCameraPlugin.h @@ -0,0 +1,56 @@ +/* + * Copyright 2012 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_MULTI_CAMERA_PLUGIN_HH_ +#define _GAZEBO_MULTI_CAMERA_PLUGIN_HH_ + +#include +#include + +#include +#include +#include +#include + +namespace gazebo +{ + class MultiCameraPlugin : public SensorPlugin + { + public: MultiCameraPlugin(); + + /// \brief Destructor + public: virtual ~MultiCameraPlugin(); + + public: virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); + + public: virtual void OnNewFrameLeft(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + public: virtual void OnNewFrameRight(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + protected: sensors::MultiCameraSensorPtr parentSensor; + + protected: std::vector width, height, depth; + protected: std::vector format; + + protected: std::vector camera; + + private: std::vector newFrameConnection; + }; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/PubQueue.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/PubQueue.h new file mode 100755 index 0000000..1f8f3b2 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/PubQueue.h @@ -0,0 +1,195 @@ +/* + * Copyright 2012 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 ROS_PUBQUEUE_H +#define ROS_PUBQUEUE_H + +#include +#include +#include +#include +#include + +#include + + +/// \brief Container for a (ROS publisher, outgoing message) pair. +/// We'll have queues of these. Templated on a ROS message type. +template +class PubMessagePair +{ + public: + /// \brief The outgoing message. + T msg_; + /// \brief The publisher to use to publish the message. + ros::Publisher pub_; + PubMessagePair(T& msg, ros::Publisher& pub) : + msg_(msg), pub_(pub) {} +}; + +/// \brief A queue of outgoing messages. Instead of calling publish() directly, +/// you can push() messages here to defer ROS serialization and locking. +/// Templated on a ROS message type. +template +class PubQueue +{ + public: + typedef boost::shared_ptr > > > QueuePtr; + typedef boost::shared_ptr > Ptr; + + private: + /// \brief Our queue of outgoing messages. + QueuePtr queue_; + /// \brief Mutex to control access to the queue. + boost::shared_ptr queue_lock_; + /// \brief Function that will be called when a new message is pushed on. + boost::function notify_func_; + + public: + PubQueue(QueuePtr queue, + boost::shared_ptr queue_lock, + boost::function notify_func) : + queue_(queue), queue_lock_(queue_lock), notify_func_(notify_func) {} + ~PubQueue() {} + + /// \brief Push a new message onto the queue. + /// \param[in] msg The outgoing message + /// \param[in] pub The ROS publisher to use to publish the message + void push(T& msg, ros::Publisher& pub) + { + boost::shared_ptr > el(new PubMessagePair(msg, pub)); + boost::mutex::scoped_lock lock(*queue_lock_); + queue_->push_back(el); + notify_func_(); + } + + /// \brief Pop all waiting messages off the queue. + /// \param[out] els Place to store the popped messages + void pop(std::vector > >& els) + { + boost::mutex::scoped_lock lock(*queue_lock_); + while(!queue_->empty()) + { + els.push_back(queue_->front()); + queue_->pop_front(); + } + } +}; + +/// \brief A collection of PubQueue objects, potentially of different types. +/// This class is the programmer's interface to this queuing system. +class PubMultiQueue +{ + private: + /// \brief List of functions to be called to service our queues. + std::list > service_funcs_; + /// \brief Mutex to lock access to service_funcs_ + boost::mutex service_funcs_lock_; + /// \brief If started, the thread that will call the service functions + boost::thread service_thread_; + /// \brief Boolean flag to shutdown the service thread if PubMultiQueue is destructed + bool service_thread_running_; + /// \brief Condition variable used to block and resume service_thread_ + boost::condition_variable service_cond_var_; + /// \brief Mutex to accompany service_cond_var_ + boost::mutex service_cond_var_lock_; + + /// \brief Service a given queue by popping outgoing message off it and + /// publishing them. + template + void serviceFunc(boost::shared_ptr > pq) + { + std::vector > > els; + pq->pop(els); + for(typename std::vector > >::iterator it = els.begin(); + it != els.end(); + ++it) + { + (*it)->pub_.publish((*it)->msg_); + } + } + + public: + PubMultiQueue() {} + ~PubMultiQueue() + { + if(service_thread_.joinable()) + { + service_thread_running_ = false; + notifyServiceThread(); + service_thread_.join(); + } + } + + /// \brief Add a new queue. Call this once for each published topic (or at + /// least each type of publish message). + /// \return Pointer to the newly created queue, good for calling push() on. + template + boost::shared_ptr > addPub() + { + typename PubQueue::QueuePtr queue(new std::deque > >); + boost::shared_ptr queue_lock(new boost::mutex); + boost::shared_ptr > pq(new PubQueue(queue, queue_lock, boost::bind(&PubMultiQueue::notifyServiceThread, this))); + boost::function f = boost::bind(&PubMultiQueue::serviceFunc, this, pq); + { + boost::mutex::scoped_lock lock(service_funcs_lock_); + service_funcs_.push_back(f); + } + return pq; + } + + /// \brief Service each queue one time. + void spinOnce() + { + boost::mutex::scoped_lock lock(service_funcs_lock_); + for(std::list >::iterator it = service_funcs_.begin(); + it != service_funcs_.end(); + ++it) + { + (*it)(); + } + } + + /// \brief Service all queues indefinitely, waiting on a condition variable + /// in between cycles. + void spin() + { + while(ros::ok() && service_thread_running_) + { + boost::unique_lock lock(service_cond_var_lock_); + service_cond_var_.wait(lock); + spinOnce(); + } + } + + /// \brief Start a thread to call spin(). + void startServiceThread() + { + service_thread_running_ = true; + service_thread_ = boost::thread(boost::bind(&PubMultiQueue::spin, this)); + } + + /// \brief Wake up the queue serive thread (e.g., after having pushed a + /// message onto one of the queues). + void notifyServiceThread() + { + service_cond_var_.notify_one(); + } +}; + +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_block_laser.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_block_laser.h new file mode 100755 index 0000000..e46cb2e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_block_laser.h @@ -0,0 +1,128 @@ +/* + * 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: ros laser controller. + * Author: Nathan Koenig + * Date: 01 Feb 2007 + */ + +#ifndef GAZEBO_ROS_BLOCK_LASER_HH +#define GAZEBO_ROS_BLOCK_LASER_HH + +// Custom Callback Queue +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace gazebo +{ + + class GazeboRosBlockLaser : public RayPlugin + { + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: GazeboRosBlockLaser(); + + /// \brief Destructor + public: ~GazeboRosBlockLaser(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Update the controller + protected: virtual void OnNewLaserScans(); + + /// \brief Put laser data to the ROS topic + private: void PutLaserData(common::Time &_updateTime); + + private: common::Time last_update_time_; + + /// \brief Keep track of number of connctions + private: int laser_connect_count_; + private: void LaserConnect(); + private: void LaserDisconnect(); + + // Pointer to the model + private: physics::WorldPtr world_; + /// \brief The parent sensor + private: sensors::SensorPtr parent_sensor_; + private: sensors::RaySensorPtr parent_ray_sensor_; + + /// \brief pointer to ros node + private: ros::NodeHandle* rosnode_; + private: ros::Publisher pub_; + + /// \brief ros message + private: sensor_msgs::PointCloud cloud_msg_; + + /// \brief topic name + private: std::string topic_name_; + + /// \brief frame transform name, should match link name + private: std::string frame_name_; + + /// \brief Gaussian noise + private: double gaussian_noise_; + + /// \brief Gaussian noise generator + private: double GaussianKernel(double mu,double sigma); + + /// \brief A mutex to lock access to fields that are used in message callbacks + private: boost::mutex lock; + + /// \brief hack to mimic hokuyo intensity cutoff of 100 + //private: ParamT *hokuyoMinIntensityP; + private: double hokuyo_min_intensity_; + + /// update rate of this sensor + private: double update_rate_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + // Custom Callback Queue + private: ros::CallbackQueue laser_queue_; + private: void LaserQueueThread(); + private: boost::thread callback_laser_queue_thread_; + + // subscribe to world stats + private: transport::NodePtr node_; + private: common::Time sim_time_; + public: void OnStats( const boost::shared_ptr &_msg); + + }; + +} + +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.h new file mode 100755 index 0000000..35a2b26 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_bumper.h @@ -0,0 +1,93 @@ +/* + * Copyright 2012 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_ROS_BUMPER_HH +#define GAZEBO_ROS_BUMPER_HH + +#include + +#include +#include +#include + +#include + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + /// \brief A Bumper controller + class GazeboRosBumper : public SensorPlugin + { + /// Constructor + public: GazeboRosBumper(); + + /// Destructor + public: ~GazeboRosBumper(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// Update the controller + private: void OnContact(); + + /// \brief pointer to ros node + private: ros::NodeHandle* rosnode_; + private: ros::Publisher contact_pub_; + + private: sensors::ContactSensorPtr parentSensor; + + /// \brief set topic name of broadcast + private: std::string bumper_topic_name_; + + private: std::string frame_name_; + + /// \brief broadcast some string for now. + private: gazebo_msgs::ContactsState contact_state_msg_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + private: ros::CallbackQueue contact_queue_; + private: void ContactQueueThread(); + private: boost::thread callback_queue_thread_; + + // Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + }; +} + +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera.h new file mode 100755 index 0000000..33f41b8 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera.h @@ -0,0 +1,54 @@ +/* + * Copyright 2012 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. +*/ + +#ifndef GAZEBO_ROS_CAMERA_HH +#define GAZEBO_ROS_CAMERA_HH + +#include + +// library for processing camera data for gazebo / ros conversions +#include + +#include + +namespace gazebo +{ + class GazeboRosCamera : public CameraPlugin, GazeboRosCameraUtils + { + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: GazeboRosCamera(); + + /// \brief Destructor + public: ~GazeboRosCamera(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Update the controller + protected: virtual void OnNewFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + }; +} +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h new file mode 100755 index 0000000..2125f40 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h @@ -0,0 +1,230 @@ +/* + * Copyright 2012 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_ROS_CAMERA_UTILS_HH +#define GAZEBO_ROS_CAMERA_UTILS_HH + +#include +// boost stuff +#include +#include + +// ros stuff +#include +#include +#include + +// ros messages stuff +#include +#include +#include +#include +#include +#include +#include + +// dynamic reconfigure stuff +#include +#include + +// Gazebo +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + class GazeboRosMultiCamera; + class GazeboRosTriggeredMultiCamera; + class GazeboRosCameraUtils + { + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: GazeboRosCameraUtils(); + + /// \brief Destructor + public: ~GazeboRosCameraUtils(); + + /// \brief Load the plugin. + /// \param[in] _parent Take in SDF root element. + /// \param[in] _sdf SDF values. + /// \param[in] _camera_name_suffix required before calling LoadThread + public: void Load(sensors::SensorPtr _parent, + sdf::ElementPtr _sdf, + const std::string &_camera_name_suffix = ""); + + /// \brief Load the plugin. + /// \param[in] _parent Take in SDF root element. + /// \param[in] _sdf SDF values. + /// \param[in] _camera_name_suffix Suffix of the camera name. + /// \param[in] _hack_baseline Multiple camera baseline. + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, + const std::string &_camera_name_suffix, + double _hack_baseline); + + public: event::ConnectionPtr OnLoad(const boost::function&); + + private: void Init(); + + /// \brief Put camera data to the ROS topic + protected: void PutCameraData(const unsigned char *_src); + protected: void PutCameraData(const unsigned char *_src, + common::Time &last_update_time); + + /// \brief Keep track of number of image connections + protected: boost::shared_ptr image_connect_count_; + /// \brief A mutex to lock access to image_connect_count_ + protected: boost::shared_ptr image_connect_count_lock_; + protected: void ImageConnect(); + protected: void ImageDisconnect(); + + /// \brief Keep track when we activate this camera through ros + /// subscription, was it already active? resume state when + /// unsubscribed. + protected: boost::shared_ptr was_active_; + + /// \brief: Camera modification functions + private: void SetHFOV(const std_msgs::Float64::ConstPtr& hfov); + private: void SetUpdateRate(const std_msgs::Float64::ConstPtr& update_rate); + + /// \brief A pointer to the ROS node. + /// A node will be instantiated if it does not exist. + protected: ros::NodeHandle* rosnode_; + protected: image_transport::Publisher image_pub_; + private: image_transport::ImageTransport* itnode_; + + /// \brief ROS image message + protected: sensor_msgs::Image image_msg_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + /// \brief ROS camera name + private: std::string camera_name_; + + /// \brief tf prefix + private: std::string tf_prefix_; + + /// \brief ROS image topic name + protected: std::string image_topic_name_; + + /// \brief Publish CameraInfo to the ROS topic + protected: void PublishCameraInfo(ros::Publisher camera_info_publisher); + protected: void PublishCameraInfo(common::Time &last_update_time); + protected: void PublishCameraInfo(); + /// \brief Keep track of number of connctions for CameraInfo + private: void InfoConnect(); + private: void InfoDisconnect(); + /// \brief camera info + protected: ros::Publisher camera_info_pub_; + protected: std::string camera_info_topic_name_; + protected: common::Time last_info_update_time_; + + /// \brief ROS frame transform name to use in the image message header. + /// This should typically match the link name the sensor is attached. + protected: std::string frame_name_; + /// update rate of this sensor + protected: double update_rate_; + protected: double update_period_; + protected: common::Time last_update_time_; + + protected: double cx_prime_; + protected: double cx_; + protected: double cy_; + protected: double focal_length_; + protected: double hack_baseline_; + protected: double distortion_k1_; + protected: double distortion_k2_; + protected: double distortion_k3_; + protected: double distortion_t1_; + protected: double distortion_t2_; + + protected: bool border_crop_; + + protected: boost::shared_ptr camera_info_manager_; + + + /// \brief A mutex to lock access to fields + /// that are used in ROS message callbacks + protected: boost::mutex lock_; + + /// \brief size of image buffer + protected: std::string type_; + protected: int skip_; + + private: ros::Subscriber cameraHFOVSubscriber_; + private: ros::Subscriber cameraUpdateRateSubscriber_; + + // Time last published, refrain from publish unless new image has + // been rendered + // Allow dynamic reconfiguration of camera params + dynamic_reconfigure::Server + *dyn_srv_; + void configCallback(gazebo_plugins::GazeboRosCameraConfig &config, + uint32_t level); + + protected: ros::CallbackQueue camera_queue_; + protected: void CameraQueueThread(); + protected: boost::thread callback_queue_thread_; + + + // copied from CameraPlugin + protected: unsigned int width_, height_, depth_; + protected: std::string format_; + + protected: sensors::SensorPtr parentSensor_; + protected: rendering::CameraPtr camera_; + + // Pointer to the world + protected: physics::WorldPtr world_; + + private: event::ConnectionPtr newFrameConnection_; + + protected: common::Time sensor_update_time_; + + // maintain for one more release for backwards compatibility + protected: physics::WorldPtr world; + + // deferred load in case ros is blocking + private: sdf::ElementPtr sdf; + private: void LoadThread(); + private: boost::thread deferred_load_thread_; + private: event::EventT load_event_; + + // make a trigger function that the child classes can override + // and a function that returns bool to indicate whether the trigger + // should be used + protected: virtual void TriggerCamera(); + protected: virtual bool CanTriggerCamera(); + private: void TriggerCameraInternal(const std_msgs::Empty::ConstPtr &dummy); + private: ros::Subscriber trigger_subscriber_; + + /// \brief ROS trigger topic name + protected: std::string trigger_topic_name_; + + /// \brief True if camera util is initialized + protected: bool initialized_; + + friend class GazeboRosMultiCamera; + friend class GazeboRosTriggeredMultiCamera; + }; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h new file mode 100755 index 0000000..28c2ad8 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_depth_camera.h @@ -0,0 +1,151 @@ +/* + * 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 GAZEBO_ROS_DEPTH_CAMERA_HH +#define GAZEBO_ROS_DEPTH_CAMERA_HH + +// ros stuff +#include +#include +#include + +// ros messages stuff +#include +#include +#include +#include +#include +#include + +// gazebo stuff +#include +#include +#include +#include +#include +#include +#include + +// dynamic reconfigure stuff +#include +#include + +// boost stuff +#include + +// camera stuff +#include + +namespace gazebo +{ + class GazeboRosDepthCamera : public DepthCameraPlugin, GazeboRosCameraUtils + { + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: GazeboRosDepthCamera(); + + /// \brief Destructor + public: ~GazeboRosDepthCamera(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Advertise point cloud and depth image + public: virtual void Advertise(); + + /// \brief Update the controller + protected: virtual void OnNewDepthFrame(const float *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Update the controller + protected: virtual void OnNewRGBPointCloud(const float *_pcd, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Update the controller + protected: virtual void OnNewImageFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Put camera data to the ROS topic + private: void FillPointdCloud(const float *_src); + + /// \brief push depth image data into ros topic + private: void FillDepthImage(const float *_src); + + /// \brief Keep track of number of connctions for point clouds + private: int point_cloud_connect_count_; + private: void PointCloudConnect(); + private: void PointCloudDisconnect(); + + /// \brief Keep track of number of connctions for point clouds + private: int depth_image_connect_count_; + private: void DepthImageConnect(); + private: void DepthImageDisconnect(); + private: common::Time last_depth_image_camera_info_update_time_; + + private: bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg); + + private: bool FillDepthImageHelper( sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg); + + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + private: ros::Publisher point_cloud_pub_; + private: ros::Publisher depth_image_pub_; + + /// \brief PointCloud2 point cloud message + private: sensor_msgs::PointCloud2 point_cloud_msg_; + private: sensor_msgs::Image depth_image_msg_; + + private: double point_cloud_cutoff_; + + /// \brief ROS image topic name + private: std::string point_cloud_topic_name_; + + private: void InfoConnect(); + private: void InfoDisconnect(); + + using GazeboRosCameraUtils::PublishCameraInfo; + protected: virtual void PublishCameraInfo(); + + /// \brief image where each pixel contains the depth information + private: std::string depth_image_topic_name_; + private: std::string depth_image_camera_info_topic_name_; + private: int depth_info_connect_count_; + private: void DepthInfoConnect(); + private: void DepthInfoDisconnect(); + + // overload with our own + private: common::Time depth_sensor_update_time_; + protected: ros::Publisher depth_image_camera_info_pub_; + + private: event::ConnectionPtr load_connection_; + }; + +} +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h new file mode 100755 index 0000000..8615f31 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_diff_drive.h @@ -0,0 +1,159 @@ +/* + * Copyright (c) 2010, Daniel Hewlett, Antons Rebguns + * All rights reserved. + * + * 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 the 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 Antons Rebguns ''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 Antons Rebguns 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 gazebo_ros_diff_drive.h + * + * \brief A differential drive plugin for gazebo. Based on the diffdrive plugin + * developed for the erratic robot (see copyright notice above). The original + * plugin can be found in the ROS package gazebo_erratic_plugins. + * + * \author Piyush Khandelwal (piyushk@gmail.com) + * + * $ Id: 06/21/2013 11:23:40 AM piyushk $ + */ + +#ifndef DIFFDRIVE_PLUGIN_HH +#define DIFFDRIVE_PLUGIN_HH + +#include + +// Gazebo +#include +#include +#include + +// ROS +#include +#include +#include +#include +#include +#include +#include + +// Custom Callback Queue +#include +#include + +// Boost +#include +#include + +namespace gazebo { + + class Joint; + class Entity; + + class GazeboRosDiffDrive : public ModelPlugin { + + enum OdomSource + { + ENCODER = 0, + WORLD = 1, + }; + public: + GazeboRosDiffDrive(); + ~GazeboRosDiffDrive(); + void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + void Reset(); + + protected: + virtual void UpdateChild(); + virtual void FiniChild(); + + private: + void publishOdometry(double step_time); + void getWheelVelocities(); + void publishWheelTF(); /// publishes the wheel tf's + void publishWheelJointState(); + void UpdateOdometryEncoder(); + + + GazeboRosPtr gazebo_ros_; + physics::ModelPtr parent; + event::ConnectionPtr update_connection_; + + double wheel_separation_; + double wheel_diameter_; + double wheel_torque; + double wheel_speed_[2]; + double wheel_accel; + double wheel_speed_instr_[2]; + + std::vector joints_; + + // ROS STUFF + ros::Publisher odometry_publisher_; + ros::Subscriber cmd_vel_subscriber_; + boost::shared_ptr transform_broadcaster_; + sensor_msgs::JointState joint_state_; + ros::Publisher joint_state_publisher_; + nav_msgs::Odometry odom_; + std::string tf_prefix_; + + boost::mutex lock; + + std::string robot_namespace_; + std::string command_topic_; + std::string odometry_topic_; + std::string odometry_frame_; + std::string robot_base_frame_; + bool publish_tf_; + bool legacy_mode_; + // Custom Callback Queue + ros::CallbackQueue queue_; + boost::thread callback_queue_thread_; + void QueueThread(); + + // DiffDrive stuff + void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); + + double x_; + double rot_; + bool alive_; + + // Update Rate + double update_rate_; + double update_period_; + common::Time last_update_time_; + + OdomSource odom_source_; + geometry_msgs::Pose2D pose_encoder_; + common::Time last_odom_update_; + + // Flags + bool publishWheelTF_; + bool publishWheelJointState_; + + }; + +} + +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_elevator.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_elevator.h new file mode 100755 index 0000000..14d7743 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_elevator.h @@ -0,0 +1,71 @@ +/* + * Copyright 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_ROS_PLUGINS_ELEVATOR_PLUGIN_H_ +#define _GAZEBO_ROS_PLUGINS_ELEVATOR_PLUGIN_H_ + +#include + +// Gazebo +#include + +// ROS +#include +#include +#include +#include + +namespace gazebo +{ + /// \brief ROS implementation of the Elevator plugin + class GazeboRosElevator : public ElevatorPlugin + { + /// \brief Constructor + public: GazeboRosElevator(); + + /// \brief Destructor + public: virtual ~GazeboRosElevator(); + + /// \brief Load the plugin. + /// \param[in] _model Pointer to the Model + /// \param[in] _sdf Pointer to the SDF element of the plugin. + public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Receives messages on the elevator's topic. + /// \param[in] _msg The string message that contains a command. + public: void OnElevator(const std_msgs::String::ConstPtr &_msg); + + /// \brief Queu to handle callbacks. + private: void QueueThread(); + + /// \brief for setting ROS name space + private: std::string robotNamespace_; + + /// \brief ros node handle + private: ros::NodeHandle *rosnode_; + + /// \brief Subscribes to a topic that controls the elevator. + private: ros::Subscriber elevatorSub_; + + /// \brief Custom Callback Queue + private: ros::CallbackQueue queue_; + + // \brief Custom Callback Queue thread + private: boost::thread callbackQueueThread_; + }; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_f3d.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_f3d.h new file mode 100755 index 0000000..5e6cf22 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_f3d.h @@ -0,0 +1,111 @@ +/* + * 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: 3D Applied Force Feedback Interface + * Author: John Hsu + * Date: 24 Sept 2008 + */ + +#ifndef GAZEBO_ROS_F3D_HH +#define GAZEBO_ROS_F3D_HH + +// Custom Callback Queue +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gazebo +{ + +/// \brief GazeboRosF3D controller +/// This is a controller that simulates a 6 dof force sensor +class GazeboRosF3D : public ModelPlugin +{ + /// \brief Constructor + /// \param parent The parent entity must be a Model + public: GazeboRosF3D(); + + /// \brief Destructor + public: virtual ~GazeboRosF3D(); + + /// \brief Load the controller + public: void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); + + /// \brief Update the controller + protected: virtual void UpdateChild(); + + private: physics::WorldPtr world_; + + /// \brief A pointer to the Gazebo Body + private: physics::LinkPtr link_; + + + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + private: ros::NodeHandle* rosnode_; + private: ros::Publisher pub_; + + /// \brief ROS WrenchStamped message + private: geometry_msgs::WrenchStamped wrench_msg_; + + /// \brief store bodyname + private: std::string link_name_; + + /// \brief ROS WrenchStamped topic name + private: std::string topic_name_; + + /// \brief ROS frame transform name to use in the image message header. + /// This should be simply map since the returned info is in Gazebo Global Frame. + private: std::string frame_name_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + /// \brief A mutex to lock access to fields that are used in message callbacks + private: boost::mutex lock_; + + /// \brief: keep track of number of connections + private: int f3d_connect_count_; + private: void F3DConnect(); + private: void F3DDisconnect(); + + // Custom Callback Queue + private: ros::CallbackQueue queue_; + private: void QueueThread(); + private: boost::thread callback_queue_thread_; + + // Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + +}; + +/** \} */ +/// @} + + +} + +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.h new file mode 100755 index 0000000..1091bfa --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_force.h @@ -0,0 +1,126 @@ +/* + * 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 performs generic force interface. + * Author: John Hsu + * Date: 24 Sept 2008 + */ + +#ifndef GAZEBO_ROS_FORCE_HH +#define GAZEBO_ROS_FORCE_HH + +#include + +// Custom Callback Queue +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + + +namespace gazebo +{ +/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins +/// @{ +/** \defgroup GazeboRosForce Plugin XML Reference and Example + + \brief Ros Force Plugin. + + This is a Plugin that collects data from a ROS topic and applies wrench to a body accordingly. + + Example Usage: + \verbatim + + + box_body + box_force + + + \endverbatim + +\{ +*/ + +/** + . + +*/ + +class GazeboRosForce : public ModelPlugin +{ + /// \brief Constructor + public: GazeboRosForce(); + + /// \brief Destructor + public: virtual ~GazeboRosForce(); + + // Documentation inherited + protected: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + // Documentation inherited + protected: virtual void UpdateChild(); + + /// \brief call back when a Wrench message is published + /// \param[in] _msg The Incoming ROS message representing the new force to exert. + private: void UpdateObjectForce(const geometry_msgs::Wrench::ConstPtr& _msg); + + /// \brief The custom callback queue thread function. + private: void QueueThread(); + + /// \brief A pointer to the gazebo world. + private: physics::WorldPtr world_; + + /// \brief A pointer to the Link, where force is applied + private: physics::LinkPtr link_; + + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + private: ros::NodeHandle* rosnode_; + private: ros::Subscriber sub_; + + /// \brief A mutex to lock access to fields that are used in ROS message callbacks + private: boost::mutex lock_; + + /// \brief ROS Wrench topic name inputs + private: std::string topic_name_; + /// \brief The Link this plugin is attached to, and will exert forces on. + private: std::string link_name_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + // Custom Callback Queue + private: ros::CallbackQueue queue_; + /// \brief Thead object for the running callback Thread. + private: boost::thread callback_queue_thread_; + /// \brief Container for the wrench force that this plugin exerts on the body. + private: geometry_msgs::Wrench wrench_msg_; + + // Pointer to the update event connection + private: event::ConnectionPtr update_connection_; +}; +/** \} */ +/// @} +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h new file mode 100755 index 0000000..e8f2191 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_ft_sensor.h @@ -0,0 +1,160 @@ +/* + * 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: Force Torque Sensor Plugin + * Author: Francisco Suarez-Ruiz + * Date: 5 June 2014 + */ + +#ifndef GAZEBO_ROS_FT_HH +#define GAZEBO_ROS_FT_HH + +// Custom Callback Queue +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gazebo +{ +/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins +/// @{ +/** \defgroup GazeboRosFTSensor Plugin XML Reference and Example + +\brief Ros Gazebo Ros Force/Torque Sensor Plugin. +This is a model plugin which broadcasts geometry_msgs/WrenchStamped messages +with measured force and torque on a specified joint. +The wrench is reported in the joint CHILD link frame and the measure direction +is child-to-parent link. + +Example Usage: + +\verbatim + + +true + + + + +100.0 +ft_sensor_topic +JOINT_NAME + + +\endverbatim +\{ +*/ + + +/// \brief GazeboRosFT controller +/// This is a controller that simulates a 6 dof force sensor +class GazeboRosFT : public ModelPlugin +{ + /// \brief Constructor + /// \param parent The parent entity must be a Model + public: GazeboRosFT(); + + /// \brief Destructor + public: virtual ~GazeboRosFT(); + + /// \brief Load the controller + public: void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); + + /// \brief Update the controller + protected: virtual void UpdateChild(); + + /// \brief Gaussian noise + private: double gaussian_noise_; + private: unsigned int seed; + /// \brief Gaussian noise generator + private: double GaussianKernel(double mu, double sigma); + + /// \brief A pointer to the Gazebo joint + private: physics::JointPtr joint_; + + /// \brief A pointer to the Gazebo parent link + private: physics::LinkPtr parent_link_; + + /// \brief A pointer to the Gazebo child link + private: physics::LinkPtr child_link_; + + /// \brief A pointer to the Gazebo model + private: physics::ModelPtr model_; + + /// \brief A pointer to the Gazebo world + private: physics::WorldPtr world_; + + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + private: ros::NodeHandle* rosnode_; + private: ros::Publisher pub_; + + /// \brief ROS WrenchStamped message + private: geometry_msgs::WrenchStamped wrench_msg_; + + /// \brief store bodyname + private: std::string joint_name_; + + /// \brief ROS WrenchStamped topic name + private: std::string topic_name_; + + /// \brief ROS frame transform name to use in the image message header. + /// FIXME: extract link name directly? + private: std::string frame_name_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + /// \brief A mutex to lock access to fields that are used in message callbacks + private: boost::mutex lock_; + + /// \brief save last_time + private: common::Time last_time_; + + // rate control + private: double update_rate_; + + /// \brief: keep track of number of connections + private: int ft_connect_count_; + private: void FTConnect(); + private: void FTDisconnect(); + + // Custom Callback Queue + private: ros::CallbackQueue queue_; + private: void QueueThread(); + private: boost::thread callback_queue_thread_; + + // Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + +}; + +/** \} */ +/// @} + + +} + +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_gpu_laser.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_gpu_laser.h new file mode 100755 index 0000000..ddd2a79 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_gpu_laser.h @@ -0,0 +1,99 @@ +/* + * Copyright 2012 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_ROS_LASER_HH +#define GAZEBO_ROS_LASER_HH + +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +namespace gazebo +{ + class GazeboRosLaser : public GpuRayPlugin + { + /// \brief Constructor + public: GazeboRosLaser(); + + /// \brief Destructor + public: ~GazeboRosLaser(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Keep track of number of connctions + private: int laser_connect_count_; + private: void LaserConnect(); + private: void LaserDisconnect(); + + // Pointer to the model + private: std::string world_name_; + private: physics::WorldPtr world_; + /// \brief The parent sensor + private: sensors::GpuRaySensorPtr parent_ray_sensor_; + + /// \brief pointer to ros node + private: ros::NodeHandle* rosnode_; + private: ros::Publisher pub_; + private: PubQueue::Ptr pub_queue_; + + /// \brief topic name + private: std::string topic_name_; + + /// \brief frame transform name, should match link name + private: std::string frame_name_; + + /// \brief tf prefix + private: std::string tf_prefix_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + // deferred load in case ros is blocking + private: sdf::ElementPtr sdf; + private: void LoadThread(); + private: boost::thread deferred_load_thread_; + private: unsigned int seed; + + private: gazebo::transport::NodePtr gazebo_node_; + private: gazebo::transport::SubscriberPtr laser_scan_sub_; + private: void OnScan(ConstLaserScanStampedPtr &_msg); + + /// \brief prevents blocking + private: PubMultiQueue pmq; + }; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h new file mode 100755 index 0000000..4773b80 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_hand_of_god.h @@ -0,0 +1,71 @@ +/* + * 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: 3D position interface. + * Author: Sachin Chitta and John Hsu + * Date: 10 June 2008 + */ + +#ifndef GAZEBO_ROS_TEMPLATE_HH +#define GAZEBO_ROS_TEMPLATE_HH + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace gazebo +{ + + class GazeboRosHandOfGod : public ModelPlugin + { + /// \brief Constructor + public: GazeboRosHandOfGod(); + + /// \brief Destructor + public: virtual ~GazeboRosHandOfGod(); + + /// \brief Load the controller + public: void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); + + /// \brief Update the controller + protected: virtual void GazeboUpdate(); + + /// Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + boost::shared_ptr tf_buffer_; + boost::shared_ptr tf_listener_; + boost::shared_ptr tf_broadcaster_; + physics::ModelPtr model_; + physics::LinkPtr floating_link_; + std::string link_name_; + std::string robot_namespace_; + std::string frame_id_; + double kl_, ka_; + double cl_, ca_; + }; + +} + +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_harness.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_harness.h new file mode 100755 index 0000000..17e0aa4 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_harness.h @@ -0,0 +1,80 @@ +/* + * 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. + * +*/ +#ifndef GAZEBO_ROS_HARNESS_H +#define GAZEBO_ROS_HARNESS_H + +// Custom Callback Queue +#include +#include + +#include +#include +#include + +#include + +namespace gazebo +{ +/// \brief See the Gazebo documentation about the HarnessPlugin. This ROS +/// wrapper exposes two topics: +/// +/// 1. //harness/velocity +/// - Message Type: std_msgs::Float32 +/// - Purpose: Set target winch velocity +/// +/// 2. //harness/detach +/// - Message Type: std_msgs::Bool +/// - Purpose: Detach the joint. +class GazeboRosHarness : public HarnessPlugin +{ + /// \brief Constructor + public: GazeboRosHarness(); + + /// \brief Destructor + public: virtual ~GazeboRosHarness(); + + /// \brief Load the plugin + public: virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Receive winch velocity control messages. + /// \param[in] msg Float message that is the target winch velocity. + private: virtual void OnVelocity(const std_msgs::Float32::ConstPtr &msg); + + /// \brief Receive detach messages + /// \param[in] msg Boolean detach message. Detach joints if data is + /// true. + private: virtual void OnDetach(const std_msgs::Bool::ConstPtr &msg); + + /// \brief Custom callback queue thread + private: void QueueThread(); + + /// \brief pointer to ros node + private: ros::NodeHandle *rosnode_; + + /// \brief Subscriber to velocity control messages. + private: ros::Subscriber velocitySub_; + + /// \brief Subscriber to detach control messages. + private: ros::Subscriber detachSub_; + + /// \brief for setting ROS name space + private: std::string robotNamespace_; + private: ros::CallbackQueue queue_; + private: boost::thread callbackQueueThread_; +}; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h new file mode 100755 index 0000000..6fa5065 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu.h @@ -0,0 +1,130 @@ +/* + * Copyright 2012 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_ROS_IMU_HH +#define GAZEBO_ROS_IMU_HH + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace gazebo +{ + class GazeboRosIMU : public ModelPlugin + { + /// \brief Constructor + public: GazeboRosIMU(); + + /// \brief Destructor + public: virtual ~GazeboRosIMU(); + + /// \brief Load the controller + /// \param node XML config node + public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Update the controller + protected: virtual void UpdateChild(); + + /// \brief The parent World + private: physics::WorldPtr world_; + + /// \brief The link referred to by this plugin + private: physics::LinkPtr link; + + /// \brief pointer to ros node + private: ros::NodeHandle* rosnode_; + private: ros::Publisher pub_; + private: PubQueue::Ptr pub_Queue; + + /// \brief ros message + private: sensor_msgs::Imu imu_msg_; + + /// \brief store link name + private: std::string link_name_; + + /// \brief store frame name + private: std::string frame_name_; + + /// \brief topic name + private: std::string topic_name_; + + /// \brief allow specifying constant xyz and rpy offsets + private: ignition::math::Pose3d offset_; + + /// \brief A mutex to lock access to fields + /// that are used in message callbacks + private: boost::mutex lock_; + + /// \brief save last_time + private: common::Time last_time_; + private: ignition::math::Vector3d last_vpos_; + private: ignition::math::Vector3d last_veul_; + private: ignition::math::Vector3d apos_; + private: ignition::math::Vector3d aeul_; + + // rate control + private: double update_rate_; + + /// \brief: keep initial pose to offset orientation in imu message + private: ignition::math::Pose3d initial_pose_; + + /// \brief Gaussian noise + private: double gaussian_noise_; + + /// \brief Gaussian noise generator + private: double GaussianKernel(double mu, double sigma); + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + /// \brief call back when using service + private: bool ServiceCallback(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res); + + private: ros::ServiceServer srv_; + private: std::string service_name_; + + private: ros::CallbackQueue imu_queue_; + private: void IMUQueueThread(); + private: boost::thread callback_queue_thread_; + + // Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + + // deferred load in case ros is blocking + private: sdf::ElementPtr sdf; + private: void LoadThread(); + private: boost::thread deferred_load_thread_; + private: unsigned int seed; + + // ros publish multi queue, prevents publish() blocking + private: PubMultiQueue pmq; + }; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu_sensor.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu_sensor.h new file mode 100755 index 0000000..15fa563 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_imu_sensor.h @@ -0,0 +1,107 @@ +/* Copyright [2015] [Alessandro Settimi] + * + * email: ale.settimi@gmail.com + * + * 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_ROS_IMU_SENSOR_H +#define GAZEBO_ROS_IMU_SENSOR_H + +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + namespace sensors + { + class ImuSensor; + } + /** + @anchor GazeboRosImuSensor + \ref GazeboRosImuSensor is a plugin to simulate an Inertial Motion Unit sensor, the main differences from \ref GazeboRosIMU are: + - inheritance from SensorPlugin instead of ModelPlugin, + - measurements are given by gazebo ImuSensor instead of being computed by the ros plugin, + - gravity is included in inertial measurements. + */ + /** @brief Gazebo Ros imu sensor plugin. */ + class GazeboRosImuSensor : public SensorPlugin + { + public: + /// \brief Constructor. + GazeboRosImuSensor(); + /// \brief Destructor. + virtual ~GazeboRosImuSensor(); + /// \brief Load the sensor. + /// \param sensor_ pointer to the sensor. + /// \param sdf_ pointer to the sdf config file. + virtual void Load(sensors::SensorPtr sensor_, sdf::ElementPtr sdf_); + + protected: + /// \brief Update the sensor. + virtual void UpdateChild(const gazebo::common::UpdateInfo &/*_info*/); + + private: + /// \brief Load the parameters from the sdf file. + bool LoadParameters(); + /// \brief Gaussian noise generator. + /// \param mu offset value. + /// \param sigma scaling value. + double GuassianKernel(double mu, double sigma); + + /// \brief Ros NodeHandle pointer. + ros::NodeHandle* node; + /// \brief Ros Publisher for imu data. + ros::Publisher imu_data_publisher; + /// \brief Ros IMU message. + sensor_msgs::Imu imu_msg; + + /// \brief last time on which the data was published. + common::Time last_time; + /// \brief Pointer to the update event connection. + gazebo::event::ConnectionPtr connection; + /// \brief Pointer to the sensor. + sensors::ImuSensor* sensor; + /// \brief Pointer to the sdf config file. + sdf::ElementPtr sdf; + /// \brief Orientation data from the sensor. + ignition::math::Quaterniond orientation; + /// \brief Linear acceleration data from the sensor. + ignition::math::Vector3d accelerometer_data; + /// \brief Angular velocity data from the sensor. + ignition::math::Vector3d gyroscope_data; + + /// \brief Seed for the Gaussian noise generator. + unsigned int seed; + + //loaded parameters + /// \brief The data is published on the topic named: /robot_namespace/topic_name. + std::string robot_namespace; + /// \brief The data is published on the topic named: /robot_namespace/topic_name. + std::string topic_name; + /// \brief Name of the link of the IMU. + std::string body_name; + /// \brief Sensor update rate. + double update_rate; + /// \brief Gaussian noise. + double gaussian_noise; + /// \brief Offset parameter, position part is unused. + ignition::math::Pose3d offset; + }; +} + +#endif //GAZEBO_ROS_IMU_SENSOR_H diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_pose_trajectory.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_pose_trajectory.h new file mode 100755 index 0000000..bc9651f --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_pose_trajectory.h @@ -0,0 +1,133 @@ +/* + * Copyright 2012 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. + * +*/ + +// ************************************************************* +// DEPRECATED +// This class has been renamed to gazebo_ros_joint_pose_trajectory +// ************************************************************* + +#ifndef GAZEBO_ROS_JOINT_TRAJECTORY_PLUGIN_HH +#define GAZEBO_ROS_JOINT_TRAJECTORY_PLUGIN_HH + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#undef ENABLE_SERVICE +#ifdef ENABLE_SERVICE +#include +#endif + +#include +#include +#include +#include +#include + +namespace gazebo +{ + class GazeboRosJointPoseTrajectory : public ModelPlugin // replaced with GazeboROSJointPoseTrajectory + { + /// \brief Constructor + public: GazeboRosJointPoseTrajectory(); + + /// \brief Destructor + public: virtual ~GazeboRosJointPoseTrajectory(); + + /// \brief Load the controller + public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + /// \brief Update the controller + private: void SetTrajectory( + const trajectory_msgs::JointTrajectory::ConstPtr& trajectory); +#ifdef ENABLE_SERVICE + private: bool SetTrajectory( + const gazebo_msgs::SetJointTrajectory::Request& req, + const gazebo_msgs::SetJointTrajectory::Response& res); +#endif + private: void UpdateStates(); + + private: physics::WorldPtr world_; + private: physics::ModelPtr model_; + + /// \brief pose should be set relative to this link (default to "world") + private: physics::LinkPtr reference_link_; + private: std::string reference_link_name_; + + /// \brief pointer to ros node + private: ros::NodeHandle* rosnode_; + private: ros::Subscriber sub_; + private: ros::ServiceServer srv_; + private: bool has_trajectory_; + + /// \brief ros message + private: trajectory_msgs::JointTrajectory trajectory_msg_; + private: bool set_model_pose_; + private: geometry_msgs::Pose model_pose_; + + /// \brief topic name + private: std::string topic_name_; + private: std::string service_name_; + + /// \brief A mutex to lock access to fields that are + /// used in message callbacks + private: boost::mutex update_mutex; + + /// \brief save last_time + private: common::Time last_time_; + + // trajectory time control + private: common::Time trajectory_start; + private: unsigned int trajectory_index; + + // rate control + private: double update_rate_; + private: bool disable_physics_updates_; + private: bool physics_engine_enabled_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + private: ros::CallbackQueue queue_; + private: void QueueThread(); + private: boost::thread callback_queue_thread_; + + private: std::vector joints_; + private: std::vector points_; + + // Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + + private: trajectory_msgs::JointTrajectory joint_trajectory_; + + // deferred load in case ros is blocking + private: sdf::ElementPtr sdf; + private: void LoadThread(); + private: boost::thread deferred_load_thread_; + }; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h new file mode 100755 index 0000000..2799910 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_state_publisher.h @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * 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 the 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 Antons Rebguns ''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 Antons Rebguns 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. + * + **/ + + +#ifndef JOINT_STATE_PUBLISHER_PLUGIN_HH +#define JOINT_STATE_PUBLISHER_PLUGIN_HH + +#include +#include +#include +#include +#include + +// ROS +#include +#include +#include + +// Usage in URDF: +// +// +// /pioneer2dx +// chassis_swivel_joint, swivel_wheel_joint, left_hub_joint, right_hub_joint +// 100.0 +// true +// +// + + + +namespace gazebo { +class GazeboRosJointStatePublisher : public ModelPlugin { +public: + GazeboRosJointStatePublisher(); + ~GazeboRosJointStatePublisher(); + void Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); + void OnUpdate ( const common::UpdateInfo & _info ); + void publishJointStates(); + // Pointer to the model +private: + event::ConnectionPtr updateConnection; + physics::WorldPtr world_; + physics::ModelPtr parent_; + std::vector joints_; + + // ROS STUFF + boost::shared_ptr rosnode_; + sensor_msgs::JointState joint_state_; + ros::Publisher joint_state_publisher_; + std::string tf_prefix_; + std::string robot_namespace_; + std::vector joint_names_; + + // Update Rate + double update_rate_; + double update_period_; + common::Time last_update_time_; + +}; + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN ( GazeboRosJointStatePublisher ) +} + +#endif //JOINT_STATE_PUBLISHER_PLUGIN_HH + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_trajectory.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_trajectory.h new file mode 100755 index 0000000..e9286e1 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_joint_trajectory.h @@ -0,0 +1,133 @@ +/* + * Copyright 2012 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. + * +*/ + +// ************************************************************* +// DEPRECATED +// This class has been renamed to gazebo_ros_joint_pose_trajectory +// ************************************************************* + +#ifndef GAZEBO_ROS_JOINT_TRAJECTORY_PLUGIN_HH +#define GAZEBO_ROS_JOINT_TRAJECTORY_PLUGIN_HH + +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#undef ENABLE_SERVICE +#ifdef ENABLE_SERVICE +#include +#endif + +#include +#include +#include +#include +#include + +namespace gazebo +{ + class GazeboRosJointTrajectory : public ModelPlugin // replaced with GazeboROSJointPoseTrajectory + { + /// \brief Constructor + public: GazeboRosJointTrajectory(); + + /// \brief Destructor + public: virtual ~GazeboRosJointTrajectory(); + + /// \brief Load the controller + public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + /// \brief Update the controller + private: void SetTrajectory( + const trajectory_msgs::JointTrajectory::ConstPtr& trajectory); +#ifdef ENABLE_SERVICE + private: bool SetTrajectory( + const gazebo_msgs::SetJointTrajectory::Request& req, + const gazebo_msgs::SetJointTrajectory::Response& res); +#endif + private: void UpdateStates(); + + private: physics::WorldPtr world_; + private: physics::ModelPtr model_; + + /// \brief pose should be set relative to this link (default to "world") + private: physics::LinkPtr reference_link_; + private: std::string reference_link_name_; + + /// \brief pointer to ros node + private: ros::NodeHandle* rosnode_; + private: ros::Subscriber sub_; + private: ros::ServiceServer srv_; + private: bool has_trajectory_; + + /// \brief ros message + private: trajectory_msgs::JointTrajectory trajectory_msg_; + private: bool set_model_pose_; + private: geometry_msgs::Pose model_pose_; + + /// \brief topic name + private: std::string topic_name_; + private: std::string service_name_; + + /// \brief A mutex to lock access to fields that are + /// used in message callbacks + private: boost::mutex update_mutex; + + /// \brief save last_time + private: common::Time last_time_; + + // trajectory time control + private: common::Time trajectory_start; + private: unsigned int trajectory_index; + + // rate control + private: double update_rate_; + private: bool disable_physics_updates_; + private: bool physics_engine_enabled_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + private: ros::CallbackQueue queue_; + private: void QueueThread(); + private: boost::thread callback_queue_thread_; + + private: std::vector joints_; + private: std::vector points_; + + // Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + + private: trajectory_msgs::JointTrajectory joint_trajectory_; + + // deferred load in case ros is blocking + private: sdf::ElementPtr sdf; + private: void LoadThread(); + private: boost::thread deferred_load_thread_; + }; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h new file mode 100755 index 0000000..d536f2d --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_laser.h @@ -0,0 +1,100 @@ +/* + * Copyright 2012 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_ROS_LASER_HH +#define GAZEBO_ROS_LASER_HH + +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace gazebo +{ + class GazeboRosLaser : public RayPlugin + { + /// \brief Constructor + public: GazeboRosLaser(); + + /// \brief Destructor + public: ~GazeboRosLaser(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Keep track of number of connctions + private: int laser_connect_count_; + private: void LaserConnect(); + private: void LaserDisconnect(); + + // Pointer to the model + GazeboRosPtr gazebo_ros_; + private: std::string world_name_; + private: physics::WorldPtr world_; + /// \brief The parent sensor + private: sensors::RaySensorPtr parent_ray_sensor_; + + /// \brief pointer to ros node + private: ros::NodeHandle* rosnode_; + private: ros::Publisher pub_; + private: PubQueue::Ptr pub_queue_; + + /// \brief topic name + private: std::string topic_name_; + + /// \brief frame transform name, should match link name + private: std::string frame_name_; + + /// \brief tf prefix + private: std::string tf_prefix_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + // deferred load in case ros is blocking + private: sdf::ElementPtr sdf; + private: void LoadThread(); + private: boost::thread deferred_load_thread_; + private: unsigned int seed; + + private: gazebo::transport::NodePtr gazebo_node_; + private: gazebo::transport::SubscriberPtr laser_scan_sub_; + private: void OnScan(ConstLaserScanStampedPtr &_msg); + + /// \brief prevents blocking + private: PubMultiQueue pmq; + }; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h new file mode 100755 index 0000000..aa99705 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_multicamera.h @@ -0,0 +1,65 @@ +/* + * Copyright 2012 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_ROS_MULTICAMERA_HH +#define GAZEBO_ROS_MULTICAMERA_HH + +#include +#include + +// library for processing camera data for gazebo / ros conversions +#include +#include + +namespace gazebo +{ + class GazeboRosMultiCamera : public MultiCameraPlugin + { + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: GazeboRosMultiCamera(); + + /// \brief Destructor + public: ~GazeboRosMultiCamera(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + std::vector utils; + + protected: void OnNewFrame(const unsigned char *_image, + GazeboRosCameraUtils* util); + /// \brief Update the controller + /// FIXME: switch to function vectors + protected: virtual void OnNewFrameLeft(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + protected: virtual void OnNewFrameRight(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// Bookkeeping flags that will be passed into the underlying + /// GazeboRosCameraUtils objects to let them share state about the parent + /// sensor. + private: boost::shared_ptr image_connect_count_; + private: boost::shared_ptr image_connect_count_lock_; + private: boost::shared_ptr was_active_; + }; +} +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h new file mode 100755 index 0000000..850c40d --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_openni_kinect.h @@ -0,0 +1,147 @@ +/* + * 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 GAZEBO_ROS_OPENNI_KINECT_HH +#define GAZEBO_ROS_OPENNI_KINECT_HH + +// ros stuff +#include +#include +#include + +// ros messages stuff +#include +#include +#include +#include +#include +#include + +// gazebo stuff +#include +#include +#include +#include +#include +#include +#include + +// dynamic reconfigure stuff +#include +#include + +// boost stuff +#include + +// camera stuff +#include + +namespace gazebo +{ + class GazeboRosOpenniKinect : public DepthCameraPlugin, GazeboRosCameraUtils + { + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: GazeboRosOpenniKinect(); + + /// \brief Destructor + public: ~GazeboRosOpenniKinect(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Advertise point cloud and depth image + public: virtual void Advertise(); + + /// \brief Update the controller + protected: virtual void OnNewDepthFrame(const float *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief Update the controller + protected: virtual void OnNewImageFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// \brief push point cloud data into ros topic + private: void FillPointdCloud(const float *_src); + + /// \brief push depth image data into ros topic + private: void FillDepthImage(const float *_src); + + /// \brief Keep track of number of connctions for point clouds + private: int point_cloud_connect_count_; + private: void PointCloudConnect(); + private: void PointCloudDisconnect(); + + /// \brief Keep track of number of connctions for point clouds + private: int depth_image_connect_count_; + private: void DepthImageConnect(); + private: void DepthImageDisconnect(); + + private: bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg); + + private: bool FillDepthImageHelper( sensor_msgs::Image& image_msg, + uint32_t height, uint32_t width, + uint32_t step, void* data_arg); + + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + private: ros::Publisher point_cloud_pub_; + private: ros::Publisher depth_image_pub_; + + /// \brief PointCloud2 point cloud message + private: sensor_msgs::PointCloud2 point_cloud_msg_; + private: sensor_msgs::Image depth_image_msg_; + + /// \brief Minimum range of the point cloud + private: double point_cloud_cutoff_; + /// \brief Maximum range of the point cloud + private: double point_cloud_cutoff_max_; + + /// \brief ROS image topic name + private: std::string point_cloud_topic_name_; + + + /// \brief image where each pixel contains the depth data + private: std::string depth_image_topic_name_; + private: common::Time depth_sensor_update_time_; + + // overload with our own + private: std::string depth_image_camera_info_topic_name_; + private: int depth_info_connect_count_; + private: void DepthInfoConnect(); + private: void DepthInfoDisconnect(); + private: common::Time last_depth_image_camera_info_update_time_; + protected: ros::Publisher depth_image_camera_info_pub_; + + using GazeboRosCameraUtils::PublishCameraInfo; + protected: virtual void PublishCameraInfo(); + + private: event::ConnectionPtr load_connection_; + }; + +} +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.h new file mode 100755 index 0000000..c8216a4 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_p3d.h @@ -0,0 +1,133 @@ +/* + * Copyright 2012 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: 3D position interface for ground truth. + * Author: Sachin Chitta and John Hsu + * Date: 1 June 2008 + */ + +#ifndef GAZEBO_ROS_P3D_HH +#define GAZEBO_ROS_P3D_HH + +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace gazebo +{ + class GazeboRosP3D : public ModelPlugin + { + /// \brief Constructor + public: GazeboRosP3D(); + + /// \brief Destructor + public: virtual ~GazeboRosP3D(); + + /// \brief Load the controller + public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Update the controller + protected: virtual void UpdateChild(); + + private: physics::WorldPtr world_; + private: physics::ModelPtr model_; + + /// \brief The parent Model + private: physics::LinkPtr link_; + + /// \brief The body of the frame to display pose, twist + private: physics::LinkPtr reference_link_; + + + /// \brief pointer to ros node + private: ros::NodeHandle* rosnode_; + private: ros::Publisher pub_; + private: PubQueue::Ptr pub_Queue; + + /// \brief ros message + private: nav_msgs::Odometry pose_msg_; + + /// \brief store bodyname + private: std::string link_name_; + + /// \brief topic name + private: std::string topic_name_; + + /// \brief frame transform name, should match link name + /// FIXME: extract link name directly? + private: std::string frame_name_; + private: std::string tf_frame_name_; + + /// \brief allow specifying constant xyz and rpy offsets + private: ignition::math::Pose3d offset_; + + /// \brief mutex to lock access to fields used in message callbacks + private: boost::mutex lock; + + /// \brief save last_time + private: common::Time last_time_; + private: ignition::math::Vector3d last_vpos_; + private: ignition::math::Vector3d last_veul_; + private: ignition::math::Vector3d apos_; + private: ignition::math::Vector3d aeul_; + private: ignition::math::Vector3d last_frame_vpos_; + private: ignition::math::Vector3d last_frame_veul_; + private: ignition::math::Vector3d frame_apos_; + private: ignition::math::Vector3d frame_aeul_; + + // rate control + private: double update_rate_; + + /// \brief Gaussian noise + private: double gaussian_noise_; + + /// \brief Gaussian noise generator + private: double GaussianKernel(double mu, double sigma); + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + private: ros::CallbackQueue p3d_queue_; + private: void P3DQueueThread(); + private: boost::thread callback_queue_thread_; + + // Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + + private: unsigned int seed; + + // ros publish multi queue, prevents publish() blocking + private: PubMultiQueue pmq; + }; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h new file mode 100755 index 0000000..d399443 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_planar_move.h @@ -0,0 +1,99 @@ +/* + * 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: Simple model controller that uses a twist message to move a robot on + * the xy plane. + * Author: Piyush Khandelwal + * Date: 29 July 2013 + */ + +#ifndef GAZEBO_ROS_PLANAR_MOVE_HH +#define GAZEBO_ROS_PLANAR_MOVE_HH + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo { + + class GazeboRosPlanarMove : public ModelPlugin { + + public: + GazeboRosPlanarMove(); + ~GazeboRosPlanarMove(); + void Load(physics::ModelPtr parent, sdf::ElementPtr sdf); + + protected: + virtual void UpdateChild(); + virtual void FiniChild(); + + private: + void publishOdometry(double step_time); + + physics::ModelPtr parent_; + event::ConnectionPtr update_connection_; + + boost::shared_ptr rosnode_; + ros::Publisher odometry_pub_; + ros::Subscriber vel_sub_; + boost::shared_ptr transform_broadcaster_; + nav_msgs::Odometry odom_; + std::string tf_prefix_; + + boost::mutex lock; + + std::string robot_namespace_; + std::string command_topic_; + std::string odometry_topic_; + std::string odometry_frame_; + std::string robot_base_frame_; + double odometry_rate_; + + // Custom Callback Queue + ros::CallbackQueue queue_; + boost::thread callback_queue_thread_; + void QueueThread(); + + // command velocity callback + void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); + + double x_; + double y_; + double rot_; + bool alive_; + common::Time last_odom_publish_time_; + ignition::math::Pose3d last_odom_pose_; + + }; + +} + +#endif /* end of include guard: GAZEBO_ROS_PLANAR_MOVE_HH */ diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.h new file mode 100755 index 0000000..5d72dce --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_projector.h @@ -0,0 +1,127 @@ +/* + * Desc: A dynamic controller plugin that controls texture projection. + * Author: Jared Duke + * Date: 17 June 2010 + * SVN: $Id$ + */ +#ifndef GAZEBO_ROS_PROJECTOR_HH +#define GAZEBO_ROS_PROJECTOR_HH + +// Custom Callback Queue +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +namespace Ogre +{ + class PlaneBoundedVolumeListSceneQuery; + class Frustum; + class Pass; + class SceneNode; +} + +namespace gazebo +{ + +/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins +/// @{ +/** \defgroup GazeboRosProjector Plugin XML Reference and Example + + \brief Ros Texture Projector Controller. + + This is a controller that controls texture projection into the world from a given body. + + Example Usage: + \verbatim + + + ... + + true + 15.0 + stereo_projection_pattern_alpha.png + stereo_projection_pattern_filter.png + projector_controller/image + projector_controller/projector + 0.785398163 + 0.1 + 10 + + + + + \endverbatim + +\{ +*/ + +class GazeboRosProjector : public ModelPlugin +{ + /// \brief Constructor + /// \param parent The parent entity, must be a Model + public: GazeboRosProjector(); + + /// \brief Destructor + public: virtual ~GazeboRosProjector(); + + /// \brief Load the controller + /// \param node XML config node + protected: virtual void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); + + /// \brief pointer to the world + private: physics::WorldPtr world_; + + /// \brief Callback when a texture is published + private: void LoadImage(const std_msgs::String::ConstPtr& imageMsg); + + /// \brief Callbakc when a projector toggle is published + private: void ToggleProjector(const std_msgs::Int32::ConstPtr& projectorMsg); + + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + private: ros::NodeHandle* rosnode_; + private: ros::Subscriber imageSubscriber_; + private: ros::Subscriber projectorSubscriber_; + + /// \brief ROS texture topic name + private: std::string texture_topic_name_; + + /// \brief ROS projector topic name + private: std::string projector_topic_name_; + + /// \brief For setting ROS name space + private: std::string robot_namespace_; + + // Custom Callback Queue + private: ros::CallbackQueue queue_; + private: void QueueThread(); + private: boost::thread callback_queue_thread_; + + private: event::ConnectionPtr add_model_event_; + + private: transport::NodePtr node_; + private: transport::PublisherPtr projector_pub_; +}; + +/** \} */ +/// @} + +} +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_prosilica.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_prosilica.h new file mode 100755 index 0000000..498898d --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_prosilica.h @@ -0,0 +1,104 @@ +/* + * Copyright 2012 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_ROS_PROSILICA_CAMERA_HH +#define GAZEBO_ROS_PROSILICA_CAMERA_HH + +#include + +// library for processing camera data for gazebo / ros conversions +#include +#include + +// ros +#include + +// image components +#include +// used by polled_camera +#include + +// prosilica components +// Stuff in image_common +#include +#include // do: sudo apt-get install ros-hydro-polled-camera +#include + +namespace gazebo +{ + +class GazeboRosProsilica : public CameraPlugin, GazeboRosCameraUtils +{ + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: GazeboRosProsilica(); + + /// \brief Destructor + public: virtual ~GazeboRosProsilica(); + + /// \brief Load the controller + /// \param node XML config node + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief does nothing for now + private: static void mouse_cb(int event, int x, int y, int flags, void* param) { }; + + /// \brief image_transport + private: polled_camera::PublicationServer poll_srv_; // Handles requests in polled mode + + private: std::string mode_; + + private: std::string mode_param_name; +/* + /// \brief Service call to publish images, cam info + private: bool camInfoService(prosilica_camera::CameraInfo::Request &req, + prosilica_camera::CameraInfo::Response &res); + private: bool triggeredGrab(prosilica_camera::PolledImage::Request &req, + prosilica_camera::PolledImage::Response &res); +*/ + + private: void pollCallback(polled_camera::GetPolledImage::Request& req, + polled_camera::GetPolledImage::Response& rsp, + sensor_msgs::Image& image, sensor_msgs::CameraInfo& info); + + /// \brief ros message + /// \brief construct raw stereo message + private: sensor_msgs::Image *roiImageMsg; + private: sensor_msgs::CameraInfo *roiCameraInfoMsg; + + /// \brief ROS image topic name + private: std::string pollServiceName; + + private: void Advertise(); + private: event::ConnectionPtr load_connection_; + + // subscribe to world stats + //private: transport::NodePtr node; + //private: transport::SubscriberPtr statsSub; + //private: common::Time simTime; + //public: void OnStats( const boost::shared_ptr &_msg); + + /// \brief Update the controller + protected: virtual void OnNewImageFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + +}; + +} +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_range.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_range.h new file mode 100755 index 0000000..4d43eae --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_range.h @@ -0,0 +1,145 @@ +/* + * Software License Agreement (Modified BSD License) + * + * Copyright (c) 2013, PAL Robotics, S.L. + * All rights reserved. + * + * 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 PAL Robotics, S.L. 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 Jose Capriles. */ + +#ifndef GAZEBO_ROS_RANGE_H +#define GAZEBO_ROS_RANGE_H + + +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace gazebo +{ + +class GazeboRosRange : public RayPlugin +{ + + /// \brief Constructor + public: GazeboRosRange(); + + /// \brief Destructor + public: ~GazeboRosRange(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Update the controller + protected: virtual void OnNewLaserScans(); + + /// \brief Put range data to the ROS topic + private: void PutRangeData(common::Time &_updateTime); + + /// \brief Keep track of number of connctions + private: int range_connect_count_; + private: void RangeConnect(); + private: void RangeDisconnect(); + + // Pointer to the model + private: physics::WorldPtr world_; + /// \brief The parent sensor + private: sensors::SensorPtr parent_sensor_; + private: sensors::RaySensorPtr parent_ray_sensor_; + + /// \brief pointer to ros node + private: ros::NodeHandle* rosnode_; + private: ros::Publisher pub_; + + /// \brief ros message + private: sensor_msgs::Range range_msg_; + + /// \brief topic name + private: std::string topic_name_; + + /// \brief frame transform name, should match link name + private: std::string frame_name_; + + /// \brief radiation type : ultrasound or infrared + private: std::string radiation_; + + /// \brief sensor field of view + private: double fov_; + /// \brief Gaussian noise + private: double gaussian_noise_; + + /// \brief Gaussian noise generator + private: double GaussianKernel(double mu, double sigma); + + /// \brief mutex to lock access to fields that are used in message callbacks + private: boost::mutex lock_; + + /// \brief hack to mimic hokuyo intensity cutoff of 100 + private: double hokuyo_min_intensity_; + + /// update rate of this sensor + private: double update_rate_; + private: double update_period_; + private: common::Time last_update_time_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + private: ros::CallbackQueue range_queue_; + private: void RangeQueueThread(); + private: boost::thread callback_queue_thread_; + + // deferred load in case ros is blocking + private: sdf::ElementPtr sdf; + private: void LoadThread(); + private: boost::thread deferred_load_thread_; + private: unsigned int seed; +}; +} +#endif // GAZEBO_ROS_RANGE_H diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_skid_steer_drive.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_skid_steer_drive.h new file mode 100755 index 0000000..451c5d9 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_skid_steer_drive.h @@ -0,0 +1,142 @@ +/* + * Copyright (c) 2010, Daniel Hewlett, Antons Rebguns + * All rights reserved. + * + * 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 the 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 Antons Rebguns ''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 Antons Rebguns 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 gazebo_ros_skid_steer_drive.h + * + * \brief A skid steering drive plugin. Inspired by gazebo_ros_diff_drive and SkidSteerDrivePlugin + * + * \author Zdenek Materna (imaterna@fit.vutbr.cz) + * + * $ Id: 06/25/2013 11:23:40 AM materna $ + */ + +#ifndef GAZEBO_ROS_SKID_STEER_DRIVE_H_ +#define GAZEBO_ROS_SKID_STEER_DRIVE_H_ + +#include + +#include +#include + +// ROS +#include +#include +#include +#include +#include +#include + +// Custom Callback Queue +#include +#include + +// Boost +#include +#include + +namespace gazebo { + + class Joint; + class Entity; + + class GazeboRosSkidSteerDrive : public ModelPlugin { + + public: + GazeboRosSkidSteerDrive(); + ~GazeboRosSkidSteerDrive(); + void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + + protected: + virtual void UpdateChild(); + virtual void FiniChild(); + + private: + void publishOdometry(double step_time); + void getWheelVelocities(); + + physics::WorldPtr world; + physics::ModelPtr parent; + event::ConnectionPtr update_connection_; + + std::string left_front_joint_name_; + std::string right_front_joint_name_; + std::string left_rear_joint_name_; + std::string right_rear_joint_name_; + + double wheel_separation_; + double wheel_diameter_; + double torque; + double wheel_speed_[4]; + + physics::JointPtr joints[4]; + + // ROS STUFF + ros::NodeHandle* rosnode_; + ros::Publisher odometry_publisher_; + ros::Subscriber cmd_vel_subscriber_; + tf::TransformBroadcaster *transform_broadcaster_; + nav_msgs::Odometry odom_; + std::string tf_prefix_; + bool broadcast_tf_; + + boost::mutex lock; + + std::string robot_namespace_; + std::string command_topic_; + std::string odometry_topic_; + std::string odometry_frame_; + std::string robot_base_frame_; + + // Custom Callback Queue + ros::CallbackQueue queue_; + boost::thread callback_queue_thread_; + void QueueThread(); + + // DiffDrive stuff + void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); + + double x_; + double rot_; + bool alive_; + + // Update Rate + double update_rate_; + double update_period_; + common::Time last_update_time_; + + double covariance_x_; + double covariance_y_; + double covariance_yaw_; + + }; + +} + + +#endif /* GAZEBO_ROS_SKID_STEER_DRIVE_H_ */ diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.h new file mode 100755 index 0000000..4e1f185 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_template.h @@ -0,0 +1,56 @@ +/* + * 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: 3D position interface. + * Author: Sachin Chitta and John Hsu + * Date: 10 June 2008 + */ + +#ifndef GAZEBO_ROS_TEMPLATE_HH +#define GAZEBO_ROS_TEMPLATE_HH + +#include + +#include +#include +#include +#include +#include + +namespace gazebo +{ + + class GazeboRosTemplate : public ModelPlugin + { + /// \brief Constructor + public: GazeboRosTemplate(); + + /// \brief Destructor + public: virtual ~GazeboRosTemplate(); + + /// \brief Load the controller + public: void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ); + + /// \brief Update the controller + protected: virtual void UpdateChild(); + + }; + +} + +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h new file mode 100755 index 0000000..b6c7ee3 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_tricycle_drive.h @@ -0,0 +1,170 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * 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 the copyright holder 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 gazebo_ros_tricycle_drive.cpp + * \brief A tricycle drive plugin for gazebo. + * \author Markus Bader + * \date 22th of June 2014 + */ + +#ifndef TRICYCLE_DRIVE_PLUGIN_HH +#define TRICYCLE_DRIVE_PLUGIN_HH + +#include + +// Gazebo +#include + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include + +// Custom Callback Queue +#include +#include + +// Boost +#include +#include + +namespace gazebo { + +class Joint; +class Entity; + + +class GazeboRosTricycleDrive : public ModelPlugin { + + class TricycleDriveCmd { + public: + TricycleDriveCmd():speed(0), angle(0) {}; + double speed; + double angle; + }; + + enum OdomSource + { + ENCODER = 0, + WORLD = 1, + }; +public: + GazeboRosTricycleDrive(); + ~GazeboRosTricycleDrive(); + void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + +protected: + virtual void UpdateChild(); + virtual void FiniChild(); + +private: + GazeboRosPtr gazebo_ros_; + physics::ModelPtr parent; + void publishOdometry(double step_time); + void publishWheelTF(); /// publishes the wheel tf's + void publishWheelJointState(); + void motorController(double target_speed, double target_angle, double dt); + + event::ConnectionPtr update_connection_; + + physics::JointPtr joint_steering_; + physics::JointPtr joint_wheel_actuated_; + physics::JointPtr joint_wheel_encoder_left_; + physics::JointPtr joint_wheel_encoder_right_; + + double diameter_encoder_wheel_; + double diameter_actuated_wheel_; + double wheel_acceleration_; + double wheel_deceleration_; + double wheel_speed_tolerance_; + double steering_angle_tolerance_; + double steering_speed_; + double separation_encoder_wheel_; + + OdomSource odom_source_; + double wheel_torque_; + + std::string robot_namespace_; + std::string command_topic_; + std::string odometry_topic_; + std::string odometry_frame_; + std::string robot_base_frame_; + + // ROS STUFF + ros::Publisher odometry_publisher_; + ros::Subscriber cmd_vel_subscriber_; + boost::shared_ptr transform_broadcaster_; + sensor_msgs::JointState joint_state_; + ros::Publisher joint_state_publisher_; + nav_msgs::Odometry odom_; + + boost::mutex lock; + + + // Custom Callback Queue + ros::CallbackQueue queue_; + boost::thread callback_queue_thread_; + void QueueThread(); + + // DiffDrive stuff + void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); + + /// updates the relative robot pose based on the wheel encoders + void UpdateOdometryEncoder(); + + TricycleDriveCmd cmd_; + bool alive_; + geometry_msgs::Pose2D pose_encoder_; + common::Time last_odom_update_; + + // Update Rate + double update_rate_; + double update_period_; + common::Time last_actuator_update_; + + // Flags + bool publishWheelTF_; + bool publishWheelJointState_; + +}; + +} + +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h new file mode 100755 index 0000000..b99debb --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_camera.h @@ -0,0 +1,77 @@ +/* + * Copyright 2017 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_ROS_TRIGGERED_CAMERA_HH +#define GAZEBO_ROS_TRIGGERED_CAMERA_HH + +#include +#include + +// library for processing camera data for gazebo / ros conversions +#include + +#include + +namespace gazebo +{ + class GazeboRosTriggeredMultiCamera; + class GazeboRosTriggeredCamera : public CameraPlugin, GazeboRosCameraUtils + { + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: GazeboRosTriggeredCamera(); + + /// \brief Destructor + public: ~GazeboRosTriggeredCamera(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + /// \brief Load the plugin. + /// \param[in] _parent Take in SDF root element. + /// \param[in] _sdf SDF values. + /// \param[in] _camera_name_suffix Suffix of the camera name. + /// \param[in] _hack_baseline Multiple camera baseline. + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, + const std::string &_camera_name_suffix, + double _hack_baseline); + + /// \brief Update the controller + protected: virtual void OnNewFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + protected: virtual void TriggerCamera(); + + protected: virtual bool CanTriggerCamera(); + + protected: event::ConnectionPtr preRenderConnection_; + + public: void SetCameraEnabled(const bool _enabled); + + protected: void PreRender(); + + protected: int triggered = 0; + + protected: std::mutex mutex; + + friend class GazeboRosTriggeredMultiCamera; + }; +} +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_multicamera.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_multicamera.h new file mode 100755 index 0000000..44769cf --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_triggered_multicamera.h @@ -0,0 +1,67 @@ +/* + * Copyright 2017 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_ROS_TRIGGERED_MULTICAMERA_HH +#define GAZEBO_ROS_TRIGGERED_MULTICAMERA_HH + +#include +#include + +// library for processing camera data for gazebo / ros conversions +#include +#include + +namespace gazebo +{ + class GazeboRosTriggeredMultiCamera : public MultiCameraPlugin + { + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: GazeboRosTriggeredMultiCamera(); + + /// \brief Destructor + public: ~GazeboRosTriggeredMultiCamera(); + + /// \brief Load the plugin + /// \param take in SDF root element + public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); + + std::vector triggered_cameras; + + /// \brief Update the controller + /// FIXME: switch to function vectors + protected: virtual void OnNewFrameLeft(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + protected: virtual void OnNewFrameRight(const unsigned char *_image, + unsigned int _width, unsigned int _height, + unsigned int _depth, const std::string &_format); + + /// Bookkeeping flags that will be passed into the underlying + /// GazeboRosCameraUtils objects to let them share state about the parent + /// sensor. + private: boost::shared_ptr image_connect_count_; + private: boost::shared_ptr image_connect_count_lock_; + private: boost::shared_ptr was_active_; + + protected: event::ConnectionPtr preRenderConnection_; + protected: void SetCameraEnabled(const bool _enabled); + protected: void PreRender(); + }; +} +#endif + diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h new file mode 100755 index 0000000..ed51a72 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_utils.h @@ -0,0 +1,293 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * 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 the copyright holder 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. + *********************************************************************/ + +#ifndef GAZEBO_ROS_UTILS_H +#define GAZEBO_ROS_UTILS_H +#include +#include + +#include +#include +#include +#include +#include + +#ifndef GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST +#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST using std::dynamic_pointer_cast +#endif + +namespace gazebo +{ + +/** +* Accessing model name like suggested by nkoenig at http://answers.gazebosim.org/question/4878/multiple-robots-with-ros-plugins-sensor-plugin-vs/ +* @param parent +* @return accessing model name +**/ +inline std::string GetModelName ( const sensors::SensorPtr &parent ) +{ + std::string modelName; + std::vector values; + std::string scopedName = parent->ScopedName(); + boost::replace_all ( scopedName, "::", "," ); + boost::split ( values, scopedName, boost::is_any_of ( "," ) ); + if ( values.size() < 2 ) { + modelName = ""; + } else { + modelName = values[1]; + } + return modelName; +} + +/** +* @brief Reads the name space tag of a sensor plugin +* @param parent +* @param sdf +* @param pInfo +* @return node namespace +**/ +inline std::string GetRobotNamespace ( const sensors::SensorPtr &parent, const sdf::ElementPtr &sdf, const char *pInfo = NULL ) +{ + std::string name_space; + std::stringstream ss; + if ( sdf->HasElement ( "robotNamespace" ) ) { + name_space = sdf->Get ( "robotNamespace" ); + if ( name_space.empty() ) { + ss << "The 'robotNamespace' param was empty"; + name_space = GetModelName ( parent ); + } else { + ss << "Using the 'robotNamespace' param: '" << name_space << "'"; + } + } else { + ss << "The 'robotNamespace' param did not exit"; + } + if ( pInfo != NULL ) { + ROS_INFO_NAMED("utils", "%s Plugin: %s" , pInfo, ss.str().c_str() ); + } + return name_space; +} + +/** + * Gazebo ros helper class + * The class simplifies the parameter and rosnode handling + * @author Markus Bader + **/ +class GazeboRos +{ +private: + sdf::ElementPtr sdf_; /// sdf to read + std::string plugin_; /// name of the plugin class + std::string namespace_; /// name of the launched node + boost::shared_ptr rosnode_; /// rosnode + std::string tf_prefix_; /// prefix for the ros tf plublisher if not set it uses the namespace_ + std::string info_text; /// info text for log messages to identify the node + /** + * Reads the common plugin parameters used by the constructor + **/ + void readCommonParameter (); +public: + /** + * Constructor + * @param _parent models parent + * @param _sdf sdf to read + * @param _name of the plugin class + **/ + GazeboRos ( physics::ModelPtr &_parent, sdf::ElementPtr _sdf, const std::string &_plugin ) + : sdf_ ( _sdf ), plugin_ ( _plugin ) { + namespace_ = _parent->GetName (); + if ( !sdf_->HasElement ( "robotNamespace" ) ) { + ROS_INFO_NAMED("utils", "%s missing , defaults is %s", plugin_.c_str(), namespace_.c_str() ); + } else { + namespace_ = sdf_->GetElement ( "robotNamespace" )->Get(); + if ( namespace_.empty() ) { + namespace_ = _parent->GetName(); + } + } + if ( !namespace_.empty() ) + this->namespace_ += "/"; + rosnode_ = boost::shared_ptr ( new ros::NodeHandle ( namespace_ ) ); + info_text = plugin_ + "(ns = " + namespace_ + ")"; + readCommonParameter (); + } + /** + * Constructor + * @param _parent sensor parent + * @param _sdf sdf to read + * @param _name of the plugin class + **/ + GazeboRos ( sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_plugin ) + : sdf_ ( _sdf ), plugin_ ( _plugin ) { + + std::stringstream ss; + if ( sdf_->HasElement ( "robotNamespace" ) ) { + namespace_ = sdf_->Get ( "robotNamespace" ); + if ( namespace_.empty() ) { + ss << "the 'robotNamespace' param was empty"; + namespace_ = GetModelName ( _parent ); + } else { + ss << "Using the 'robotNamespace' param: '" << namespace_ << "'"; + } + } else { + ss << "the 'robotNamespace' param did not exit"; + } + info_text = plugin_ + "(ns = " + namespace_ + ")"; + ROS_INFO_NAMED("utils", "%s: %s" , info_text.c_str(), ss.str().c_str() ); + readCommonParameter (); + } + + /** + * Returns info text used for log messages + * @return class name and node name as string + **/ + const char* info() const; + /** + * returns the initialized created within the constuctor + * @return rosnode + **/ + boost::shared_ptr& node();; + /** + * returns the initialized within the constuctor + * @return rosnode + **/ + const boost::shared_ptr& node() const; + /** + * resolves a tf frame name by adding the tf_prefix initialized within the constuctor + * @param _name + * @retun resolved tf name + **/ + std::string resolveTF ( const std::string &_name ); + + /** + * reads the follwoing _tag_name paramer or sets a _default value + * @param _value + * @param _tag_name + * @param _default + * @retun sdf tag value + **/ + void getParameterBoolean ( bool &_value, const char *_tag_name, const bool &_default ); + /** + * reads the follwoing _tag_name paramer + * @param _value + * @param _tag_name + * @retun sdf tag value + **/ + void getParameterBoolean ( bool &_value, const char *_tag_name ); + /** + * retuns a JointPtr based on an sdf tag_name entry + * @param _value + * @param _tag_name + * @param _joint_default_name + * @retun JointPtr + **/ + physics::JointPtr getJoint ( physics::ModelPtr &_parent, const char *_tag_name, const std::string &_joint_default_name ); + /** + * checks if the ros not is initialized + * @retun JointPtr + **/ + void isInitialized(); + + + /** + * reads the follwoing _tag_name paramer or sets a _default value + * @param _value + * @param _tag_name + * @param _default + * @retun sdf tag value + **/ + template + void getParameter ( T &_value, const char *_tag_name, const T &_default ) { + _value = _default; + if ( !sdf_->HasElement ( _tag_name ) ) { + ROS_WARN_NAMED("utils", "%s: missing <%s> default is %s", info(), _tag_name, boost::lexical_cast ( _default ).c_str() ); + } else { + this->getParameter ( _value, _tag_name ); + } + } + /** + * reads the follwoing _tag_name paramer + * @param _value + * @param _tag_name + * @param _default + * @retun sdf tag value + **/ + template + void getParameter ( T &_value, const char *_tag_name ) { + if ( sdf_->HasElement ( _tag_name ) ) { + _value = sdf_->GetElement ( _tag_name )->Get(); + } + ROS_DEBUG_NAMED("utils", "%s: <%s> = %s", info(), _tag_name, boost::lexical_cast ( _value ).c_str() ); + + } + + /** + * reads the following _tag_name paramer and compares the value with an _options map keys and retuns the corresponding value. + * @param _value + * @param _tag_name + * @param _default + * @retun sdf tag value + **/ + template + void getParameter ( T &_value, const char *_tag_name, const std::map &_options, const T &_default ) { + _value = _default; + if ( !sdf_->HasElement ( _tag_name ) ) { + ROS_WARN_NAMED("utils", "%s: missing <%s> default is %s", info(), _tag_name, boost::lexical_cast ( _default ).c_str() ); + } else { + this->getParameter ( _value, _tag_name, _options ); + } + } + /** + * reads the following _tag_name paramer and compares the value with an _options map keys and retuns the corresponding value. + * @param _value + * @param _tag_name + * @param _default + * @retun sdf tag value + **/ + template + void getParameter ( T &_value, const char *_tag_name, const std::map &_options ) { + typename std::map::const_iterator it; + if ( sdf_->HasElement ( _tag_name ) ) { + std::string value = sdf_->GetElement ( _tag_name )->Get(); + it = _options.find ( value ); + if ( it == _options.end() ) { + ROS_WARN_NAMED("utils", "%s: <%s> no matching key to %s", info(), _tag_name, value.c_str() ); + } else { + _value = it->second; + } + } + ROS_DEBUG_NAMED("utils", "%s: <%s> = %s := %s", info(), _tag_name, ( it == _options.end() ?"default":it->first.c_str() ), boost::lexical_cast ( _value ).c_str() ); + } +}; + +typedef boost::shared_ptr GazeboRosPtr; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.h new file mode 100755 index 0000000..b9e6a09 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_vacuum_gripper.h @@ -0,0 +1,161 @@ +/* + * Copyright (C) 2015-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: GazeboVacuumGripper plugin for manipulating objects in Gazebo + * Author: Kentaro Wada + * Date: 7 Dec 2015 + */ + +#ifndef GAZEBO_ROS_VACUUM_GRIPPER_HH +#define GAZEBO_ROS_VACUUM_GRIPPER_HH + +#include + +// Custom Callback Queue +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include + + +namespace gazebo +{ +/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins +/// @{ +/** \defgroup GazeboRosVacuumGripper Plugin XML Reference and Example + + \brief Ros Vacuum Gripper Plugin. + + This is a Plugin that collects data from a ROS topic and applies wrench to a body accordingly. + + Example Usage: + - left_end_effector will be the power point + - revolute type joint is necessary (fixed joint disappears on gazebo and plugin can't find the joint and link) + \verbatim + + 0 + + + + + + + + + + + + + + + + + + + + + + + + /robot/left_vacuum_gripper + left_end_effector + grasping + + + \endverbatim + + +\{ +*/ + + +class GazeboRosVacuumGripper : public ModelPlugin +{ + /// \brief Constructor + public: GazeboRosVacuumGripper(); + + /// \brief Destructor + public: virtual ~GazeboRosVacuumGripper(); + + // Documentation inherited + protected: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + + // Documentation inherited + protected: virtual void UpdateChild(); + + /// \brief The custom callback queue thread function. + private: void QueueThread(); + + private: bool OnServiceCallback(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res); + private: bool OffServiceCallback(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res); + + private: bool status_; + + private: physics::ModelPtr parent_; + + /// \brief A pointer to the gazebo world. + private: physics::WorldPtr world_; + + /// \brief A pointer to the Link, where force is applied + private: physics::LinkPtr link_; + + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + private: ros::NodeHandle* rosnode_; + + /// \brief A mutex to lock access to fields that are used in ROS message callbacks + private: boost::mutex lock_; + private: ros::Publisher pub_; + private: ros::ServiceServer srv1_; + private: ros::ServiceServer srv2_; + + /// \brief ROS Wrench topic name inputs + private: std::string topic_name_; + private: std::string service_name_; + /// \brief The Link this plugin is attached to, and will exert forces on. + private: std::string link_name_; + + /// \brief for setting ROS name space + private: std::string robot_namespace_; + + // Custom Callback Queue + private: ros::CallbackQueue queue_; + /// \brief Thead object for the running callback Thread. + private: boost::thread callback_queue_thread_; + + // Pointer to the update event connection + private: event::ConnectionPtr update_connection_; + + /// \brief: keep track of number of connections + private: int connect_count_; + private: void Connect(); + private: void Disconnect(); +}; +/** \} */ +/// @} +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_video.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_video.h new file mode 100755 index 0000000..1bacd57 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/gazebo_ros_video.h @@ -0,0 +1,100 @@ +/* + * 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: Video plugin for displaying ROS image topics on Ogre textures + * Author: Piyush Khandelwal + * Date: 26 July 2013 + */ + +#ifndef GAZEBO_ROS_VIDEO_H +#define GAZEBO_ROS_VIDEO_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace gazebo +{ + + class VideoVisual : public rendering::Visual + { + public: + VideoVisual( + const std::string &name, rendering::VisualPtr parent, + int height, int width); + virtual ~VideoVisual(); + void render(const cv::Mat& image); + private: + Ogre::TexturePtr texture_; + int height_; + int width_; + }; + + class GazeboRosVideo : public VisualPlugin + { + public: + + GazeboRosVideo(); + virtual ~GazeboRosVideo(); + + void Load(rendering::VisualPtr parent, sdf::ElementPtr sdf); + void processImage(const sensor_msgs::ImageConstPtr &msg); + + protected: + + virtual void UpdateChild(); + + // Pointer to the model + rendering::VisualPtr model_; + // Pointer to the update event connection + event::ConnectionPtr update_connection_; + + boost::shared_ptr video_visual_; + + cv_bridge::CvImagePtr image_; + boost::mutex m_image_; + bool new_image_available_; + + /// \brief A pointer to the ROS node. A node will be instantiated if it does not exist. + ros::NodeHandle* rosnode_; + + // ROS Stuff + ros::Subscriber camera_subscriber_; + std::string robot_namespace_; + std::string topic_name_; + + ros::CallbackQueue queue_; + boost::thread callback_queue_thread_; + void QueueThread(); + + }; + +} + +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/vision_reconfigure.h b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/vision_reconfigure.h new file mode 100755 index 0000000..dceadda --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/include/gazebo_plugins/vision_reconfigure.h @@ -0,0 +1,70 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009-2010, Willow Garage, Inc. +* All rights reserved. +* +* 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 the Willow Garage 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. +*********************************************************************/ + +#ifndef VISION_RECONFIGURE_HH +#define VISION_RECONFIGURE_HH + +#include +#include +#include +#include +#include +#include + +#include + +class VisionReconfigure +{ + public: + VisionReconfigure(); + + ~VisionReconfigure(); + + void ReconfigureCallback(gazebo_plugins::CameraSynchronizerConfig &config, uint32_t level); + void QueueThread(); + void spinOnce(); + void spin(double spin_frequency); + + private: + ros::NodeHandle nh_; + ros::Publisher pub_projector_; + ros::Publisher pub_header_; + dynamic_reconfigure::Server srv_; + std_msgs::Int32 projector_msg_; + ros::CallbackQueue queue_; + boost::thread callback_queue_thread_; + +}; + +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/package.xml b/gazebo_ros_pkgs/gazebo_plugins/package.xml new file mode 100755 index 0000000..d0e1d98 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/package.xml @@ -0,0 +1,55 @@ + + + gazebo_plugins + 2.5.15 + + Robot-independent Gazebo plugins for sensors, motors and dynamic reconfigurable components. + + + Jose Luis Rivero + + BSD, Apache 2.0 + + http://gazebosim.org/tutorials?cat=connect_ros + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs + + John Hsu + + catkin + + gazebo_dev + + gazebo_dev + + gazebo_msgs + geometry_msgs + sensor_msgs + trajectory_msgs + std_srvs + roscpp + rospy + nodelet + angles + nav_msgs + urdf + tf + tf2_ros + dynamic_reconfigure + rosgraph_msgs + image_transport + rosconsole + message_generation + message_runtime + cv_bridge + polled_camera + diagnostic_updater + camera_info_manager + std_msgs + + rostest + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/scripts/gazebo_model b/gazebo_ros_pkgs/gazebo_plugins/scripts/gazebo_model new file mode 100755 index 0000000..5f3382f --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/scripts/gazebo_model @@ -0,0 +1,68 @@ +#!/usr/bin/env python +# +# testing, do not use +# +# Provides quick access to the services exposed by gazebo_ros_factory plugin + +import roslib, time +roslib.load_manifest('gazebo_plugins') + +import rospy, sys +import string + +from gazebo_plugins import gazebo_plugins_interface +from gazebo_plugins.msg import GazeboModel +from geometry_msgs.msg import Pose, Point, Quaternion + +def usage(): + print '''Commands: + load [param|file] [urdf|gazebo] - Load model + delete - Deletes the model named + ''' + sys.exit(1) + +if __name__ == "__main__": + if len(sys.argv) < 13: + print usage() + + if sys.argv[1] == 'load': + if sys.argv[2] == 'param': + print "loading from parameter" + elif sys.argv[2] == 'file': + print "loading from file" + else: + print "invalid syntax, first argument has to be either param or file" + print usage() + + if sys.argv[3] == 'urdf': + print "loading urdf" + elif sys.argv[3] == 'gazebo': + print "loading gazebo model" + else: + print "invalid syntax, xml type has to be either urdf or gazebo" + print usage() + + param_name = sys.argv[4] + model_name = sys.argv[5] + lx = float(sys.argv[6]) + ly = float(sys.argv[7]) + lz = float(sys.argv[8]) + + #FIXME: rotation ignored for now + ax = float(sys.argv[9]) + ay = float(sys.argv[10]) + az = float(sys.argv[11]) + + namespace = sys.argv[12] + + model_msg = GazeboModel(model_name,param_name,GazeboModel.URDF_PARAM_NAME,namespace,Pose(Point(lx,ly,lz),Quaternion())) + success = gazebo_plugins_interface.load_model(model_msg) + print "spawning success",success + + elif sys.argv[1] == 'delete': + print "not implemented yet" + + else: + print "unknown command: ",sys.argv[1] + print usage() + diff --git a/gazebo_ros_pkgs/gazebo_plugins/scripts/set_pose.py b/gazebo_ros_pkgs/gazebo_plugins/scripts/set_pose.py new file mode 100755 index 0000000..eb5fb16 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/scripts/set_pose.py @@ -0,0 +1,218 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# 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 the Willow Garage 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. +# + +## Gazebo send position topic or calls set pose service for ros_sim_iface consumption + +PKG = 'gazebo_plugins' +NAME = 'set_pose' + +import math +import roslib +roslib.load_manifest(PKG) + +import sys, unittest +import os, os.path, threading, time +import rospy, rostest + +from gazebo_plugins.srv import SetPose + +from std_msgs.msg import String +from geometry_msgs.msg import Pose,Quaternion,Point, PoseStamped, PoseWithCovariance, TwistWithCovariance, Twist, Vector3 +from nav_msgs.msg import Odometry +import tf.transformations as tft +from numpy import float64 + +COV = [float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \ + float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \ + float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \ + float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \ + float64(0),float64(0),float64(0),float64(0),float64(0),float64(0), \ + float64(0),float64(0),float64(0),float64(0),float64(0),float64(0) ] + + +def normalize_angle_positive(angle): + return math.fmod(math.fmod(angle, 2*math.pi) + 2*math.pi, 2*math.pi) + +def normalize_angle(angle): + anorm = normalize_angle_positive(angle) + if anorm > math.pi: + anorm -= 2*math.pi + return anorm + +def shortest_angular_distance(angle_from, angle_to): + angle_diff = normalize_angle_positive(angle_to) - normalize_angle_positive(angle_from) + if angle_diff > math.pi: + angle_diff = -(2*math.pi - angle_diff) + return normalize_angle(angle_diff) + +class SimIfaceControl(): + def __init__(self): + self.update_rate=10 + self.timeout=1 + self.target_p = [0,0,0] # position + self.target_q = [0,0,0] # quaternion + self.target_e = [0,0,0] # euler pose + self.wait_topic_initialized = False + self.frame_id = "world" + self.service_name = "set_pose_service" + self.topic_name = "set_pose_topic" + self.use_topic = False; + self.use_service = False; + self.wait_topic_name = "clock" + self.wait_for_topic = False; + + rospy.init_node(NAME, anonymous=True) + + def setPoseService(self,pose_msg): + print "waiting for service to set pose" + rospy.wait_for_service(self.service_name); + try: + set_pose = rospy.ServiceProxy(self.service_name, SetPose) + resp1 = set_pose(pose_msg) + return resp1.success + except rospy.ServiceException, e: + print "service call failed: %s"%e + + def waitTopicInput(self,p3d): + #self.p3d_p = [p3d.pose.pose.position.x, p3d.pose.pose.position.y, p3d.pose.pose.position.z] + #self.p3d_q = [p3d.pose.pose.orientation.x, p3d.pose.pose.orientation.y, p3d.pose.pose.orientation.z, p3d.pose.pose.orientation.w] + #self.p3d_e = tft.euler_from_quaternion(self.p3d_q) + self.wait_topic_initialized = True + + def setPose(self): + # get goal from commandline + for i in range(0,len(sys.argv)): + if sys.argv[i] == '-update_rate': + if len(sys.argv) > i+1: + self.update_rate = float(sys.argv[i+1]) + if sys.argv[i] == '-timeout': + if len(sys.argv) > i+1: + self.timeout = float(sys.argv[i+1]) + if sys.argv[i] == '-x': + if len(sys.argv) > i+1: + self.target_p[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-y': + if len(sys.argv) > i+1: + self.target_p[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-z': + if len(sys.argv) > i+1: + self.target_p[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-R': + if len(sys.argv) > i+1: + self.target_e[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-P': + if len(sys.argv) > i+1: + self.target_e[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-Y': + if len(sys.argv) > i+1: + self.target_e[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-f': + if len(sys.argv) > i+1: + self.frame_id = sys.argv[i+1] + if sys.argv[i] == '-s': + if len(sys.argv) > i+1: + self.service_name = sys.argv[i+1] + self.use_service = True; + if sys.argv[i] == '-t': + if len(sys.argv) > i+1: + self.topic_name = sys.argv[i+1] + self.use_topic = True; + if sys.argv[i] == '-p': + if len(sys.argv) > i+1: + self.wait_topic_name = sys.argv[i+1] + self.wait_for_topic = True; + + # setup rospy + self.pub_set_pose_topic = rospy.Publisher(self.topic_name, Odometry) + rospy.Subscriber(self.wait_topic_name, rospy.AnyMsg, self.waitTopicInput) + + # wait for topic if user requests + if self.wait_for_topic: + while not self.wait_topic_initialized: + time.sleep(0.1) + + # compoose goal message + h = rospy.Header() + h.stamp = rospy.get_rostime() + h.frame_id = self.frame_id + p = Point(self.target_p[0],self.target_p[1],self.target_p[2]) + tmpq = tft.quaternion_from_euler(self.target_e[0],self.target_e[1],self.target_e[2]) + q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3]) + pose = Pose(p,q) + pwc = PoseWithCovariance(pose,COV) + twc = TwistWithCovariance(Twist(Vector3(),Vector3()),COV) + child_frame_id = "" # what should this be? + target_pose = Odometry(h,child_frame_id,pwc,twc) + + if self.use_service: + success = self.setPoseService(target_pose) + + # publish topic if specified + if self.use_topic: + timeout_t = time.time() + self.timeout + while not rospy.is_shutdown() and time.time() < timeout_t: + # publish target pose + self.pub_set_pose_topic.publish(target_pose) + + if self.update_rate > 0: + time.sleep(1.0/self.update_rate) + else: + time.sleep(0.001) + +def print_usage(exit_code = 0): + print '''Commands: + -update_rate - update rate, default to 10 Hz + -timeout - test timeout in seconds. default to 1 seconds + -x + -y + -z + -R + -P + -Y + -f target frame_id + -s set pose service name + -t set pose topic name + -p wait for this ros topic to be published first +''' + +if __name__ == '__main__': + #print usage if not arguments + if len(sys.argv) == 1: + print_usage() + else: + sic = SimIfaceControl() + sic.setPose() + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/scripts/set_wrench.py b/gazebo_ros_pkgs/gazebo_plugins/scripts/set_wrench.py new file mode 100755 index 0000000..7eef8cc --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/scripts/set_wrench.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# 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 the Willow Garage 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. +# + +## Gazebo send wrench topic for gazebo_ros_force consumption + +PKG = 'gazebo_plugins' +NAME = 'set_wrench' + +import math +import roslib +roslib.load_manifest(PKG) + +import sys, unittest +import os, os.path, threading, time +import rospy, rostest + +from geometry_msgs.msg import Wrench, Vector3 +import tf.transformations as tft +from numpy import float64 + +class SimIfaceControl(): + def __init__(self): + self.update_rate=10 + self.target_l = [0,0,0] # linear + self.target_e = [0,0,0] # angular + self.topic_name = "set_wrench_topic" + self.timeout=1 + rospy.init_node(NAME, anonymous=True) + + def setWrench(self): + # get goal from commandline + for i in range(0,len(sys.argv)): + if sys.argv[i] == '-update_rate': + if len(sys.argv) > i+1: + self.update_rate = float(sys.argv[i+1]) + if sys.argv[i] == '-timeout': + if len(sys.argv) > i+1: + self.timeout = float(sys.argv[i+1]) + if sys.argv[i] == '-x': + if len(sys.argv) > i+1: + self.target_l[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-y': + if len(sys.argv) > i+1: + self.target_l[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-z': + if len(sys.argv) > i+1: + self.target_l[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-R': + if len(sys.argv) > i+1: + self.target_e[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-P': + if len(sys.argv) > i+1: + self.target_e[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-Y': + if len(sys.argv) > i+1: + self.target_e[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-t': + if len(sys.argv) > i+1: + self.topic_name = sys.argv[i+1] + + # setup rospy + self.pub_set_wrench_topic = rospy.Publisher(self.topic_name, Wrench) + + # compoose goal message + w = Wrench(Vector3(self.target_l[0],self.target_l[1],self.target_l[2]),Vector3(self.target_e[0],self.target_e[1],self.target_e[2])) + + # publish topic if specified + timeout_t = time.time() + self.timeout + while not rospy.is_shutdown() and time.time() < timeout_t: + # publish target wrench + self.pub_set_wrench_topic.publish(w) + + if self.update_rate > 0: + time.sleep(1.0/self.update_rate) + else: + time.sleep(0.001) + +def print_usage(exit_code = 0): + print '''Commands: + -update_rate - update rate, default to 10 Hz + -timeout - test timeout in seconds. default to 1 seconds + -x + -y + -z + -R + -P + -Y + -t set wrench topic name +''' + +if __name__ == '__main__': + #print usage if not arguments + if len(sys.argv) == 1: + print_usage() + else: + sic = SimIfaceControl() + sic.setWrench() + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/scripts/test_range.py b/gazebo_ros_pkgs/gazebo_plugins/scripts/test_range.py new file mode 100755 index 0000000..95de332 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/scripts/test_range.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python +import rospy +from sensor_msgs.msg import Range + +import unittest + +class TestRangePlugin(unittest.TestCase): + + def test_max_range(self): + msg = rospy.wait_for_message('/sonar2', Range) + self.assertAlmostEqual(msg.range, msg.max_range) + + def test_inside_range(self): + msg = rospy.wait_for_message('/sonar', Range) + self.assertTrue(msg.range < 0.25 and msg.range > 0.22) + +if __name__ == '__main__': + import rostest + PKG_NAME = 'gazebo_plugins' + TEST_NAME = PKG_NAME + 'range_test' + rospy.init_node(TEST_NAME) + rostest.rosrun(PKG_NAME, TEST_NAME, TestRangePlugin) diff --git a/gazebo_ros_pkgs/gazebo_plugins/setup.py b/gazebo_ros_pkgs/gazebo_plugins/setup.py new file mode 100755 index 0000000..2bb982c --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/setup.py @@ -0,0 +1,9 @@ +#!/usr/bin/env python +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup() +d['packages'] = ['gazebo_plugins'] +d['package_dir'] = {'':'src'} + +setup(**d) diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/MultiCameraPlugin.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/MultiCameraPlugin.cpp new file mode 100755 index 0000000..457d7cc --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/MultiCameraPlugin.cpp @@ -0,0 +1,118 @@ +/* + * 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. + * +*/ +#include +#include +#include +#include + +using namespace gazebo; +GZ_REGISTER_SENSOR_PLUGIN(MultiCameraPlugin) + +///////////////////////////////////////////////// +MultiCameraPlugin::MultiCameraPlugin() : SensorPlugin() +{ +} + +///////////////////////////////////////////////// +MultiCameraPlugin::~MultiCameraPlugin() +{ + this->parentSensor.reset(); + this->camera.clear(); +} + +///////////////////////////////////////////////// +void MultiCameraPlugin::Load(sensors::SensorPtr _sensor, + sdf::ElementPtr /*_sdf*/) +{ + if (!_sensor) + gzerr << "Invalid sensor pointer.\n"; + + GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; + this->parentSensor = + dynamic_pointer_cast(_sensor); + + if (!this->parentSensor) + { + gzerr << "MultiCameraPlugin requires a CameraSensor.\n"; + if (dynamic_pointer_cast(_sensor)) + gzmsg << "It is a depth camera sensor\n"; + if (dynamic_pointer_cast(_sensor)) + gzmsg << "It is a camera sensor\n"; + } + + if (!this->parentSensor) + { + gzerr << "MultiCameraPlugin not attached to a camera sensor\n"; + return; + } + + for (unsigned int i = 0; i < this->parentSensor->CameraCount(); ++i) + { + this->camera.push_back(this->parentSensor->Camera(i)); + + // save camera attributes + this->width.push_back(this->camera[i]->ImageWidth()); + this->height.push_back(this->camera[i]->ImageHeight()); + this->depth.push_back(this->camera[i]->ImageDepth()); + this->format.push_back(this->camera[i]->ImageFormat()); + + std::string cameraName = this->parentSensor->Camera(i)->Name(); + // gzdbg << "camera(" << i << ") name [" << cameraName << "]\n"; + + // FIXME: hardcoded 2 camera support only + if (cameraName.find("left") != std::string::npos) + { + this->newFrameConnection.push_back(this->camera[i]->ConnectNewImageFrame( + boost::bind(&MultiCameraPlugin::OnNewFrameLeft, + this, _1, _2, _3, _4, _5))); + } + else if (cameraName.find("right") != std::string::npos) + { + this->newFrameConnection.push_back(this->camera[i]->ConnectNewImageFrame( + boost::bind(&MultiCameraPlugin::OnNewFrameRight, + this, _1, _2, _3, _4, _5))); + } + } + + this->parentSensor->SetActive(true); +} + +///////////////////////////////////////////////// +void MultiCameraPlugin::OnNewFrameLeft(const unsigned char * /*_image*/, + unsigned int /*_width*/, + unsigned int /*_height*/, + unsigned int /*_depth*/, + const std::string &/*_format*/) +{ + /*rendering::Camera::SaveFrame(_image, this->width, + this->height, this->depth, this->format, + "/tmp/camera/me.jpg"); + */ +} + +///////////////////////////////////////////////// +void MultiCameraPlugin::OnNewFrameRight(const unsigned char * /*_image*/, + unsigned int /*_width*/, + unsigned int /*_height*/, + unsigned int /*_depth*/, + const std::string &/*_format*/) +{ + /*rendering::Camera::SaveFrame(_image, this->width, + this->height, this->depth, this->format, + "/tmp/camera/me.jpg"); + */ +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/camera_synchronizer.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/camera_synchronizer.cpp new file mode 100755 index 0000000..c6feba0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/camera_synchronizer.cpp @@ -0,0 +1,48 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009-2010, Willow Garage, Inc. +* All rights reserved. +* +* 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 the Willow Garage 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. +*********************************************************************/ + +#include + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "camera_synchronizer"); + + VisionReconfigure vr; + + double spin_frequency = 100.0; + ROS_INFO_NAMED("camera_synchronizer", "Starting to spin camera_synchronizer at %f Hz...",spin_frequency); + vr.spin(spin_frequency); + + return 0; +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_plugins/__init__.py b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_plugins/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_plugins/gazebo_plugins_interface.py b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_plugins/gazebo_plugins_interface.py new file mode 100755 index 0000000..cf83203 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_plugins/gazebo_plugins_interface.py @@ -0,0 +1,26 @@ +#! /usr/bin/env python +# Wrappers around the services provided by gazebo_ros_factory plugin + +import roslib; roslib.load_manifest('gazebo_plugins') + +import sys + +import rospy +from gazebo_plugins.msg import GazeboModel +from gazebo_plugins.srv import SpawnModel +from gazebo_plugins.srv import DeleteModel +from geometry_msgs.msg import Pose, Point, Quaternion + +def load_model(model_msg): + print "waiting for service spawn_model" + rospy.wait_for_service('spawn_model') + try: + spawn_model= rospy.ServiceProxy('spawn_model', SpawnModel) + resp1 = spawn_model(model_msg) + return resp1.success + except rospy.ServiceException, e: + print "Service call failed: %s"%e + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_block_laser.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_block_laser.cpp new file mode 100755 index 0000000..b330e01 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_block_laser.cpp @@ -0,0 +1,414 @@ +/* + * 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: Ros Block Laser controller. + * Author: Nathan Koenig + * Date: 01 Feb 2007 + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#define EPSILON_DIFF 0.000001 + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosBlockLaser) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosBlockLaser::GazeboRosBlockLaser() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosBlockLaser::~GazeboRosBlockLaser() +{ + //////////////////////////////////////////////////////////////////////////////// + // Finalize the controller / Custom Callback Queue + this->laser_queue_.clear(); + this->laser_queue_.disable(); + this->rosnode_->shutdown(); + this->callback_laser_queue_thread_.join(); + + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosBlockLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + // load plugin + RayPlugin::Load(_parent, _sdf); + + // Get then name of the parent sensor + this->parent_sensor_ = _parent; + + // Get the world name. + std::string worldName = _parent->WorldName(); + this->world_ = physics::get_world(worldName); + +#if GAZEBO_MAJOR_VERSION >= 8 + last_update_time_ = this->world_->SimTime(); +#else + last_update_time_ = this->world_->GetSimTime(); +#endif + + this->node_ = transport::NodePtr(new transport::Node()); + this->node_->Init(worldName); + + GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; + this->parent_ray_sensor_ = dynamic_pointer_cast(this->parent_sensor_); + + if (!this->parent_ray_sensor_) + gzthrow("GazeboRosBlockLaser controller requires a Ray Sensor as its parent"); + + this->robot_namespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; + + if (!_sdf->HasElement("frameName")) + { + ROS_INFO_NAMED("block_laser", "Block laser plugin missing , defaults to /world"); + this->frame_name_ = "/world"; + } + else + this->frame_name_ = _sdf->GetElement("frameName")->Get(); + + if (!_sdf->HasElement("topicName")) + { + ROS_INFO_NAMED("block_laser", "Block laser plugin missing , defaults to /world"); + this->topic_name_ = "/world"; + } + else + this->topic_name_ = _sdf->GetElement("topicName")->Get(); + + if (!_sdf->HasElement("gaussianNoise")) + { + ROS_INFO_NAMED("block_laser", "Block laser plugin missing , defaults to 0.0"); + this->gaussian_noise_ = 0; + } + else + this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->Get(); + + if (!_sdf->HasElement("hokuyoMinIntensity")) + { + ROS_INFO_NAMED("block_laser", "Block laser plugin missing , defaults to 101"); + this->hokuyo_min_intensity_ = 101; + } + else + this->hokuyo_min_intensity_ = _sdf->GetElement("hokuyoMinIntensity")->Get(); + + ROS_DEBUG_NAMED("block_laser", "gazebo_ros_laser plugin should set minimum intensity to %f due to cutoff in hokuyo filters." , this->hokuyo_min_intensity_); + + if (!_sdf->HasElement("updateRate")) + { + ROS_INFO_NAMED("block_laser", "Block laser plugin missing , defaults to 0"); + this->update_rate_ = 0; + } + else + this->update_rate_ = _sdf->GetElement("updateRate")->Get(); + // FIXME: update the update_rate_ + + + this->laser_connect_count_ = 0; + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("block_laser", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + // resolve tf prefix + std::string prefix; + this->rosnode_->getParam(std::string("tf_prefix"), prefix); + this->frame_name_ = tf::resolve(prefix, this->frame_name_); + + // set size of cloud message, starts at 0!! FIXME: not necessary + this->cloud_msg_.points.clear(); + this->cloud_msg_.channels.clear(); + this->cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32()); + + if (this->topic_name_ != "") + { + // Custom Callback Queue + ros::AdvertiseOptions ao = ros::AdvertiseOptions::create( + this->topic_name_,1, + boost::bind( &GazeboRosBlockLaser::LaserConnect,this), + boost::bind( &GazeboRosBlockLaser::LaserDisconnect,this), ros::VoidPtr(), &this->laser_queue_); + this->pub_ = this->rosnode_->advertise(ao); + } + + + // Initialize the controller + + // sensor generation off by default + this->parent_ray_sensor_->SetActive(false); + // start custom queue for laser + this->callback_laser_queue_thread_ = boost::thread( boost::bind( &GazeboRosBlockLaser::LaserQueueThread,this ) ); + +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosBlockLaser::LaserConnect() +{ + this->laser_connect_count_++; + this->parent_ray_sensor_->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosBlockLaser::LaserDisconnect() +{ + this->laser_connect_count_--; + + if (this->laser_connect_count_ == 0) + this->parent_ray_sensor_->SetActive(false); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosBlockLaser::OnNewLaserScans() +{ + if (this->topic_name_ != "") + { + common::Time sensor_update_time = this->parent_sensor_->LastUpdateTime(); + if (sensor_update_time < last_update_time_) + { + ROS_WARN_NAMED("block_laser", "Negative sensor update time difference detected."); + last_update_time_ = sensor_update_time; + } + + if (last_update_time_ < sensor_update_time) + { + this->PutLaserData(sensor_update_time); + last_update_time_ = sensor_update_time; + } + } + else + { + ROS_INFO_NAMED("block_laser", "gazebo_ros_block_laser topic name not set"); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Put laser data to the interface +void GazeboRosBlockLaser::PutLaserData(common::Time &_updateTime) +{ + int i, hja, hjb; + int j, vja, vjb; + double vb, hb; + int j1, j2, j3, j4; // four corners indices + double r1, r2, r3, r4, r; // four corner values + interpolated range + double intensity; + + this->parent_ray_sensor_->SetActive(false); + + auto maxAngle = this->parent_ray_sensor_->AngleMax(); + auto minAngle = this->parent_ray_sensor_->AngleMin(); + + double maxRange = this->parent_ray_sensor_->RangeMax(); + double minRange = this->parent_ray_sensor_->RangeMin(); + int rayCount = this->parent_ray_sensor_->RayCount(); + int rangeCount = this->parent_ray_sensor_->RangeCount(); + + int verticalRayCount = this->parent_ray_sensor_->VerticalRayCount(); + int verticalRangeCount = this->parent_ray_sensor_->VerticalRangeCount(); + auto verticalMaxAngle = this->parent_ray_sensor_->VerticalAngleMax(); + auto verticalMinAngle = this->parent_ray_sensor_->VerticalAngleMin(); + + double yDiff = maxAngle.Radian() - minAngle.Radian(); + double pDiff = verticalMaxAngle.Radian() - verticalMinAngle.Radian(); + + + // set size of cloud message everytime! + //int r_size = rangeCount * verticalRangeCount; + this->cloud_msg_.points.clear(); + this->cloud_msg_.channels.clear(); + this->cloud_msg_.channels.push_back(sensor_msgs::ChannelFloat32()); + + /***************************************************************/ + /* */ + /* point scan from laser */ + /* */ + /***************************************************************/ + boost::mutex::scoped_lock sclock(this->lock); + // Add Frame Name + this->cloud_msg_.header.frame_id = this->frame_name_; + this->cloud_msg_.header.stamp.sec = _updateTime.sec; + this->cloud_msg_.header.stamp.nsec = _updateTime.nsec; + + for (j = 0; j= 0 && vja < verticalRayCount); + assert(vjb >= 0 && vjb < verticalRayCount); + + for (i = 0; i= 0 && hja < rayCount); + assert(hjb >= 0 && hjb < rayCount); + + // indices of 4 corners + j1 = hja + vja * rayCount; + j2 = hjb + vja * rayCount; + j3 = hja + vjb * rayCount; + j4 = hjb + vjb * rayCount; + // range readings of 4 corners + r1 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j1) , maxRange-minRange); + r2 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j2) , maxRange-minRange); + r3 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j3) , maxRange-minRange); + r4 = std::min(this->parent_ray_sensor_->LaserShape()->GetRange(j4) , maxRange-minRange); + + // Range is linear interpolation if values are close, + // and min if they are very different + r = (1-vb)*((1 - hb) * r1 + hb * r2) + + vb *((1 - hb) * r3 + hb * r4); + + // Intensity is averaged + intensity = 0.25*(this->parent_ray_sensor_->LaserShape()->GetRetro(j1) + + this->parent_ray_sensor_->LaserShape()->GetRetro(j2) + + this->parent_ray_sensor_->LaserShape()->GetRetro(j3) + + this->parent_ray_sensor_->LaserShape()->GetRetro(j4)); + + // std::cout << " block debug " + // << " ij("<cloud_msg_.points.push_back(point); + } + else + { + geometry_msgs::Point32 point; + //pAngle is rotated by yAngle: + point.x = r * cos(pAngle) * cos(yAngle) + this->GaussianKernel(0,this->gaussian_noise_); + point.y = r * cos(pAngle) * sin(yAngle) + this->GaussianKernel(0,this->gaussian_noise_); + point.z = r * sin(pAngle) + this->GaussianKernel(0,this->gaussian_noise_); + this->cloud_msg_.points.push_back(point); + } // only 1 channel + + this->cloud_msg_.channels[0].values.push_back(intensity + this->GaussianKernel(0,this->gaussian_noise_)) ; + } + } + this->parent_ray_sensor_->SetActive(true); + + // send data out via ros message + this->pub_.publish(this->cloud_msg_); + + + +} + + +////////////////////////////////////////////////////////////////////////////// +// Utility for adding noise +double GazeboRosBlockLaser::GaussianKernel(double mu,double sigma) +{ + // using Box-Muller transform to generate two independent standard normally disbributed normal variables + // see wikipedia + double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable + double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable + double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V); + //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable + // we'll just use X + // scale to our mu and sigma + X = sigma * X + mu; + return X; +} + +// Custom Callback Queue +//////////////////////////////////////////////////////////////////////////////// +// custom callback queue thread +void GazeboRosBlockLaser::LaserQueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + { + this->laser_queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +void GazeboRosBlockLaser::OnStats( const boost::shared_ptr &_msg) +{ + this->sim_time_ = msgs::Convert( _msg->sim_time() ); + + ignition::math::Pose3d pose; + pose.Pos().X() = 0.5*sin(0.01*this->sim_time_.Double()); + gzdbg << "plugin simTime [" << this->sim_time_.Double() << "] update pose [" << pose.Pos().X() << "]\n"; +} + + +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_bumper.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_bumper.cpp new file mode 100755 index 0000000..2c5ba04 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_bumper.cpp @@ -0,0 +1,330 @@ +/* + * 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: Bumper controller + * Author: Nate Koenig + * Date: 09 Sept. 2008 + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosBumper) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosBumper::GazeboRosBumper() : SensorPlugin() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosBumper::~GazeboRosBumper() +{ + this->rosnode_->shutdown(); + this->callback_queue_thread_.join(); + + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosBumper::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; + this->parentSensor = dynamic_pointer_cast(_parent); + if (!this->parentSensor) + { + ROS_ERROR_NAMED("bumper", "Contact sensor parent is not of type ContactSensor"); + return; + } + + this->robot_namespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + this->robot_namespace_ = + _sdf->GetElement("robotNamespace")->Get() + "/"; + + // "publishing contact/collisions to this topic name: " + // << this->bumper_topic_name_ << std::endl; + this->bumper_topic_name_ = "bumper_states"; + if (_sdf->HasElement("bumperTopicName")) + this->bumper_topic_name_ = + _sdf->GetElement("bumperTopicName")->Get(); + + // "transform contact/collisions pose, forces to this body (link) name: " + // << this->frame_name_ << std::endl; + if (!_sdf->HasElement("frameName")) + { + ROS_INFO_NAMED("bumper", "bumper plugin missing , defaults to world"); + this->frame_name_ = "world"; + } + else + this->frame_name_ = _sdf->GetElement("frameName")->Get(); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("bumper", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + // resolve tf prefix + std::string prefix; + this->rosnode_->getParam(std::string("tf_prefix"), prefix); + this->frame_name_ = tf::resolve(prefix, this->frame_name_); + + this->contact_pub_ = this->rosnode_->advertise( + std::string(this->bumper_topic_name_), 1); + + // Initialize + // start custom queue for contact bumper + this->callback_queue_thread_ = boost::thread( + boost::bind(&GazeboRosBumper::ContactQueueThread, this)); + + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->update_connection_ = this->parentSensor->ConnectUpdated( + boost::bind(&GazeboRosBumper::OnContact, this)); + + // Make sure the parent sensor is active. + this->parentSensor->SetActive(true); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosBumper::OnContact() +{ + if (this->contact_pub_.getNumSubscribers() <= 0) + return; + + msgs::Contacts contacts; + contacts = this->parentSensor->Contacts(); + /// \TODO: need a time for each Contact in i-loop, they may differ + this->contact_state_msg_.header.frame_id = this->frame_name_; + this->contact_state_msg_.header.stamp = ros::Time(contacts.time().sec(), + contacts.time().nsec()); + +/* + /// \TODO: get frame_name_ transforms from tf or gazebo + /// and rotate results to local frame. for now, results are reported in world frame. + + // if frameName specified is "world", "/map" or "map" report back + // inertial values in the gazebo world + physics::LinkPtr myFrame; + if (myFrame == NULL && this->frame_name_ != "world" && + this->frame_name_ != "/map" && this->frame_name_ != "map") + { + // lock in case a model is being spawned + boost::recursive_mutex::scoped_lock lock( + *Simulator::Instance()->GetMRMutex()); + // look through all models in the world, search for body + // name that matches frameName + physics::Model_V all_models = World::Instance()->Models(); + for (physics::Model_V::iterator iter = all_models.begin(); + iter != all_models.end(); iter++) + { + if (*iter) myFrame = + boost::dynamic_pointer_cast((*iter)->GetLink(this->frame_name_)); + if (myFrame) break; + } + + // not found + if (!myFrame) + { + ROS_INFO_NAMED("bumper", "gazebo_ros_bumper plugin: frameName: %s does not exist" + " yet, will not publish\n",this->frame_name_.c_str()); + return; + } + } +*/ + // get reference frame (body(link)) pose and subtract from it to get + // relative force, torque, position and normal vectors + ignition::math::Pose3d pose, frame_pose; + ignition::math::Quaterniond rot, frame_rot; + ignition::math::Vector3d pos, frame_pos; + /* + if (myFrame) + { + frame_pose = myFrame->WorldPose(); //-this->myBody->GetCoMPose(); + frame_pos = frame_pose.Pos(); + frame_rot = frame_pose.Rot(); + } + else + */ + { + // no specific frames specified, use identity pose, keeping + // relative frame at inertial origin + frame_pos = ignition::math::Vector3d(0, 0, 0); + frame_rot = ignition::math::Quaterniond(1, 0, 0, 0); // gazebo u,x,y,z == identity + frame_pose = ignition::math::Pose3d(frame_pos, frame_rot); + } + + + + // set contact states size + this->contact_state_msg_.states.clear(); + + // GetContacts returns all contacts on the collision body + unsigned int contactsPacketSize = contacts.contact_size(); + for (unsigned int i = 0; i < contactsPacketSize; ++i) + { + + // For each collision contact + // Create a ContactState + gazebo_msgs::ContactState state; + /// \TODO: + gazebo::msgs::Contact contact = contacts.contact(i); + + state.collision1_name = contact.collision1(); + state.collision2_name = contact.collision2(); + std::ostringstream stream; + stream << "Debug: i:(" << i << "/" << contactsPacketSize + << ") my geom:" << state.collision1_name + << " other geom:" << state.collision2_name + << " time:" << ros::Time(contact.time().sec(), contact.time().nsec()) + << std::endl; + state.info = stream.str(); + + state.wrenches.clear(); + state.contact_positions.clear(); + state.contact_normals.clear(); + state.depths.clear(); + + // sum up all wrenches for each DOF + geometry_msgs::Wrench total_wrench; + total_wrench.force.x = 0; + total_wrench.force.y = 0; + total_wrench.force.z = 0; + total_wrench.torque.x = 0; + total_wrench.torque.y = 0; + total_wrench.torque.z = 0; + + unsigned int contactGroupSize = contact.position_size(); + for (unsigned int j = 0; j < contactGroupSize; ++j) + { + // loop through individual contacts between collision1 and collision2 + // gzerr << j << " Position:" + // << contact.position(j).x() << " " + // << contact.position(j).y() << " " + // << contact.position(j).z() << "\n"; + // gzerr << " Normal:" + // << contact.normal(j).x() << " " + // << contact.normal(j).y() << " " + // << contact.normal(j).z() << "\n"; + // gzerr << " Depth:" << contact.depth(j) << "\n"; + + // Get force, torque and rotate into user specified frame. + // frame_rot is identity if world is used (default for now) + ignition::math::Vector3d force = frame_rot.RotateVectorReverse(ignition::math::Vector3d( + contact.wrench(j).body_1_wrench().force().x(), + contact.wrench(j).body_1_wrench().force().y(), + contact.wrench(j).body_1_wrench().force().z())); + ignition::math::Vector3d torque = frame_rot.RotateVectorReverse(ignition::math::Vector3d( + contact.wrench(j).body_1_wrench().torque().x(), + contact.wrench(j).body_1_wrench().torque().y(), + contact.wrench(j).body_1_wrench().torque().z())); + + // set wrenches + geometry_msgs::Wrench wrench; + wrench.force.x = force.X(); + wrench.force.y = force.Y(); + wrench.force.z = force.Z(); + wrench.torque.x = torque.X(); + wrench.torque.y = torque.Y(); + wrench.torque.z = torque.Z(); + state.wrenches.push_back(wrench); + + total_wrench.force.x += wrench.force.x; + total_wrench.force.y += wrench.force.y; + total_wrench.force.z += wrench.force.z; + total_wrench.torque.x += wrench.torque.x; + total_wrench.torque.y += wrench.torque.y; + total_wrench.torque.z += wrench.torque.z; + + // transform contact positions into relative frame + // set contact positions + ignition::math::Vector3d position = frame_rot.RotateVectorReverse( + ignition::math::Vector3d(contact.position(j).x(), + contact.position(j).y(), + contact.position(j).z()) - frame_pos); + geometry_msgs::Vector3 contact_position; + contact_position.x = position.X(); + contact_position.y = position.Y(); + contact_position.z = position.Z(); + state.contact_positions.push_back(contact_position); + + // rotate normal into user specified frame. + // frame_rot is identity if world is used. + ignition::math::Vector3d normal = frame_rot.RotateVectorReverse( + ignition::math::Vector3d(contact.normal(j).x(), + contact.normal(j).y(), + contact.normal(j).z())); + // set contact normals + geometry_msgs::Vector3 contact_normal; + contact_normal.x = normal.X(); + contact_normal.y = normal.Y(); + contact_normal.z = normal.Z(); + state.contact_normals.push_back(contact_normal); + + // set contact depth, interpenetration + state.depths.push_back(contact.depth(j)); + } + + state.total_wrench = total_wrench; + this->contact_state_msg_.states.push_back(state); + } + + this->contact_pub_.publish(this->contact_state_msg_); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Put laser data to the interface +void GazeboRosBumper::ContactQueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + { + this->contact_queue_.callAvailable(ros::WallDuration(timeout)); + } +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_camera.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_camera.cpp new file mode 100755 index 0000000..8a59a12 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_camera.cpp @@ -0,0 +1,113 @@ +/* + * 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. + * +*/ + +/* + @mainpage + Desc: GazeboRosCamera plugin for simulating cameras in Gazebo + Author: John Hsu + Date: 24 Sept 2008 +*/ + +#include "gazebo_plugins/gazebo_ros_camera.h" + +#include + +#include +#include +#include + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosCamera) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosCamera::GazeboRosCamera() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosCamera::~GazeboRosCamera() +{ + ROS_DEBUG_STREAM_NAMED("camera","Unloaded"); +} + +void GazeboRosCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("camera", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + CameraPlugin::Load(_parent, _sdf); + // copying from CameraPlugin 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->camera; + + GazeboRosCameraUtils::Load(_parent, _sdf); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosCamera::OnNewFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ + common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime(); + + if (!this->parentSensor->IsActive()) + { + if ((*this->image_connect_count_) > 0) + // do this first so there's chance for sensor to run once after activated + this->parentSensor->SetActive(true); + } + else + { + if ((*this->image_connect_count_) > 0) + { + if (sensor_update_time < this->last_update_time_) + { + ROS_WARN_NAMED("camera", "Negative sensor update time difference detected."); + this->last_update_time_ = sensor_update_time; + } + + // OnNewFrame is triggered at the gazebo sensor + // while there is also a plugin that can throttle the + // rate down further (but then why not reduce the sensor rate? + // what is the use case?). + // Setting the to zero will make this plugin + // update at the gazebo sensor , update_period_ will be + // zero and the conditional always will be true. + if (sensor_update_time - this->last_update_time_ >= this->update_period_) + { + this->PutCameraData(_image, sensor_update_time); + this->PublishCameraInfo(sensor_update_time); + this->last_update_time_ = sensor_update_time; + } + } + } +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_camera_utils.cpp new file mode 100755 index 0000000..ae744b4 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_camera_utils.cpp @@ -0,0 +1,686 @@ +/* + * 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. + * +*/ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "gazebo_plugins/gazebo_ros_camera_utils.h" + +namespace gazebo +{ +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosCameraUtils::GazeboRosCameraUtils() +{ + this->last_update_time_ = common::Time(0); + this->last_info_update_time_ = common::Time(0); + this->height_ = 0; + this->width_ = 0; + this->skip_ = 0; + this->format_ = ""; + this->initialized_ = false; +} + +void GazeboRosCameraUtils::configCallback( + gazebo_plugins::GazeboRosCameraConfig &config, uint32_t level) +{ + if (this->initialized_) + { + ROS_INFO_NAMED("camera_utils", "Reconfigure request for the gazebo ros camera_: %s. New rate: %.2f", + this->camera_name_.c_str(), config.imager_rate); + this->parentSensor_->SetUpdateRate(config.imager_rate); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosCameraUtils::~GazeboRosCameraUtils() +{ + this->parentSensor_->SetActive(false); + this->rosnode_->shutdown(); + this->camera_queue_.clear(); + this->camera_queue_.disable(); + this->callback_queue_thread_.join(); + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent, + sdf::ElementPtr _sdf, + const std::string &_camera_name_suffix, + double _hack_baseline) +{ + // default Load: + // provide _camera_name_suffix to prevent LoadThread() creating the ros::NodeHandle with + //an incomplete this->camera_name_ namespace. There was a race condition when the _camera_name_suffix + //was appended in this function. + this->Load(_parent, _sdf, _camera_name_suffix); + + // overwrite hack baseline if specified at load + // example usage in gazebo_ros_multicamera + this->hack_baseline_ = _hack_baseline; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent, + sdf::ElementPtr _sdf, + const std::string &_camera_name_suffix) +{ + // Get the world name. + std::string world_name = _parent->WorldName(); + + // Get the world_ + this->world_ = physics::get_world(world_name); + + // save pointers + this->sdf = _sdf; + + // maintain for one more release for backwards compatibility with + // pr2_gazebo_plugins + this->world = this->world_; + + std::stringstream ss; + this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Camera"); + + this->image_topic_name_ = "image_raw"; + if (this->sdf->HasElement("imageTopicName")) + this->image_topic_name_ = this->sdf->Get("imageTopicName"); + + this->trigger_topic_name_ = "image_trigger"; + if (this->sdf->HasElement("triggerTopicName")) + this->trigger_topic_name_ = this->sdf->Get("triggerTopicName"); + + this->camera_info_topic_name_ = "camera_info"; + if (this->sdf->HasElement("cameraInfoTopicName")) + this->camera_info_topic_name_ = + this->sdf->Get("cameraInfoTopicName"); + + if (!this->sdf->HasElement("cameraName")) + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , default to empty"); + else + this->camera_name_ = this->sdf->Get("cameraName"); + + // overwrite camera suffix + // example usage in gazebo_ros_multicamera + this->camera_name_ += _camera_name_suffix; + + if (!this->sdf->HasElement("frameName")) + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to /world"); + else + this->frame_name_ = this->sdf->Get("frameName"); + + if (!this->sdf->HasElement("updateRate")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to unlimited (0)."); + this->update_rate_ = 0; + } + else + this->update_rate_ = this->sdf->Get("updateRate"); + + if (!this->sdf->HasElement("CxPrime")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); + this->cx_prime_ = 0; + } + else + this->cx_prime_ = this->sdf->Get("CxPrime"); + + if (!this->sdf->HasElement("Cx")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); + this->cx_= 0; + } + else + this->cx_ = this->sdf->Get("Cx"); + + if (!this->sdf->HasElement("Cy")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); + this->cy_= 0; + } + else + this->cy_ = this->sdf->Get("Cy"); + + if (!this->sdf->HasElement("focalLength")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); + this->focal_length_= 0; + } + else + this->focal_length_ = this->sdf->Get("focalLength"); + + if (!this->sdf->HasElement("hackBaseline")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); + this->hack_baseline_= 0; + } + else + this->hack_baseline_ = this->sdf->Get("hackBaseline"); + + if (!this->sdf->HasElement("distortionK1")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); + this->distortion_k1_= 0; + } + else + this->distortion_k1_ = this->sdf->Get("distortionK1"); + + if (!this->sdf->HasElement("distortionK2")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); + this->distortion_k2_= 0; + } + else + this->distortion_k2_ = this->sdf->Get("distortionK2"); + + if (!this->sdf->HasElement("distortionK3")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); + this->distortion_k3_= 0; + } + else + this->distortion_k3_ = this->sdf->Get("distortionK3"); + + if (!this->sdf->HasElement("distortionT1")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); + this->distortion_t1_= 0; + } + else + this->distortion_t1_ = this->sdf->Get("distortionT1"); + + if (!this->sdf->HasElement("distortionT2")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to 0"); + this->distortion_t2_= 0; + } + else + this->distortion_t2_ = this->sdf->Get("distortionT2"); + + if (!this->sdf->HasElement("borderCrop")) + { + ROS_DEBUG_NAMED("camera_utils", "Camera plugin missing , defaults to true"); + this->border_crop_ = true; + } + else + this->border_crop_ = this->sdf->Get("borderCrop"); + + // initialize shared_ptr members + if (!this->image_connect_count_) this->image_connect_count_ = boost::shared_ptr(new int(0)); + if (!this->image_connect_count_lock_) this->image_connect_count_lock_ = boost::shared_ptr(new boost::mutex); + if (!this->was_active_) this->was_active_ = boost::shared_ptr(new bool(false)); + + // ros callback queue for processing subscription + this->deferred_load_thread_ = boost::thread( + boost::bind(&GazeboRosCameraUtils::LoadThread, this)); +} + +event::ConnectionPtr GazeboRosCameraUtils::OnLoad(const boost::function& load_function) +{ + return load_event_.Connect(load_function); +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosCameraUtils::LoadThread() +{ + // Exit if no ROS + if (!ros::isInitialized()) + { + gzerr << "Not loading plugin since ROS hasn't been " + << "properly initialized. Try starting gazebo with ros plugin:\n" + << " gazebo -s libgazebo_ros_api_plugin.so\n"; + return; + } + + // Sensor generation off by default. Must do this before advertising the + // associated ROS topics. + this->parentSensor_->SetActive(false); + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_ + "/" + this->camera_name_); + + // initialize camera_info_manager + this->camera_info_manager_.reset(new camera_info_manager::CameraInfoManager( + *this->rosnode_, this->camera_name_)); + + this->itnode_ = new image_transport::ImageTransport(*this->rosnode_); + + // resolve tf prefix + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); + if(this->tf_prefix_.empty()) { + this->tf_prefix_ = this->robot_namespace_; + boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/")); + } + this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); + + ROS_INFO_NAMED("camera_utils", "Camera Plugin (ns = %s) , set to \"%s\"", + this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); + + + if (!this->camera_name_.empty()) + { + dyn_srv_ = + new dynamic_reconfigure::Server + (*this->rosnode_); + dynamic_reconfigure::Server + ::CallbackType f = + boost::bind(&GazeboRosCameraUtils::configCallback, this, _1, _2); + dyn_srv_->setCallback(f); + } + else + { + ROS_WARN_NAMED("camera_utils", "dynamic reconfigure is not enabled for this image topic [%s]" + " becuase is not specified", + this->image_topic_name_.c_str()); + } + + this->image_pub_ = this->itnode_->advertise( + this->image_topic_name_, 2, + boost::bind(&GazeboRosCameraUtils::ImageConnect, this), + boost::bind(&GazeboRosCameraUtils::ImageDisconnect, this), + ros::VoidPtr(), true); + + // camera info publish rate will be synchronized to image sensor + // publish rates. + // If someone connects to camera_info, sensor will be activated + // and camera_info will be published alongside image_raw with the + // same timestamps. This incurrs additional computational cost when + // there are subscribers to camera_info, but better mimics behavior + // of image_pipeline. + ros::AdvertiseOptions cio = + ros::AdvertiseOptions::create( + this->camera_info_topic_name_, 2, + boost::bind(&GazeboRosCameraUtils::ImageConnect, this), + boost::bind(&GazeboRosCameraUtils::ImageDisconnect, this), + ros::VoidPtr(), &this->camera_queue_); + this->camera_info_pub_ = this->rosnode_->advertise(cio); + + /* disabling fov and rate setting for each camera + ros::SubscribeOptions zoom_so = + ros::SubscribeOptions::create( + "set_hfov", 1, + boost::bind(&GazeboRosCameraUtils::SetHFOV, this, _1), + ros::VoidPtr(), &this->camera_queue_); + this->cameraHFOVSubscriber_ = this->rosnode_->subscribe(zoom_so); + + ros::SubscribeOptions rate_so = + ros::SubscribeOptions::create( + "set_update_rate", 1, + boost::bind(&GazeboRosCameraUtils::SetUpdateRate, this, _1), + ros::VoidPtr(), &this->camera_queue_); + this->cameraUpdateRateSubscriber_ = this->rosnode_->subscribe(rate_so); + */ + + if (this->CanTriggerCamera()) + { + ros::SubscribeOptions trigger_so = + ros::SubscribeOptions::create( + this->trigger_topic_name_, 1, + boost::bind(&GazeboRosCameraUtils::TriggerCameraInternal, this, _1), + ros::VoidPtr(), &this->camera_queue_); + this->trigger_subscriber_ = this->rosnode_->subscribe(trigger_so); + } + + this->Init(); +} + +void GazeboRosCameraUtils::TriggerCamera() +{ +} + +bool GazeboRosCameraUtils::CanTriggerCamera() +{ + return false; +} + +void GazeboRosCameraUtils::TriggerCameraInternal( + const std_msgs::Empty::ConstPtr &dummy) +{ + TriggerCamera(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Set Horizontal Field of View +void GazeboRosCameraUtils::SetHFOV(const std_msgs::Float64::ConstPtr& hfov) +{ + this->camera_->SetHFOV(ignition::math::Angle(hfov->data)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Set Update Rate +void GazeboRosCameraUtils::SetUpdateRate( + const std_msgs::Float64::ConstPtr& update_rate) +{ + this->parentSensor_->SetUpdateRate(update_rate->data); +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosCameraUtils::ImageConnect() +{ + boost::mutex::scoped_lock lock(*this->image_connect_count_lock_); + + // upon first connection, remember if camera was active. + if ((*this->image_connect_count_) == 0) + *this->was_active_ = this->parentSensor_->IsActive(); + + (*this->image_connect_count_)++; + + this->parentSensor_->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosCameraUtils::ImageDisconnect() +{ + boost::mutex::scoped_lock lock(*this->image_connect_count_lock_); + + (*this->image_connect_count_)--; + + // if there are no more subscribers, but camera was active to begin with, + // leave it active. Use case: this could be a multicamera, where + // each camera shares the same parentSensor_. + if ((*this->image_connect_count_) <= 0 && !*this->was_active_) + this->parentSensor_->SetActive(false); +} + +//////////////////////////////////////////////////////////////////////////////// +// Initialize the controller +void GazeboRosCameraUtils::Init() +{ + // prepare to throttle this plugin at the same rate + // ideally, we should invoke a plugin update when the sensor updates, + // have to think about how to do that properly later + if (this->update_rate_ > 0.0) + this->update_period_ = 1.0/this->update_rate_; + else + this->update_period_ = 0.0; + + // set buffer size + if (this->format_ == "L8" || this->format_ == "L_INT8") + { + this->type_ = sensor_msgs::image_encodings::MONO8; + this->skip_ = 1; + } + else if (this->format_ == "L16" || this->format_ == "L_INT16") + { + this->type_ = sensor_msgs::image_encodings::MONO16; + this->skip_ = 2; + } + else if (this->format_ == "R8G8B8" || this->format_ == "RGB_INT8") + { + this->type_ = sensor_msgs::image_encodings::RGB8; + this->skip_ = 3; + } + else if (this->format_ == "B8G8R8" || this->format_ == "BGR_INT8") + { + this->type_ = sensor_msgs::image_encodings::BGR8; + this->skip_ = 3; + } + else if (this->format_ == "R16G16B16" || this->format_ == "RGB_INT16") + { + this->type_ = sensor_msgs::image_encodings::RGB16; + this->skip_ = 6; + } + else if (this->format_ == "BAYER_RGGB8") + { + ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive."); + this->type_ = sensor_msgs::image_encodings::BAYER_RGGB8; + this->skip_ = 1; + } + else if (this->format_ == "BAYER_BGGR8") + { + ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive."); + this->type_ = sensor_msgs::image_encodings::BAYER_BGGR8; + this->skip_ = 1; + } + else if (this->format_ == "BAYER_GBRG8") + { + ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive."); + this->type_ = sensor_msgs::image_encodings::BAYER_GBRG8; + this->skip_ = 1; + } + else if (this->format_ == "BAYER_GRBG8") + { + ROS_INFO_NAMED("camera_utils", "bayer simulation maybe computationally expensive."); + this->type_ = sensor_msgs::image_encodings::BAYER_GRBG8; + this->skip_ = 1; + } + else + { + ROS_ERROR_NAMED("camera_utils", "Unsupported Gazebo ImageFormat\n"); + this->type_ = sensor_msgs::image_encodings::BGR8; + this->skip_ = 3; + } + + /// Compute camera_ parameters if set to 0 + if (this->cx_prime_ == 0) + this->cx_prime_ = (static_cast(this->width_) + 1.0) /2.0; + if (this->cx_ == 0) + this->cx_ = (static_cast(this->width_) + 1.0) /2.0; + if (this->cy_ == 0) + this->cy_ = (static_cast(this->height_) + 1.0) /2.0; + + + double hfov = this->camera_->HFOV().Radian(); + double computed_focal_length = + (static_cast(this->width_)) / + (2.0 * tan(hfov / 2.0)); + + if (this->focal_length_ == 0) + { + this->focal_length_ = computed_focal_length; + } + else + { + // check against float precision + if (!ignition::math::equal(this->focal_length_, computed_focal_length)) + { + ROS_WARN_NAMED("camera_utils", "The [%f] you have provided for camera_ [%s]" + " is inconsistent with specified image_width [%d] and" + " HFOV [%f]. Please double check to see that" + " focal_length = width_ / (2.0 * tan(HFOV/2.0))," + " the explected focal_lengtth value is [%f]," + " please update your camera_ model description accordingly.", + this->focal_length_, this->parentSensor_->Name().c_str(), + this->width_, hfov, + computed_focal_length); + } + } + + // fill CameraInfo + sensor_msgs::CameraInfo camera_info_msg; + + camera_info_msg.header.frame_id = this->frame_name_; + + camera_info_msg.height = this->height_; + camera_info_msg.width = this->width_; + // distortion +#if ROS_VERSION_MINIMUM(1, 3, 0) + camera_info_msg.distortion_model = "plumb_bob"; + camera_info_msg.D.resize(5); +#endif + // Allow the user to disable automatic cropping (used to remove barrel + // distortion black border. The crop can be useful, but also skewes + // the lens distortion, making the supplied k and t values incorrect. + if(this->camera_->LensDistortion()) + { + this->camera_->LensDistortion()->SetCrop(this->border_crop_); + } + + // D = {k1, k2, t1, t2, k3}, as specified in: + // - sensor_msgs/CameraInfo: http://docs.ros.org/api/sensor_msgs/html/msg/CameraInfo.html + // - OpenCV: http://docs.opencv.org/2.4/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html + camera_info_msg.D[0] = this->distortion_k1_; + camera_info_msg.D[1] = this->distortion_k2_; + camera_info_msg.D[2] = this->distortion_t1_; + camera_info_msg.D[3] = this->distortion_t2_; + camera_info_msg.D[4] = this->distortion_k3_; + // original camera_ matrix + camera_info_msg.K[0] = this->focal_length_; + camera_info_msg.K[1] = 0.0; + camera_info_msg.K[2] = this->cx_; + camera_info_msg.K[3] = 0.0; + camera_info_msg.K[4] = this->focal_length_; + camera_info_msg.K[5] = this->cy_; + camera_info_msg.K[6] = 0.0; + camera_info_msg.K[7] = 0.0; + camera_info_msg.K[8] = 1.0; + // rectification + camera_info_msg.R[0] = 1.0; + camera_info_msg.R[1] = 0.0; + camera_info_msg.R[2] = 0.0; + camera_info_msg.R[3] = 0.0; + camera_info_msg.R[4] = 1.0; + camera_info_msg.R[5] = 0.0; + camera_info_msg.R[6] = 0.0; + camera_info_msg.R[7] = 0.0; + camera_info_msg.R[8] = 1.0; + // camera_ projection matrix (same as camera_ matrix due + // to lack of distortion/rectification) (is this generated?) + camera_info_msg.P[0] = this->focal_length_; + camera_info_msg.P[1] = 0.0; + camera_info_msg.P[2] = this->cx_; + camera_info_msg.P[3] = -this->focal_length_ * this->hack_baseline_; + camera_info_msg.P[4] = 0.0; + camera_info_msg.P[5] = this->focal_length_; + camera_info_msg.P[6] = this->cy_; + camera_info_msg.P[7] = 0.0; + camera_info_msg.P[8] = 0.0; + camera_info_msg.P[9] = 0.0; + camera_info_msg.P[10] = 1.0; + camera_info_msg.P[11] = 0.0; + + this->camera_info_manager_->setCameraInfo(camera_info_msg); + + // start custom queue for camera_ + this->callback_queue_thread_ = boost::thread( + boost::bind(&GazeboRosCameraUtils::CameraQueueThread, this)); + + load_event_(); + this->initialized_ = true; +} + +//////////////////////////////////////////////////////////////////////////////// +// Put camera_ data to the interface +void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src, + common::Time &last_update_time) +{ + this->sensor_update_time_ = last_update_time; + this->PutCameraData(_src); +} + +void GazeboRosCameraUtils::PutCameraData(const unsigned char *_src) +{ + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + + /// don't bother if there are no subscribers + if ((*this->image_connect_count_) > 0) + { + boost::mutex::scoped_lock lock(this->lock_); + + // copy data into image + this->image_msg_.header.frame_id = this->frame_name_; + this->image_msg_.header.stamp.sec = this->sensor_update_time_.sec; + this->image_msg_.header.stamp.nsec = this->sensor_update_time_.nsec; + + // copy from src to image_msg_ + fillImage(this->image_msg_, this->type_, this->height_, this->width_, + this->skip_*this->width_, reinterpret_cast(_src)); + + // publish to ros + this->image_pub_.publish(this->image_msg_); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Put camera_ data to the interface +void GazeboRosCameraUtils::PublishCameraInfo(common::Time &last_update_time) +{ + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + + this->sensor_update_time_ = last_update_time; + this->PublishCameraInfo(); +} + +void GazeboRosCameraUtils::PublishCameraInfo() +{ + if (!this->initialized_ || this->height_ <=0 || this->width_ <=0) + return; + + if (this->camera_info_pub_.getNumSubscribers() > 0) + { + this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime(); + if (this->sensor_update_time_ - this->last_info_update_time_ >= this->update_period_) + { + this->PublishCameraInfo(this->camera_info_pub_); + this->last_info_update_time_ = this->sensor_update_time_; + } + } +} + +void GazeboRosCameraUtils::PublishCameraInfo( + ros::Publisher camera_info_publisher) +{ + sensor_msgs::CameraInfo camera_info_msg = camera_info_manager_->getCameraInfo(); + + camera_info_msg.header.stamp.sec = this->sensor_update_time_.sec; + camera_info_msg.header.stamp.nsec = this->sensor_update_time_.nsec; + + camera_info_publisher.publish(camera_info_msg); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Put camera_ data to the interface +void GazeboRosCameraUtils::CameraQueueThread() +{ + static const double timeout = 0.001; + + while (this->rosnode_->ok()) + { + /// take care of callback queue + this->camera_queue_.callAvailable(ros::WallDuration(timeout)); + } +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_depth_camera.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_depth_camera.cpp new file mode 100755 index 0000000..54dc539 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_depth_camera.cpp @@ -0,0 +1,536 @@ +/* + * 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: GazeboRosDepthCamera plugin for simulating cameras in Gazebo + Author: John Hsu + Date: 24 Sept 2008 + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosDepthCamera) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosDepthCamera::GazeboRosDepthCamera() +{ + this->point_cloud_connect_count_ = 0; + this->depth_info_connect_count_ = 0; + this->last_depth_image_camera_info_update_time_ = common::Time(0); +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosDepthCamera::~GazeboRosDepthCamera() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosDepthCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + DepthCameraPlugin::Load(_parent, _sdf); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("depth_camera", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + // 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"; + + // point cloud stuff + if (!_sdf->HasElement("pointCloudTopicName")) + this->point_cloud_topic_name_ = "points"; + else + this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get(); + + // 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(); + + if (!_sdf->HasElement("pointCloudCutoff")) + this->point_cloud_cutoff_ = 0.4; + else + this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get(); + + load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboRosDepthCamera::Advertise, this)); + GazeboRosCameraUtils::Load(_parent, _sdf); +} + +void GazeboRosDepthCamera::Advertise() +{ + ros::AdvertiseOptions point_cloud_ao = + ros::AdvertiseOptions::create( + this->point_cloud_topic_name_,1, + boost::bind( &GazeboRosDepthCamera::PointCloudConnect,this), + boost::bind( &GazeboRosDepthCamera::PointCloudDisconnect,this), + ros::VoidPtr(), &this->camera_queue_); + this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao); + + ros::AdvertiseOptions depth_image_ao = + ros::AdvertiseOptions::create< sensor_msgs::Image >( + this->depth_image_topic_name_,1, + boost::bind( &GazeboRosDepthCamera::DepthImageConnect,this), + boost::bind( &GazeboRosDepthCamera::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( &GazeboRosDepthCamera::DepthInfoConnect,this), + boost::bind( &GazeboRosDepthCamera::DepthInfoDisconnect,this), + ros::VoidPtr(), &this->camera_queue_); + this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::PointCloudConnect() +{ + this->point_cloud_connect_count_++; + (*this->image_connect_count_)++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::PointCloudDisconnect() +{ + this->point_cloud_connect_count_--; + (*this->image_connect_count_)--; + if (this->point_cloud_connect_count_ <= 0) + this->parentSensor->SetActive(false); +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::DepthImageConnect() +{ + this->depth_image_connect_count_++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::DepthImageDisconnect() +{ + this->depth_image_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosDepthCamera::DepthInfoConnect() +{ + this->depth_info_connect_count_++; +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosDepthCamera::DepthInfoDisconnect() +{ + this->depth_info_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosDepthCamera::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(); + + if (this->parentSensor->IsActive()) + { + if (this->point_cloud_connect_count_ <= 0 && + this->depth_image_connect_count_ <= 0 && + (*this->image_connect_count_) <= 0) + { + this->parentSensor->SetActive(false); + } + else + { + if (this->point_cloud_connect_count_ > 0) + this->FillPointdCloud(_image); + + if (this->depth_image_connect_count_ > 0) + this->FillDepthImage(_image); + } + } + else + { + if (this->point_cloud_connect_count_ > 0 || + this->depth_image_connect_count_ <= 0) + // do this first so there's chance for sensor to run 1 frame after activate + this->parentSensor->SetActive(true); + } +} + +/////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosDepthCamera::OnNewRGBPointCloud(const float *_pcd, + 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(); + + if (!this->parentSensor->IsActive()) + { + if (this->point_cloud_connect_count_ > 0) + // do this first so there's chance for sensor to run 1 frame after activate + this->parentSensor->SetActive(true); + } + else + { + if (this->point_cloud_connect_count_ > 0) + { + this->lock_.lock(); + this->point_cloud_msg_.header.frame_id = this->frame_name_; + this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; + this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; + this->point_cloud_msg_.width = this->width; + this->point_cloud_msg_.height = this->height; + this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width; + + sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg_); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + pcd_modifier.resize(_width*_height); + + point_cloud_msg_.is_dense = true; + + sensor_msgs::PointCloud2Iterator iter_x(point_cloud_msg_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(point_cloud_msg_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(point_cloud_msg_, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(point_cloud_msg_, "rgb"); + + for (unsigned int i = 0; i < _width; i++) + { + for (unsigned int j = 0; j < _height; j++, ++iter_x, ++iter_y, ++iter_z, ++iter_rgb) + { + unsigned int index = (j * _width) + i; + *iter_x = _pcd[4 * index]; + *iter_y = _pcd[4 * index + 1]; + *iter_z = _pcd[4 * index + 2]; + *iter_rgb = _pcd[4 * index + 3]; + if (i == _width /2 && j == _height / 2) + { + uint32_t rgb = *reinterpret_cast(&(*iter_rgb)); + uint8_t r = (rgb >> 16) & 0x0000ff; + uint8_t g = (rgb >> 8) & 0x0000ff; + uint8_t b = (rgb) & 0x0000ff; + std::cerr << (int)r << " " << (int)g << " " << (int)b << "\n"; + } + } + } + + this->point_cloud_pub_.publish(this->point_cloud_msg_); + this->lock_.unlock(); + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosDepthCamera::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; + + //ROS_ERROR_NAMED("depth_camera", "camera_ new frame %s %s",this->parentSensor_->GetName().c_str(),this->frame_name_.c_str()); + this->sensor_update_time_ = this->parentSensor->LastMeasurementTime(); + + if (!this->parentSensor->IsActive()) + { + if ((*this->image_connect_count_) > 0) + // do this first so there's chance for sensor to run 1 frame after activate + this->parentSensor->SetActive(true); + } + else + { + if ((*this->image_connect_count_) > 0) + { + this->PutCameraData(_image); + // TODO(lucasw) publish camera info with depth image + // this->PublishCameraInfo(sensor_update_time); + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Put camera data to the interface +void GazeboRosDepthCamera::FillPointdCloud(const float *_src) +{ + this->lock_.lock(); + + this->point_cloud_msg_.header.frame_id = this->frame_name_; + this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; + this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; + this->point_cloud_msg_.width = this->width; + this->point_cloud_msg_.height = this->height; + this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width; + + ///copy from depth to point cloud message + FillPointCloudHelper(this->point_cloud_msg_, + this->height, + this->width, + this->skip_, + (void*)_src ); + + this->point_cloud_pub_.publish(this->point_cloud_msg_); + + this->lock_.unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Put depth image data to the interface +void GazeboRosDepthCamera::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 + FillDepthImageHelper(this->depth_image_msg_, + this->height, + this->width, + this->skip_, + (void*)_src ); + + this->depth_image_pub_.publish(this->depth_image_msg_); + + this->lock_.unlock(); +} + + +// Fill depth information +bool GazeboRosDepthCamera::FillPointCloudHelper( + sensor_msgs::PointCloud2 &point_cloud_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + pcd_modifier.resize(rows_arg*cols_arg); + + sensor_msgs::PointCloud2Iterator iter_x(point_cloud_msg_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(point_cloud_msg_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(point_cloud_msg_, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(point_cloud_msg_, "rgb"); + + point_cloud_msg.is_dense = true; + + float* toCopyFrom = (float*)data_arg; + int index = 0; + + double hfov = this->parentSensor->DepthCamera()->HFOV().Radian(); + double fl = ((double)this->width) / (2.0 *tan(hfov/2.0)); + + // convert depth to point cloud + for (uint32_t j=0; j1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl); + else pAngle = 0.0; + + for (uint32_t i=0; i1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl); + else yAngle = 0.0; + + double depth = toCopyFrom[index++]; + + // in optical frame + // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in + // to urdf, where the *_optical_frame should have above relative + // rotation from the physical camera *_frame + *iter_x = depth * tan(yAngle); + *iter_y = depth * tan(pAngle); + if(depth > this->point_cloud_cutoff_) + { + *iter_z = depth; + } + else //point in the unseeable range + { + *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN (); + point_cloud_msg.is_dense = false; + } + + // put image color data for each point + uint8_t* image_src = (uint8_t*)(&(this->image_msg_.data[0])); + if (this->image_msg_.data.size() == rows_arg*cols_arg*3) + { + // color + iter_rgb[0] = image_src[i*3+j*cols_arg*3+0]; + iter_rgb[1] = image_src[i*3+j*cols_arg*3+1]; + iter_rgb[2] = image_src[i*3+j*cols_arg*3+2]; + } + else if (this->image_msg_.data.size() == rows_arg*cols_arg) + { + // mono (or bayer? @todo; fix for bayer) + iter_rgb[0] = image_src[i+j*cols_arg]; + iter_rgb[1] = image_src[i+j*cols_arg]; + iter_rgb[2] = image_src[i+j*cols_arg]; + } + else + { + // no image + iter_rgb[0] = 0; + iter_rgb[1] = 0; + iter_rgb[2] = 0; + } + } + } + + return true; +} + +// Fill depth information +bool GazeboRosDepthCamera::FillDepthImageHelper( + sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + 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; + + const float bad_point = std::numeric_limits::quiet_NaN(); + + float* dest = (float*)(&(image_msg.data[0])); + float* toCopyFrom = (float*)data_arg; + int index = 0; + + // convert depth to point cloud + for (uint32_t j = 0; j < rows_arg; j++) + { + for (uint32_t i = 0; i < cols_arg; i++) + { + float depth = toCopyFrom[index++]; + + if (depth > this->point_cloud_cutoff_) + { + dest[i + j * cols_arg] = depth; + } + else //point in the unseeable range + { + dest[i + j * cols_arg] = bad_point; + } + } + } + return true; +} + +void GazeboRosDepthCamera::PublishCameraInfo() +{ + ROS_DEBUG_NAMED("depth_camera", "publishing default camera info, then depth camera info"); + GazeboRosCameraUtils::PublishCameraInfo(); + + if (this->depth_info_connect_count_ > 0) + { + common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime(); + + this->sensor_update_time_ = sensor_update_time; + if (sensor_update_time - this->last_depth_image_camera_info_update_time_ >= this->update_period_) + { + this->PublishCameraInfo(this->depth_image_camera_info_pub_); // , sensor_update_time); + this->last_depth_image_camera_info_update_time_ = sensor_update_time; + } + } +} + +//@todo: publish disparity similar to openni_camera_deprecated/src/nodelets/openni_nodelet.cpp. +/* +#include +pub_disparity_ = comm_nh.advertise ("depth/disparity", 5, subscriberChanged2, subscriberChanged2); + +void GazeboRosDepthCamera::PublishDisparityImage(const DepthImage& depth, ros::Time time) +{ + stereo_msgs::DisparityImagePtr disp_msg = boost::make_shared (); + disp_msg->header.stamp = time; + disp_msg->header.frame_id = device_->isDepthRegistered () ? rgb_frame_id_ : depth_frame_id_; + disp_msg->image.header = disp_msg->header; + disp_msg->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + disp_msg->image.height = depth_height_; + disp_msg->image.width = depth_width_; + disp_msg->image.step = disp_msg->image.width * sizeof (float); + disp_msg->image.data.resize (disp_msg->image.height * disp_msg->image.step); + disp_msg->T = depth.getBaseline (); + disp_msg->f = depth.getFocalLength () * depth_width_ / depth.getWidth (); + + /// @todo Compute these values from DepthGenerator::GetDeviceMaxDepth() and the like + disp_msg->min_disparity = 0.0; + disp_msg->max_disparity = disp_msg->T * disp_msg->f / 0.3; + disp_msg->delta_d = 0.125; + + depth.fillDisparityImage (depth_width_, depth_height_, reinterpret_cast(&disp_msg->image.data[0]), disp_msg->image.step); + + pub_disparity_.publish (disp_msg); +} +*/ + + +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_diff_drive.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_diff_drive.cpp new file mode 100755 index 0000000..f9e9e27 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_diff_drive.cpp @@ -0,0 +1,493 @@ +/* + Copyright (c) 2010, Daniel Hewlett, Antons Rebguns + All rights reserved. + + 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 the 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 Antons Rebguns ''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 Antons Rebguns 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 gazebo_ros_diff_drive.cpp + * + * \brief A differential drive plugin for gazebo. Based on the diffdrive plugin + * developed for the erratic robot (see copyright notice above). The original + * plugin can be found in the ROS package gazebo_erratic_plugins. + * + * \author Piyush Khandelwal (piyushk@gmail.com) + * + * $ Id: 06/21/2013 11:23:40 AM piyushk $ + */ + + +/* + * + * The support of acceleration limit was added by + * \author George Todoran + * \author Markus Bader + * \date 22th of May 2014 + */ + +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +namespace gazebo +{ + +enum { + RIGHT, + LEFT, +}; + +GazeboRosDiffDrive::GazeboRosDiffDrive() {} + +// Destructor +GazeboRosDiffDrive::~GazeboRosDiffDrive() {} + +// Load the controller +void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) +{ + + this->parent = _parent; + gazebo_ros_ = GazeboRosPtr ( new GazeboRos ( _parent, _sdf, "DiffDrive" ) ); + // Make sure the ROS node for Gazebo has already been initialized + gazebo_ros_->isInitialized(); + + gazebo_ros_->getParameter ( command_topic_, "commandTopic", "cmd_vel" ); + gazebo_ros_->getParameter ( odometry_topic_, "odometryTopic", "odom" ); + gazebo_ros_->getParameter ( odometry_frame_, "odometryFrame", "odom" ); + gazebo_ros_->getParameter ( robot_base_frame_, "robotBaseFrame", "base_footprint" ); + gazebo_ros_->getParameterBoolean ( publishWheelTF_, "publishWheelTF", false ); + gazebo_ros_->getParameterBoolean ( publishWheelJointState_, "publishWheelJointState", false ); + gazebo_ros_->getParameterBoolean ( legacy_mode_, "legacyMode", true ); + + if (!_sdf->HasElement("legacyMode")) + { + ROS_ERROR_NAMED("diff_drive", "GazeboRosDiffDrive Plugin missing , defaults to true\n" + "This setting assumes you have a old package, where the right and left wheel are changed to fix a former code issue\n" + "To get rid of this error just set to false if you just created a new package.\n" + "To fix an old package you have to exchange left wheel by the right wheel.\n" + "If you do not want to fix this issue in an old package or your z axis points down instead of the ROS standard defined in REP 103\n" + "just set to true.\n" + ); + } + + gazebo_ros_->getParameter ( wheel_separation_, "wheelSeparation", 0.34 ); + gazebo_ros_->getParameter ( wheel_diameter_, "wheelDiameter", 0.15 ); + gazebo_ros_->getParameter ( wheel_accel, "wheelAcceleration", 0.0 ); + gazebo_ros_->getParameter ( wheel_torque, "wheelTorque", 5.0 ); + gazebo_ros_->getParameter ( update_rate_, "updateRate", 100.0 ); + std::map odomOptions; + odomOptions["encoder"] = ENCODER; + odomOptions["world"] = WORLD; + gazebo_ros_->getParameter ( odom_source_, "odometrySource", odomOptions, WORLD ); + + + joints_.resize ( 2 ); + joints_[LEFT] = gazebo_ros_->getJoint ( parent, "leftJoint", "left_joint" ); + joints_[RIGHT] = gazebo_ros_->getJoint ( parent, "rightJoint", "right_joint" ); + joints_[LEFT]->SetParam ( "fmax", 0, wheel_torque ); + joints_[RIGHT]->SetParam ( "fmax", 0, wheel_torque ); + + + + this->publish_tf_ = true; + if (!_sdf->HasElement("publishTf")) { + ROS_WARN_NAMED("diff_drive", "GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %d", + this->robot_namespace_.c_str(), this->publish_tf_); + } else { + this->publish_tf_ = _sdf->GetElement("publishTf")->Get(); + } + + // Initialize update rate stuff + if ( this->update_rate_ > 0.0 ) this->update_period_ = 1.0 / this->update_rate_; + else this->update_period_ = 0.0; +#if GAZEBO_MAJOR_VERSION >= 8 + last_update_time_ = parent->GetWorld()->SimTime(); +#else + last_update_time_ = parent->GetWorld()->GetSimTime(); +#endif + + // Initialize velocity stuff + wheel_speed_[RIGHT] = 0; + wheel_speed_[LEFT] = 0; + + // Initialize velocity support stuff + wheel_speed_instr_[RIGHT] = 0; + wheel_speed_instr_[LEFT] = 0; + + x_ = 0; + rot_ = 0; + alive_ = true; + + + if (this->publishWheelJointState_) + { + joint_state_publisher_ = gazebo_ros_->node()->advertise("joint_states", 1000); + ROS_INFO_NAMED("diff_drive", "%s: Advertise joint_states", gazebo_ros_->info()); + } + + transform_broadcaster_ = boost::shared_ptr(new tf::TransformBroadcaster()); + + // ROS: Subscribe to the velocity command topic (usually "cmd_vel") + ROS_INFO_NAMED("diff_drive", "%s: Try to subscribe to %s", gazebo_ros_->info(), command_topic_.c_str()); + + ros::SubscribeOptions so = + ros::SubscribeOptions::create(command_topic_, 1, + boost::bind(&GazeboRosDiffDrive::cmdVelCallback, this, _1), + ros::VoidPtr(), &queue_); + + cmd_vel_subscriber_ = gazebo_ros_->node()->subscribe(so); + ROS_INFO_NAMED("diff_drive", "%s: Subscribe to %s", gazebo_ros_->info(), command_topic_.c_str()); + + if (this->publish_tf_) + { + odometry_publisher_ = gazebo_ros_->node()->advertise(odometry_topic_, 1); + ROS_INFO_NAMED("diff_drive", "%s: Advertise odom on %s ", gazebo_ros_->info(), odometry_topic_.c_str()); + } + + // start custom queue for diff drive + this->callback_queue_thread_ = + boost::thread ( boost::bind ( &GazeboRosDiffDrive::QueueThread, this ) ); + + // listen to the update event (broadcast every simulation iteration) + this->update_connection_ = + event::Events::ConnectWorldUpdateBegin ( boost::bind ( &GazeboRosDiffDrive::UpdateChild, this ) ); + +} + +void GazeboRosDiffDrive::Reset() +{ +#if GAZEBO_MAJOR_VERSION >= 8 + last_update_time_ = parent->GetWorld()->SimTime(); +#else + last_update_time_ = parent->GetWorld()->GetSimTime(); +#endif + pose_encoder_.x = 0; + pose_encoder_.y = 0; + pose_encoder_.theta = 0; + x_ = 0; + rot_ = 0; + joints_[LEFT]->SetParam ( "fmax", 0, wheel_torque ); + joints_[RIGHT]->SetParam ( "fmax", 0, wheel_torque ); +} + +void GazeboRosDiffDrive::publishWheelJointState() +{ + ros::Time current_time = ros::Time::now(); + + joint_state_.header.stamp = current_time; + joint_state_.name.resize ( joints_.size() ); + joint_state_.position.resize ( joints_.size() ); + + for ( int i = 0; i < 2; i++ ) { + physics::JointPtr joint = joints_[i]; +#if GAZEBO_MAJOR_VERSION >= 8 + double position = joint->Position ( 0 ); +#else + double position = joint->GetAngle ( 0 ).Radian(); +#endif + joint_state_.name[i] = joint->GetName(); + joint_state_.position[i] = position; + } + joint_state_publisher_.publish ( joint_state_ ); +} + +void GazeboRosDiffDrive::publishWheelTF() +{ + ros::Time current_time = ros::Time::now(); + for ( int i = 0; i < 2; i++ ) { + + std::string wheel_frame = gazebo_ros_->resolveTF(joints_[i]->GetChild()->GetName ()); + std::string wheel_parent_frame = gazebo_ros_->resolveTF(joints_[i]->GetParent()->GetName ()); + +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d poseWheel = joints_[i]->GetChild()->RelativePose(); +#else + ignition::math::Pose3d poseWheel = joints_[i]->GetChild()->GetRelativePose().Ign(); +#endif + + tf::Quaternion qt ( poseWheel.Rot().X(), poseWheel.Rot().Y(), poseWheel.Rot().Z(), poseWheel.Rot().W() ); + tf::Vector3 vt ( poseWheel.Pos().X(), poseWheel.Pos().Y(), poseWheel.Pos().Z() ); + + tf::Transform tfWheel ( qt, vt ); + transform_broadcaster_->sendTransform ( + tf::StampedTransform ( tfWheel, current_time, wheel_parent_frame, wheel_frame ) ); + } +} + +// Update the controller +void GazeboRosDiffDrive::UpdateChild() +{ + + /* force reset SetParam("fmax") since Joint::Reset reset MaxForce to zero at + https://bitbucket.org/osrf/gazebo/src/8091da8b3c529a362f39b042095e12c94656a5d1/gazebo/physics/Joint.cc?at=gazebo2_2.2.5#cl-331 + (this has been solved in https://bitbucket.org/osrf/gazebo/diff/gazebo/physics/Joint.cc?diff2=b64ff1b7b6ff&at=issue_964 ) + and Joint::Reset is called after ModelPlugin::Reset, so we need to set maxForce to wheel_torque other than GazeboRosDiffDrive::Reset + (this seems to be solved in https://bitbucket.org/osrf/gazebo/commits/ec8801d8683160eccae22c74bf865d59fac81f1e) + */ + for ( int i = 0; i < 2; i++ ) { + if ( fabs(wheel_torque -joints_[i]->GetParam ( "fmax", 0 )) > 1e-6 ) { + joints_[i]->SetParam ( "fmax", 0, wheel_torque ); + } + } + + + if ( odom_source_ == ENCODER ) UpdateOdometryEncoder(); +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time current_time = parent->GetWorld()->SimTime(); +#else + common::Time current_time = parent->GetWorld()->GetSimTime(); +#endif + double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); + + if ( seconds_since_last_update > update_period_ ) { + if (this->publish_tf_) publishOdometry ( seconds_since_last_update ); + if ( publishWheelTF_ ) publishWheelTF(); + if ( publishWheelJointState_ ) publishWheelJointState(); + + // Update robot in case new velocities have been requested + getWheelVelocities(); + + double current_speed[2]; + + current_speed[LEFT] = joints_[LEFT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 ); + current_speed[RIGHT] = joints_[RIGHT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 ); + + if ( wheel_accel == 0 || + ( fabs ( wheel_speed_[LEFT] - current_speed[LEFT] ) < 0.01 ) || + ( fabs ( wheel_speed_[RIGHT] - current_speed[RIGHT] ) < 0.01 ) ) { + //if max_accel == 0, or target speed is reached + joints_[LEFT]->SetParam ( "vel", 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) ); + joints_[RIGHT]->SetParam ( "vel", 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) ); + } else { + if ( wheel_speed_[LEFT]>=current_speed[LEFT] ) + wheel_speed_instr_[LEFT]+=fmin ( wheel_speed_[LEFT]-current_speed[LEFT], wheel_accel * seconds_since_last_update ); + else + wheel_speed_instr_[LEFT]+=fmax ( wheel_speed_[LEFT]-current_speed[LEFT], -wheel_accel * seconds_since_last_update ); + + if ( wheel_speed_[RIGHT]>current_speed[RIGHT] ) + wheel_speed_instr_[RIGHT]+=fmin ( wheel_speed_[RIGHT]-current_speed[RIGHT], wheel_accel * seconds_since_last_update ); + else + wheel_speed_instr_[RIGHT]+=fmax ( wheel_speed_[RIGHT]-current_speed[RIGHT], -wheel_accel * seconds_since_last_update ); + + // ROS_INFO_NAMED("diff_drive", "actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]); + // ROS_INFO_NAMED("diff_drive", "actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]); + + joints_[LEFT]->SetParam ( "vel", 0, wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) ); + joints_[RIGHT]->SetParam ( "vel", 0, wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) ); + } + last_update_time_+= common::Time ( update_period_ ); + } +} + +// Finalize the controller +void GazeboRosDiffDrive::FiniChild() +{ + alive_ = false; + queue_.clear(); + queue_.disable(); + gazebo_ros_->node()->shutdown(); + callback_queue_thread_.join(); +} + +void GazeboRosDiffDrive::getWheelVelocities() +{ + boost::mutex::scoped_lock scoped_lock ( lock ); + + double vr = x_; + double va = rot_; + + if(legacy_mode_) + { + wheel_speed_[LEFT] = vr + va * wheel_separation_ / 2.0; + wheel_speed_[RIGHT] = vr - va * wheel_separation_ / 2.0; + } + else + { + wheel_speed_[LEFT] = vr - va * wheel_separation_ / 2.0; + wheel_speed_[RIGHT] = vr + va * wheel_separation_ / 2.0; + } +} + +void GazeboRosDiffDrive::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr& cmd_msg ) +{ + boost::mutex::scoped_lock scoped_lock ( lock ); + x_ = cmd_msg->linear.x; + rot_ = cmd_msg->angular.z; +} + +void GazeboRosDiffDrive::QueueThread() +{ + static const double timeout = 0.01; + + while ( alive_ && gazebo_ros_->node()->ok() ) { + queue_.callAvailable ( ros::WallDuration ( timeout ) ); + } +} + +void GazeboRosDiffDrive::UpdateOdometryEncoder() +{ + double vl = joints_[LEFT]->GetVelocity ( 0 ); + double vr = joints_[RIGHT]->GetVelocity ( 0 ); +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time current_time = parent->GetWorld()->SimTime(); +#else + common::Time current_time = parent->GetWorld()->GetSimTime(); +#endif + double seconds_since_last_update = ( current_time - last_odom_update_ ).Double(); + last_odom_update_ = current_time; + + double b = wheel_separation_; + + // Book: Sigwart 2011 Autonompus Mobile Robots page:337 + double sl = vl * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update; + double sr = vr * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update; + double ssum = sl + sr; + + double sdiff; + if(legacy_mode_) + { + sdiff = sl - sr; + } + else + { + + sdiff = sr - sl; + } + + double dx = ( ssum ) /2.0 * cos ( pose_encoder_.theta + ( sdiff ) / ( 2.0*b ) ); + double dy = ( ssum ) /2.0 * sin ( pose_encoder_.theta + ( sdiff ) / ( 2.0*b ) ); + double dtheta = ( sdiff ) /b; + + pose_encoder_.x += dx; + pose_encoder_.y += dy; + pose_encoder_.theta += dtheta; + + double w = dtheta/seconds_since_last_update; + double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update; + + tf::Quaternion qt; + tf::Vector3 vt; + qt.setRPY ( 0,0,pose_encoder_.theta ); + vt = tf::Vector3 ( pose_encoder_.x, pose_encoder_.y, 0 ); + + odom_.pose.pose.position.x = vt.x(); + odom_.pose.pose.position.y = vt.y(); + odom_.pose.pose.position.z = vt.z(); + + odom_.pose.pose.orientation.x = qt.x(); + odom_.pose.pose.orientation.y = qt.y(); + odom_.pose.pose.orientation.z = qt.z(); + odom_.pose.pose.orientation.w = qt.w(); + + odom_.twist.twist.angular.z = w; + odom_.twist.twist.linear.x = dx/seconds_since_last_update; + odom_.twist.twist.linear.y = dy/seconds_since_last_update; +} + +void GazeboRosDiffDrive::publishOdometry ( double step_time ) +{ + + ros::Time current_time = ros::Time::now(); + std::string odom_frame = gazebo_ros_->resolveTF ( odometry_frame_ ); + std::string base_footprint_frame = gazebo_ros_->resolveTF ( robot_base_frame_ ); + + tf::Quaternion qt; + tf::Vector3 vt; + + if ( odom_source_ == ENCODER ) { + // getting data form encoder integration + qt = tf::Quaternion ( odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w ); + vt = tf::Vector3 ( odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z ); + + } + if ( odom_source_ == WORLD ) { + // getting data from gazebo world +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d pose = parent->WorldPose(); +#else + ignition::math::Pose3d pose = parent->GetWorldPose().Ign(); +#endif + qt = tf::Quaternion ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() ); + vt = tf::Vector3 ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() ); + + odom_.pose.pose.position.x = vt.x(); + odom_.pose.pose.position.y = vt.y(); + odom_.pose.pose.position.z = vt.z(); + + odom_.pose.pose.orientation.x = qt.x(); + odom_.pose.pose.orientation.y = qt.y(); + odom_.pose.pose.orientation.z = qt.z(); + odom_.pose.pose.orientation.w = qt.w(); + + // get velocity in /odom frame + ignition::math::Vector3d linear; +#if GAZEBO_MAJOR_VERSION >= 8 + linear = parent->WorldLinearVel(); + odom_.twist.twist.angular.z = parent->WorldAngularVel().Z(); +#else + linear = parent->GetWorldLinearVel().Ign(); + odom_.twist.twist.angular.z = parent->GetWorldAngularVel().Ign().Z(); +#endif + + // convert velocity to child_frame_id (aka base_footprint) + float yaw = pose.Rot().Yaw(); + odom_.twist.twist.linear.x = cosf ( yaw ) * linear.X() + sinf ( yaw ) * linear.Y(); + odom_.twist.twist.linear.y = cosf ( yaw ) * linear.Y() - sinf ( yaw ) * linear.X(); + } + + tf::Transform base_footprint_to_odom ( qt, vt ); + transform_broadcaster_->sendTransform ( + tf::StampedTransform ( base_footprint_to_odom, current_time, + odom_frame, base_footprint_frame ) ); + + + // set covariance + odom_.pose.covariance[0] = 0.00001; + odom_.pose.covariance[7] = 0.00001; + odom_.pose.covariance[14] = 1000000000000.0; + odom_.pose.covariance[21] = 1000000000000.0; + odom_.pose.covariance[28] = 1000000000000.0; + odom_.pose.covariance[35] = 0.001; + + + // set header + odom_.header.stamp = current_time; + odom_.header.frame_id = odom_frame; + odom_.child_frame_id = base_footprint_frame; + + odometry_publisher_.publish ( odom_ ); +} + +GZ_REGISTER_MODEL_PLUGIN ( GazeboRosDiffDrive ) +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_elevator.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_elevator.cpp new file mode 100755 index 0000000..080d8fb --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_elevator.cpp @@ -0,0 +1,92 @@ +/* + * Copyright 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 "gazebo_plugins/gazebo_ros_elevator.h" + +using namespace gazebo; + +GZ_REGISTER_MODEL_PLUGIN(GazeboRosElevator); + +///////////////////////////////////////////////// +GazeboRosElevator::GazeboRosElevator() +{ +} + +///////////////////////////////////////////////// +GazeboRosElevator::~GazeboRosElevator() +{ + this->queue_.clear(); + this->queue_.disable(); + this->rosnode_->shutdown(); + this->callbackQueueThread_.join(); + + delete this->rosnode_; +} + +///////////////////////////////////////////////// +void GazeboRosElevator::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) +{ + // load parameters + this->robotNamespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + { + this->robotNamespace_ = _sdf->GetElement( + "robotNamespace")->Get() + "/"; + } + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("elevator", "A ROS node for Gazebo has not been initialized," + << "unable to load plugin. Load the Gazebo system plugin " + << "'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + std::string topic = "elevator"; + if (_sdf->HasElement("topic")) + topic = _sdf->Get("topic"); + + ElevatorPlugin::Load(_parent, _sdf); + + this->rosnode_ = new ros::NodeHandle(this->robotNamespace_); + + ros::SubscribeOptions so = + ros::SubscribeOptions::create(topic, 1, + boost::bind(&GazeboRosElevator::OnElevator, this, _1), + ros::VoidPtr(), &this->queue_); + + this->elevatorSub_ = this->rosnode_->subscribe(so); + + // start custom queue for elevator + this->callbackQueueThread_ = + boost::thread(boost::bind(&GazeboRosElevator::QueueThread, this)); +} + +///////////////////////////////////////////////// +void GazeboRosElevator::OnElevator(const std_msgs::String::ConstPtr &_msg) +{ + this->MoveToFloor(std::stoi(_msg->data)); +} + +///////////////////////////////////////////////// +void GazeboRosElevator::QueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + this->queue_.callAvailable(ros::WallDuration(timeout)); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_f3d.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_f3d.cpp new file mode 100755 index 0000000..22f9f99 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_f3d.cpp @@ -0,0 +1,200 @@ +/* + * 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: Force Feed Back Ground Truth + Author: Sachin Chitta and John Hsu + Date: 1 June 2008 + */ + +#include +#include + +namespace gazebo +{ +GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosF3D::GazeboRosF3D() +{ + this->f3d_connect_count_ = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosF3D::~GazeboRosF3D() +{ + this->update_connection_.reset(); + // Custom Callback Queue + this->queue_.clear(); + this->queue_.disable(); + this->rosnode_->shutdown(); + this->callback_queue_thread_.join(); + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosF3D::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) +{ + // Get the world name. + this->world_ = _parent->GetWorld(); + + // load parameters + this->robot_namespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; + + if (!_sdf->HasElement("bodyName")) + { + ROS_FATAL_NAMED("f3d", "f3d plugin missing , cannot proceed"); + return; + } + else + this->link_name_ = _sdf->GetElement("bodyName")->Get(); + + this->link_ = _parent->GetLink(this->link_name_); + if (!this->link_) + { + ROS_FATAL_NAMED("f3d", "gazebo_ros_f3d plugin error: bodyName: %s does not exist\n",this->link_name_.c_str()); + return; + } + + if (!_sdf->HasElement("topicName")) + { + ROS_FATAL_NAMED("f3d", "f3d plugin missing , cannot proceed"); + return; + } + else + this->topic_name_ = _sdf->GetElement("topicName")->Get(); + + if (!_sdf->HasElement("frameName")) + { + ROS_INFO_NAMED("f3d", "f3d plugin missing , defaults to world"); + this->frame_name_ = "world"; + } + else + { + this->frame_name_ = _sdf->GetElement("frameName")->Get(); + // todo: frameName not used + ROS_INFO_NAMED("f3d", "f3d plugin specifies [%s], not used, default to world",this->frame_name_.c_str()); + } + + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("f3d", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + // resolve tf prefix + std::string prefix; + this->rosnode_->getParam(std::string("tf_prefix"), prefix); + this->frame_name_ = tf::resolve(prefix, this->frame_name_); + + // Custom Callback Queue + ros::AdvertiseOptions ao = ros::AdvertiseOptions::create( + this->topic_name_,1, + boost::bind( &GazeboRosF3D::F3DConnect,this), + boost::bind( &GazeboRosF3D::F3DDisconnect,this), ros::VoidPtr(), &this->queue_); + this->pub_ = this->rosnode_->advertise(ao); + + // Custom Callback Queue + this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosF3D::QueueThread,this ) ); + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosF3D::UpdateChild, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Someone subscribes to me +void GazeboRosF3D::F3DConnect() +{ + this->f3d_connect_count_++; +} + +//////////////////////////////////////////////////////////////////////////////// +// Someone subscribes to me +void GazeboRosF3D::F3DDisconnect() +{ + this->f3d_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosF3D::UpdateChild() +{ + if (this->f3d_connect_count_ == 0) + return; + + ignition::math::Vector3d torque; + ignition::math::Vector3d force; + + // get force and torque on body +#if GAZEBO_MAJOR_VERSION >= 8 + force = this->link_->WorldForce(); + torque = this->link_->WorldTorque(); +#else + force = this->link_->GetWorldForce().Ign(); + torque = this->link_->GetWorldTorque().Ign(); +#endif + + this->lock_.lock(); + // copy data into wrench message + this->wrench_msg_.header.frame_id = this->frame_name_; +#if GAZEBO_MAJOR_VERSION >= 8 + this->wrench_msg_.header.stamp.sec = (this->world_->SimTime()).sec; + this->wrench_msg_.header.stamp.nsec = (this->world_->SimTime()).nsec; +#else + this->wrench_msg_.header.stamp.sec = (this->world_->GetSimTime()).sec; + this->wrench_msg_.header.stamp.nsec = (this->world_->GetSimTime()).nsec; +#endif + + this->wrench_msg_.wrench.force.x = force.X(); + this->wrench_msg_.wrench.force.y = force.Y(); + this->wrench_msg_.wrench.force.z = force.Z(); + this->wrench_msg_.wrench.torque.x = torque.X(); + this->wrench_msg_.wrench.torque.y = torque.Y(); + this->wrench_msg_.wrench.torque.z = torque.Z(); + + this->pub_.publish(this->wrench_msg_); + this->lock_.unlock(); + +} + +// Custom Callback Queue +//////////////////////////////////////////////////////////////////////////////// +// custom callback queue thread +void GazeboRosF3D::QueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + { + this->queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_force.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_force.cpp new file mode 100755 index 0000000..769f14e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_force.cpp @@ -0,0 +1,160 @@ +/* + * 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: GazeboRosForce plugin for manipulating objects in Gazebo + Author: John Hsu + Date: 24 Sept 2008 + */ + +#include +#include + +#include + +namespace gazebo +{ +GZ_REGISTER_MODEL_PLUGIN(GazeboRosForce); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosForce::GazeboRosForce() +{ + this->wrench_msg_.force.x = 0; + this->wrench_msg_.force.y = 0; + this->wrench_msg_.force.z = 0; + this->wrench_msg_.torque.x = 0; + this->wrench_msg_.torque.y = 0; + this->wrench_msg_.torque.z = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosForce::~GazeboRosForce() +{ + this->update_connection_.reset(); + + // Custom Callback Queue + this->queue_.clear(); + this->queue_.disable(); + this->rosnode_->shutdown(); + this->callback_queue_thread_.join(); + + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosForce::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + // Get the world name. + this->world_ = _model->GetWorld(); + + // load parameters + this->robot_namespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; + + if (!_sdf->HasElement("bodyName")) + { + ROS_FATAL_NAMED("force", "force plugin missing , cannot proceed"); + return; + } + else + this->link_name_ = _sdf->GetElement("bodyName")->Get(); + + this->link_ = _model->GetLink(this->link_name_); + if (!this->link_) + { + ROS_FATAL_NAMED("force", "gazebo_ros_force plugin error: link named: %s does not exist\n",this->link_name_.c_str()); + return; + } + + if (!_sdf->HasElement("topicName")) + { + ROS_FATAL_NAMED("force", "force plugin missing , cannot proceed"); + return; + } + else + this->topic_name_ = _sdf->GetElement("topicName")->Get(); + + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("force", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + // Custom Callback Queue + ros::SubscribeOptions so = ros::SubscribeOptions::create( + this->topic_name_,1, + boost::bind( &GazeboRosForce::UpdateObjectForce,this,_1), + ros::VoidPtr(), &this->queue_); + this->sub_ = this->rosnode_->subscribe(so); + + // Custom Callback Queue + this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosForce::QueueThread,this ) ); + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosForce::UpdateChild, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosForce::UpdateObjectForce(const geometry_msgs::Wrench::ConstPtr& _msg) +{ + this->wrench_msg_.force.x = _msg->force.x; + this->wrench_msg_.force.y = _msg->force.y; + this->wrench_msg_.force.z = _msg->force.z; + this->wrench_msg_.torque.x = _msg->torque.x; + this->wrench_msg_.torque.y = _msg->torque.y; + this->wrench_msg_.torque.z = _msg->torque.z; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosForce::UpdateChild() +{ + this->lock_.lock(); + ignition::math::Vector3d force(this->wrench_msg_.force.x,this->wrench_msg_.force.y,this->wrench_msg_.force.z); + ignition::math::Vector3d torque(this->wrench_msg_.torque.x,this->wrench_msg_.torque.y,this->wrench_msg_.torque.z); + this->link_->AddForce(force); + this->link_->AddTorque(torque); + this->lock_.unlock(); +} + +// Custom Callback Queue +//////////////////////////////////////////////////////////////////////////////// +// custom callback queue thread +void GazeboRosForce::QueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + { + this->queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp new file mode 100755 index 0000000..586b998 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_ft_sensor.cpp @@ -0,0 +1,255 @@ +/* + * 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: Force Torque Sensor Plugin + * Author: Francisco Suarez-Ruiz + * Date: 5 June 2014 + */ + +#include +#include + +namespace gazebo +{ +GZ_REGISTER_MODEL_PLUGIN(GazeboRosFT); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosFT::GazeboRosFT() +{ + this->ft_connect_count_ = 0; + this->seed = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosFT::~GazeboRosFT() +{ + this->update_connection_.reset(); + // Custom Callback Queue + this->queue_.clear(); + this->queue_.disable(); + this->rosnode_->shutdown(); + this->callback_queue_thread_.join(); + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosFT::Load( physics::ModelPtr _model, sdf::ElementPtr _sdf ) +{ + // Save pointers + this->model_ = _model; + this->world_ = this->model_->GetWorld(); + + // load parameters + this->robot_namespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; + + if (!_sdf->HasElement("jointName")) + { + ROS_FATAL_NAMED("ft_sensor", "ft_sensor plugin missing , cannot proceed"); + return; + } + else + this->joint_name_ = _sdf->GetElement("jointName")->Get(); + + this->joint_ = this->model_->GetJoint(this->joint_name_); + if (!this->joint_) + { + ROS_FATAL_NAMED("ft_sensor", "gazebo_ros_ft_sensor plugin error: jointName: %s does not exist\n",this->joint_name_.c_str()); + return; + } + + this->parent_link_ = this->joint_->GetParent(); + this->child_link_ = this->joint_->GetChild(); + this->frame_name_ = this->child_link_->GetName(); + + ROS_INFO_NAMED("ft_sensor", "ft_sensor plugin reporting wrench values to the frame [%s]", this->frame_name_.c_str()); + + if (!_sdf->HasElement("topicName")) + { + ROS_FATAL_NAMED("ft_sensor", "ft_sensor plugin missing , cannot proceed"); + return; + } + else + this->topic_name_ = _sdf->GetElement("topicName")->Get(); + + if (!_sdf->HasElement("gaussianNoise")) + { + ROS_INFO_NAMED("ft_sensor", "imu plugin missing , defaults to 0.0"); + this->gaussian_noise_ = 0.0; + } + else + this->gaussian_noise_ = _sdf->Get("gaussianNoise"); + + if (!_sdf->HasElement("updateRate")) + { + ROS_DEBUG_NAMED("ft_sensor", "ft_sensor plugin missing , defaults to 0.0" + " (as fast as possible)"); + this->update_rate_ = 0; + } + else + this->update_rate_ = _sdf->GetElement("updateRate")->Get(); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("ft_sensor", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + // resolve tf prefix + std::string prefix; + this->rosnode_->getParam(std::string("tf_prefix"), prefix); + this->frame_name_ = tf::resolve(prefix, this->frame_name_); + + // Custom Callback Queue + ros::AdvertiseOptions ao = ros::AdvertiseOptions::create( + this->topic_name_,1, + boost::bind( &GazeboRosFT::FTConnect,this), + boost::bind( &GazeboRosFT::FTDisconnect,this), ros::VoidPtr(), &this->queue_); + this->pub_ = this->rosnode_->advertise(ao); + + // Custom Callback Queue + this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosFT::QueueThread,this ) ); + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosFT::UpdateChild, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Someone subscribes to me +void GazeboRosFT::FTConnect() +{ + this->ft_connect_count_++; +} + +//////////////////////////////////////////////////////////////////////////////// +// Someone subscribes to me +void GazeboRosFT::FTDisconnect() +{ + this->ft_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosFT::UpdateChild() +{ +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time cur_time = this->world_->SimTime(); +#else + common::Time cur_time = this->world_->GetSimTime(); +#endif + + // rate control + if (this->update_rate_ > 0 && + (cur_time-this->last_time_).Double() < (1.0/this->update_rate_)) + return; + + if (this->ft_connect_count_ == 0) + return; + + physics::JointWrench wrench; + ignition::math::Vector3d torque; + ignition::math::Vector3d force; + + // FIXME: Should include options for diferent frames and measure directions + // E.g: https://bitbucket.org/osrf/gazebo/raw/default/gazebo/sensors/ForceTorqueSensor.hh + // Get force torque at the joint + // The wrench is reported in the CHILD + // The is child_to_parent + wrench = this->joint_->GetForceTorque(0); +#if GAZEBO_MAJOR_VERSION >= 8 + force = wrench.body2Force; + torque = wrench.body2Torque; +#else + force = wrench.body2Force.Ign(); + torque = wrench.body2Torque.Ign(); +#endif + + + this->lock_.lock(); + // copy data into wrench message + this->wrench_msg_.header.frame_id = this->frame_name_; +#if GAZEBO_MAJOR_VERSION >= 8 + this->wrench_msg_.header.stamp.sec = (this->world_->SimTime()).sec; + this->wrench_msg_.header.stamp.nsec = (this->world_->SimTime()).nsec; +#else + this->wrench_msg_.header.stamp.sec = (this->world_->GetSimTime()).sec; + this->wrench_msg_.header.stamp.nsec = (this->world_->GetSimTime()).nsec; +#endif + + this->wrench_msg_.wrench.force.x = force.X() + this->GaussianKernel(0, this->gaussian_noise_); + this->wrench_msg_.wrench.force.y = force.Y() + this->GaussianKernel(0, this->gaussian_noise_); + this->wrench_msg_.wrench.force.z = force.Z() + this->GaussianKernel(0, this->gaussian_noise_); + this->wrench_msg_.wrench.torque.x = torque.X() + this->GaussianKernel(0, this->gaussian_noise_); + this->wrench_msg_.wrench.torque.y = torque.Y() + this->GaussianKernel(0, this->gaussian_noise_); + this->wrench_msg_.wrench.torque.z = torque.Z() + this->GaussianKernel(0, this->gaussian_noise_); + + this->pub_.publish(this->wrench_msg_); + this->lock_.unlock(); + + // save last time stamp + this->last_time_ = cur_time; +} + +////////////////////////////////////////////////////////////////////////////// +// Utility for adding noise +double GazeboRosFT::GaussianKernel(double mu, double sigma) +{ + // using Box-Muller transform to generate two independent standard + // normally disbributed normal variables see wikipedia + + // normalized uniform random variable + double U = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + // normalized uniform random variable + double V = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V); + // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V); + + // there are 2 indep. vars, we'll just use X + // scale to our mu and sigma + X = sigma * X + mu; + return X; +} + +// Custom Callback Queue +//////////////////////////////////////////////////////////////////////////////// +// custom callback queue thread +void GazeboRosFT::QueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + { + this->queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp new file mode 100755 index 0000000..5a48d6e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_gpu_laser.cpp @@ -0,0 +1,209 @@ +/* + * 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: GazeboRosGpuLaser plugin for simulating ray sensors in Gazebo + Author: Mihai Emanuel Dolha + Date: 29 March 2012 + */ + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "gazebo_plugins/gazebo_ros_gpu_laser.h" +#include + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosLaser) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosLaser::GazeboRosLaser() +{ + this->seed = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosLaser::~GazeboRosLaser() +{ + ROS_DEBUG_STREAM_NAMED("gpu_laser","Shutting down GPU Laser"); + this->rosnode_->shutdown(); + delete this->rosnode_; + ROS_DEBUG_STREAM_NAMED("gpu_laser","Unloaded"); +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + // load plugin + GpuRayPlugin::Load(_parent, this->sdf); + // Get the world name. + std::string worldName = _parent->WorldName(); + this->world_ = physics::get_world(worldName); + // save pointers + this->sdf = _sdf; + + GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; + this->parent_ray_sensor_ = + dynamic_pointer_cast(_parent); + + if (!this->parent_ray_sensor_) + gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent"); + + this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Laser"); + + if (!this->sdf->HasElement("frameName")) + { + ROS_INFO_NAMED("gpu_laser", "GazeboRosLaser plugin missing , defaults to /world"); + this->frame_name_ = "/world"; + } + else + this->frame_name_ = this->sdf->Get("frameName"); + + if (!this->sdf->HasElement("topicName")) + { + ROS_INFO_NAMED("gpu_laser", "GazeboRosLaser plugin missing , defaults to /world"); + this->topic_name_ = "/world"; + } + else + this->topic_name_ = this->sdf->Get("topicName"); + + this->laser_connect_count_ = 0; + + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("gpu_laser", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + ROS_INFO_NAMED("gpu_laser", "Starting GazeboRosLaser Plugin (ns = %s)", this->robot_namespace_.c_str() ); + // ros callback queue for processing subscription + this->deferred_load_thread_ = boost::thread( + boost::bind(&GazeboRosLaser::LoadThread, this)); + +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosLaser::LoadThread() +{ + this->gazebo_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node()); + this->gazebo_node_->Init(this->world_name_); + + this->pmq.startServiceThread(); + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); + if(this->tf_prefix_.empty()) { + this->tf_prefix_ = this->robot_namespace_; + boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/")); + } + ROS_INFO_NAMED("gpu_laser", "GPU Laser Plugin (ns = %s) , set to \"%s\"", + this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); + + // resolve tf prefix + this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); + + if (this->topic_name_ != "") + { + ros::AdvertiseOptions ao = + ros::AdvertiseOptions::create( + this->topic_name_, 1, + boost::bind(&GazeboRosLaser::LaserConnect, this), + boost::bind(&GazeboRosLaser::LaserDisconnect, this), + ros::VoidPtr(), NULL); + this->pub_ = this->rosnode_->advertise(ao); + this->pub_queue_ = this->pmq.addPub(); + } + + // Initialize the controller + + // sensor generation off by default + this->parent_ray_sensor_->SetActive(false); + + ROS_INFO_STREAM_NAMED("gpu_laser","LoadThread function completed"); +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosLaser::LaserConnect() +{ + this->laser_connect_count_++; + if (this->laser_connect_count_ == 1) + this->laser_scan_sub_ = + this->gazebo_node_->Subscribe(this->parent_ray_sensor_->Topic(), + &GazeboRosLaser::OnScan, this); +} + +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosLaser::LaserDisconnect() +{ + this->laser_connect_count_--; + if (this->laser_connect_count_ == 0) + this->laser_scan_sub_.reset(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Convert new Gazebo message to ROS message and publish it +void GazeboRosLaser::OnScan(ConstLaserScanStampedPtr &_msg) +{ + // We got a new message from the Gazebo sensor. Stuff a + // corresponding ROS message and publish it. + sensor_msgs::LaserScan laser_msg; + laser_msg.header.stamp = ros::Time(_msg->time().sec(), _msg->time().nsec()); + laser_msg.header.frame_id = this->frame_name_; + laser_msg.angle_min = _msg->scan().angle_min(); + laser_msg.angle_max = _msg->scan().angle_max(); + laser_msg.angle_increment = _msg->scan().angle_step(); + laser_msg.time_increment = 0; // instantaneous simulator scan + laser_msg.scan_time = 0; // not sure whether this is correct + laser_msg.range_min = _msg->scan().range_min(); + laser_msg.range_max = _msg->scan().range_max(); + laser_msg.ranges.resize(_msg->scan().ranges_size()); + std::copy(_msg->scan().ranges().begin(), + _msg->scan().ranges().end(), + laser_msg.ranges.begin()); + laser_msg.intensities.resize(_msg->scan().intensities_size()); + std::copy(_msg->scan().intensities().begin(), + _msg->scan().intensities().end(), + laser_msg.intensities.begin()); + this->pub_queue_->push(laser_msg, this->pub_); +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp new file mode 100755 index 0000000..2725feb --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_hand_of_god.cpp @@ -0,0 +1,204 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * All rights reserved. + * + * 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 the Open Source Robotics Foundation + * 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 Jonathan Bohren + * \desc A "hand-of-god" plugin which drives a floating object around based + * on the location of a TF frame. This plugin is useful for connecting human input + * devices to "god-like" objects in a Gazebo simulation. + */ + +#include +#include + +namespace gazebo +{ + // Register this plugin with the simulator + GZ_REGISTER_MODEL_PLUGIN(GazeboRosHandOfGod); + + //////////////////////////////////////////////////////////////////////////////// + // Constructor + GazeboRosHandOfGod::GazeboRosHandOfGod() : + ModelPlugin(), + robot_namespace_(""), + frame_id_("hog"), + kl_(200), + ka_(200) + { + } + + //////////////////////////////////////////////////////////////////////////////// + // Destructor + GazeboRosHandOfGod::~GazeboRosHandOfGod() + { + } + + //////////////////////////////////////////////////////////////////////////////// + // Load the controller + void GazeboRosHandOfGod::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) + { + // Make sure the ROS node for Gazebo has already been initalized + if (!ros::isInitialized()) { + ROS_FATAL_STREAM_NAMED("hand_of_god", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + // Get sdf parameters + if(_sdf->HasElement("robotNamespace")) { + this->robot_namespace_ = _sdf->Get("robotNamespace") + "/"; + } + + if(_sdf->HasElement("frameId")) { + this->frame_id_ = _sdf->Get("frameId"); + } + + if(_sdf->HasElement("kl")) { + this->kl_ = _sdf->Get("kl"); + } + if(_sdf->HasElement("ka")) { + this->ka_ = _sdf->Get("ka"); + } + + if(_sdf->HasElement("linkName")) { + this->link_name_ = _sdf->Get("linkName"); + } else { + ROS_FATAL_STREAM_NAMED("hand_of_god", "The hand-of-god plugin requires a `linkName` parameter tag"); + return; + } + + // Store the model + model_ = _parent; + + // Get the floating link + floating_link_ = model_->GetLink(link_name_); + // Disable gravity for the hog + floating_link_->SetGravityMode(false); + if(!floating_link_) { + ROS_ERROR_NAMED("hand_of_god", "Floating link not found"); + const std::vector &links = model_->GetLinks(); + for(unsigned i=0; i < links.size(); i++) { + ROS_ERROR_STREAM_NAMED("hand_of_god", " -- Link "<GetName()); + } + return; + } + +#if GAZEBO_MAJOR_VERSION >= 8 + cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->Mass()); + ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->IXX()); +#else + cl_ = 2.0 * sqrt(kl_*floating_link_->GetInertial()->GetMass()); + ca_ = 2.0 * sqrt(ka_*floating_link_->GetInertial()->GetIXX()); +#endif + + // Create the TF listener for the desired position of the hog + tf_buffer_.reset(new tf2_ros::Buffer()); + tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_)); + tf_broadcaster_.reset(new tf2_ros::TransformBroadcaster()); + + // Register update event handler + this->update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosHandOfGod::GazeboUpdate, this)); + } + + //////////////////////////////////////////////////////////////////////////////// + // Update the controller + void GazeboRosHandOfGod::GazeboUpdate() + { + // Get TF transform relative to the /world link + geometry_msgs::TransformStamped hog_desired_tform; + static bool errored = false; + try{ + hog_desired_tform = tf_buffer_->lookupTransform("world", frame_id_+"_desired", ros::Time(0)); + errored = false; + } catch (tf2::TransformException ex){ + if(!errored) { + ROS_ERROR_NAMED("hand_of_god", "%s",ex.what()); + errored = true; + } + return; + } + + // Convert TF transform to Gazebo Pose + const geometry_msgs::Vector3 &p = hog_desired_tform.transform.translation; + const geometry_msgs::Quaternion &q = hog_desired_tform.transform.rotation; + ignition::math::Pose3d hog_desired( + ignition::math::Vector3d(p.x, p.y, p.z), + ignition::math::Quaterniond(q.w, q.x, q.y, q.z)); + + // Relative transform from actual to desired pose +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d world_pose = floating_link_->DirtyPose(); + ignition::math::Vector3d worldLinearVel = floating_link_->WorldLinearVel(); + ignition::math::Vector3d relativeAngularVel = floating_link_->RelativeAngularVel(); +#else + ignition::math::Pose3d world_pose = floating_link_->GetDirtyPose().Ign(); + ignition::math::Vector3d worldLinearVel = floating_link_->GetWorldLinearVel().Ign(); + ignition::math::Vector3d relativeAngularVel = floating_link_->GetRelativeAngularVel().Ign(); +#endif + ignition::math::Vector3d err_pos = hog_desired.Pos() - world_pose.Pos(); + // Get exponential coordinates for rotation + ignition::math::Quaterniond err_rot = (ignition::math::Matrix4d(world_pose.Rot()).Inverse() + * ignition::math::Matrix4d(hog_desired.Rot())).Rotation(); + ignition::math::Quaterniond not_a_quaternion = err_rot.Log(); + + floating_link_->AddForce( + kl_ * err_pos - cl_ * worldLinearVel); + + floating_link_->AddRelativeTorque( + ka_ * ignition::math::Vector3d(not_a_quaternion.X(), not_a_quaternion.Y(), not_a_quaternion.Z()) + - ca_ * relativeAngularVel); + + // Convert actual pose to TransformStamped message + geometry_msgs::TransformStamped hog_actual_tform; + + hog_actual_tform.header.frame_id = "world"; + hog_actual_tform.header.stamp = ros::Time::now(); + + hog_actual_tform.child_frame_id = frame_id_ + "_actual"; + + hog_actual_tform.transform.translation.x = world_pose.Pos().X(); + hog_actual_tform.transform.translation.y = world_pose.Pos().Y(); + hog_actual_tform.transform.translation.z = world_pose.Pos().Z(); + + hog_actual_tform.transform.rotation.w = world_pose.Rot().W(); + hog_actual_tform.transform.rotation.x = world_pose.Rot().X(); + hog_actual_tform.transform.rotation.y = world_pose.Rot().Y(); + hog_actual_tform.transform.rotation.z = world_pose.Rot().Z(); + + tf_broadcaster_->sendTransform(hog_actual_tform); + } + +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_harness.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_harness.cpp new file mode 100755 index 0000000..d661ec8 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_harness.cpp @@ -0,0 +1,109 @@ +/* + * 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 +#include +#include + +#include "gazebo_plugins/gazebo_ros_harness.h" + +namespace gazebo +{ + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboRosHarness) + +///////////////////////////////////////////////// +GazeboRosHarness::GazeboRosHarness() +{ +} + +///////////////////////////////////////////////// +GazeboRosHarness::~GazeboRosHarness() +{ + // Custom Callback Queue + this->queue_.clear(); + this->queue_.disable(); + + this->rosnode_->shutdown(); + delete this->rosnode_; +} + +///////////////////////////////////////////////// +// Load the controller +void GazeboRosHarness::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) +{ + // Load the plugin + HarnessPlugin::Load(_parent, _sdf); + + this->robotNamespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + this->robotNamespace_ = _sdf->Get("robotNamespace") + "/"; + + // Init ROS + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("harness", "Not loading plugin since ROS hasn't been " + << "properly initialized. Try starting gazebo with ros plugin:\n" + << " gazebo -s libgazebo_ros_api_plugin.so\n"); + return; + } + + this->rosnode_ = new ros::NodeHandle(this->robotNamespace_); + + ros::SubscribeOptions so = ros::SubscribeOptions::create( + "/" + _parent->GetName() + "/harness/velocity", 1, + boost::bind(&GazeboRosHarness::OnVelocity, this, _1), + ros::VoidPtr(), &this->queue_); + this->velocitySub_ = this->rosnode_->subscribe(so); + + so = ros::SubscribeOptions::create( + "/" + _parent->GetName() + "/harness/detach", 1, + boost::bind(&GazeboRosHarness::OnDetach, this, _1), + ros::VoidPtr(), &this->queue_); + this->detachSub_ = this->rosnode_->subscribe(so); + + // Custom Callback Queue + this->callbackQueueThread_ = + boost::thread(boost::bind(&GazeboRosHarness::QueueThread, this)); +} + +///////////////////////////////////////////////// +void GazeboRosHarness::OnVelocity(const std_msgs::Float32::ConstPtr &msg) +{ + // Set the target winch velocity + this->SetWinchVelocity(msg->data); +} + +///////////////////////////////////////////////// +void GazeboRosHarness::OnDetach(const std_msgs::Bool::ConstPtr &msg) +{ + // Detach if true + if (msg->data) + this->Detach(); +} + +///////////////////////////////////////////////// +void GazeboRosHarness::QueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + { + this->queue_.callAvailable(ros::WallDuration(timeout)); + } +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_imu.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_imu.cpp new file mode 100755 index 0000000..511dda2 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_imu.cpp @@ -0,0 +1,373 @@ +/* + * 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: 3D position interface for ground truth. + * Author: Sachin Chitta and John Hsu + * Date: 1 June 2008 + */ + +#include + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboRosIMU) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosIMU::GazeboRosIMU() +{ + this->seed = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosIMU::~GazeboRosIMU() +{ + this->update_connection_.reset(); + // Finalize the controller + this->rosnode_->shutdown(); + this->callback_queue_thread_.join(); + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosIMU::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) +{ + // save pointers + this->world_ = _parent->GetWorld(); + this->sdf = _sdf; + + // ros callback queue for processing subscription + this->deferred_load_thread_ = boost::thread( + boost::bind(&GazeboRosIMU::LoadThread, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosIMU::LoadThread() +{ + // load parameters + this->robot_namespace_ = ""; + if (this->sdf->HasElement("robotNamespace")) + this->robot_namespace_ = this->sdf->Get("robotNamespace") + "/"; + + if (!this->sdf->HasElement("serviceName")) + { + ROS_INFO_NAMED("imu", "imu plugin missing , defaults to /default_imu"); + this->service_name_ = "/default_imu"; + } + else + this->service_name_ = this->sdf->Get("serviceName"); + + if (!this->sdf->HasElement("topicName")) + { + ROS_INFO_NAMED("imu", "imu plugin missing , defaults to /default_imu"); + this->topic_name_ = "/default_imu"; + } + else + this->topic_name_ = this->sdf->Get("topicName"); + + if (!this->sdf->HasElement("gaussianNoise")) + { + ROS_INFO_NAMED("imu", "imu plugin missing , defaults to 0.0"); + this->gaussian_noise_ = 0.0; + } + else + this->gaussian_noise_ = this->sdf->Get("gaussianNoise"); + + if (!this->sdf->HasElement("bodyName")) + { + ROS_FATAL_NAMED("imu", "imu plugin missing , cannot proceed"); + return; + } + else + this->link_name_ = this->sdf->Get("bodyName"); + + if (!this->sdf->HasElement("xyzOffset")) + { + ROS_INFO_NAMED("imu", "imu plugin missing , defaults to 0s"); + this->offset_.Pos() = ignition::math::Vector3d(0, 0, 0); + } + else + this->offset_.Pos() = this->sdf->Get("xyzOffset"); + + if (!this->sdf->HasElement("rpyOffset")) + { + ROS_INFO_NAMED("imu", "imu plugin missing , defaults to 0s"); + this->offset_.Rot() = ignition::math::Quaterniond(ignition::math::Vector3d(0, 0, 0)); + } + else + this->offset_.Rot() = ignition::math::Quaterniond(this->sdf->Get("rpyOffset")); + + if (!this->sdf->HasElement("updateRate")) + { + ROS_DEBUG_NAMED("imu", "imu plugin missing , defaults to 0.0" + " (as fast as possible)"); + this->update_rate_ = 0.0; + } + else + this->update_rate_ = this->sdf->GetElement("updateRate")->Get(); + + if (!this->sdf->HasElement("frameName")) + { + ROS_INFO_NAMED("imu", "imu plugin missing , defaults to "); + this->frame_name_ = link_name_; + } + else + this->frame_name_ = this->sdf->Get("frameName"); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("imu", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + // publish multi queue + this->pmq.startServiceThread(); + + // assert that the body by link_name_ exists + this->link = boost::dynamic_pointer_cast( +#if GAZEBO_MAJOR_VERSION >= 8 + this->world_->EntityByName(this->link_name_)); +#else + this->world_->GetEntity(this->link_name_)); +#endif + if (!this->link) + { + ROS_FATAL_NAMED("imu", "gazebo_ros_imu plugin error: bodyName: %s does not exist\n", + this->link_name_.c_str()); + return; + } + + // if topic name specified as empty, do not publish + if (this->topic_name_ != "") + { + this->pub_Queue = this->pmq.addPub(); + this->pub_ = this->rosnode_->advertise( + this->topic_name_, 1); + + // advertise services on the custom queue + ros::AdvertiseServiceOptions aso = + ros::AdvertiseServiceOptions::create( + this->service_name_, boost::bind(&GazeboRosIMU::ServiceCallback, + this, _1, _2), ros::VoidPtr(), &this->imu_queue_); + this->srv_ = this->rosnode_->advertiseService(aso); + } + + // Initialize the controller +#if GAZEBO_MAJOR_VERSION >= 8 + this->last_time_ = this->world_->SimTime(); +#else + this->last_time_ = this->world_->GetSimTime(); +#endif + + // this->initial_pose_ = this->link->GetPose(); +#if GAZEBO_MAJOR_VERSION >= 8 + this->last_vpos_ = this->link->WorldLinearVel(); + this->last_veul_ = this->link->WorldAngularVel(); +#else + this->last_vpos_ = this->link->GetWorldLinearVel().Ign(); + this->last_veul_ = this->link->GetWorldAngularVel().Ign(); +#endif + this->apos_ = 0; + this->aeul_ = 0; + + // start custom queue for imu + this->callback_queue_thread_ = + boost::thread(boost::bind(&GazeboRosIMU::IMUQueueThread, this)); + + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosIMU::UpdateChild, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// returns true always, imu is always calibrated in sim +bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res) +{ + return true; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosIMU::UpdateChild() +{ +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time cur_time = this->world_->SimTime(); +#else + common::Time cur_time = this->world_->GetSimTime(); +#endif + + // rate control + if (this->update_rate_ > 0 && + (cur_time - this->last_time_).Double() < (1.0 / this->update_rate_)) + return; + + if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")) + { + ignition::math::Pose3d pose; + ignition::math::Quaterniond rot; + ignition::math::Vector3d pos; + + // Get Pose/Orientation ///@todo: verify correctness +#if GAZEBO_MAJOR_VERSION >= 8 + pose = this->link->WorldPose(); +#else + pose = this->link->GetWorldPose().Ign(); +#endif + // apply xyz offsets and get position and rotation components + pos = pose.Pos() + this->offset_.Pos(); + rot = pose.Rot(); + + // apply rpy offsets + rot = this->offset_.Rot()*rot; + rot.Normalize(); + + // get Rates +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Vector3d vpos = this->link->WorldLinearVel(); + ignition::math::Vector3d veul = this->link->WorldAngularVel(); +#else + ignition::math::Vector3d vpos = this->link->GetWorldLinearVel().Ign(); + ignition::math::Vector3d veul = this->link->GetWorldAngularVel().Ign(); +#endif + + // differentiate to get accelerations + double tmp_dt = this->last_time_.Double() - cur_time.Double(); + if (tmp_dt != 0) + { + this->apos_ = (this->last_vpos_ - vpos) / tmp_dt; + this->aeul_ = (this->last_veul_ - veul) / tmp_dt; + this->last_vpos_ = vpos; + this->last_veul_ = veul; + } + + // copy data into pose message + this->imu_msg_.header.frame_id = this->frame_name_; + this->imu_msg_.header.stamp.sec = cur_time.sec; + this->imu_msg_.header.stamp.nsec = cur_time.nsec; + + // orientation quaternion + + // uncomment this if we are reporting orientation in the local frame + // not the case for our imu definition + // // apply fixed orientation offsets of initial pose + // rot = this->initial_pose_.Rot()*rot; + // rot.Normalize(); + + this->imu_msg_.orientation.x = rot.X(); + this->imu_msg_.orientation.y = rot.Y(); + this->imu_msg_.orientation.z = rot.Z(); + this->imu_msg_.orientation.w = rot.W(); + + // pass euler angular rates + ignition::math::Vector3d linear_velocity( + veul.X() + this->GaussianKernel(0, this->gaussian_noise_), + veul.Y() + this->GaussianKernel(0, this->gaussian_noise_), + veul.Z() + this->GaussianKernel(0, this->gaussian_noise_)); + // rotate into local frame + // @todo: deal with offsets! + linear_velocity = rot.RotateVector(linear_velocity); + this->imu_msg_.angular_velocity.x = linear_velocity.X(); + this->imu_msg_.angular_velocity.y = linear_velocity.Y(); + this->imu_msg_.angular_velocity.z = linear_velocity.Z(); + + // pass accelerations + ignition::math::Vector3d linear_acceleration( + apos_.X() + this->GaussianKernel(0, this->gaussian_noise_), + apos_.Y() + this->GaussianKernel(0, this->gaussian_noise_), + apos_.Z() + this->GaussianKernel(0, this->gaussian_noise_)); + // rotate into local frame + // @todo: deal with offsets! + linear_acceleration = rot.RotateVector(linear_acceleration); + this->imu_msg_.linear_acceleration.x = linear_acceleration.X(); + this->imu_msg_.linear_acceleration.y = linear_acceleration.Y(); + this->imu_msg_.linear_acceleration.z = linear_acceleration.Z(); + + // fill in covariance matrix + /// @todo: let user set separate linear and angular covariance values. + /// @todo: apply appropriate rotations from frame_pose + double gn2 = this->gaussian_noise_*this->gaussian_noise_; + this->imu_msg_.orientation_covariance[0] = gn2; + this->imu_msg_.orientation_covariance[4] = gn2; + this->imu_msg_.orientation_covariance[8] = gn2; + this->imu_msg_.angular_velocity_covariance[0] = gn2; + this->imu_msg_.angular_velocity_covariance[4] = gn2; + this->imu_msg_.angular_velocity_covariance[8] = gn2; + this->imu_msg_.linear_acceleration_covariance[0] = gn2; + this->imu_msg_.linear_acceleration_covariance[4] = gn2; + this->imu_msg_.linear_acceleration_covariance[8] = gn2; + + { + boost::mutex::scoped_lock lock(this->lock_); + // publish to ros + if (this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "") + this->pub_Queue->push(this->imu_msg_, this->pub_); + } + + // save last time stamp + this->last_time_ = cur_time; + } +} + + +////////////////////////////////////////////////////////////////////////////// +// Utility for adding noise +double GazeboRosIMU::GaussianKernel(double mu, double sigma) +{ + // using Box-Muller transform to generate two independent standard + // normally disbributed normal variables see wikipedia + + // normalized uniform random variable + double U = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + // normalized uniform random variable + double V = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V); + // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V); + + // there are 2 indep. vars, we'll just use X + // scale to our mu and sigma + X = sigma * X + mu; + return X; +} + +//////////////////////////////////////////////////////////////////////////////// +// Put laser data to the interface +void GazeboRosIMU::IMUQueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + { + this->imu_queue_.callAvailable(ros::WallDuration(timeout)); + } +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp new file mode 100755 index 0000000..d00e3a9 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_imu_sensor.cpp @@ -0,0 +1,237 @@ +/* Copyright [2015] [Alessandro Settimi] + * + * email: ale.settimi@gmail.com + * + * 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 + +GZ_REGISTER_SENSOR_PLUGIN(gazebo::GazeboRosImuSensor) + +gazebo::GazeboRosImuSensor::GazeboRosImuSensor(): SensorPlugin() +{ + accelerometer_data = ignition::math::Vector3d(0, 0, 0); + gyroscope_data = ignition::math::Vector3d(0, 0, 0); + orientation = ignition::math::Quaterniond(1,0,0,0); + seed=0; + sensor=NULL; +} + +void gazebo::GazeboRosImuSensor::Load(gazebo::sensors::SensorPtr sensor_, sdf::ElementPtr sdf_) +{ + sdf=sdf_; + sensor=dynamic_cast(sensor_.get()); + + if(sensor==NULL) + { + ROS_FATAL("Error: Sensor pointer is NULL!"); + return; + } + + sensor->SetActive(true); + + if(!LoadParameters()) + { + ROS_FATAL("Error Loading Parameters!"); + return; + } + + if (!ros::isInitialized()) //check if ros is initialized properly + { + ROS_FATAL("ROS has not been initialized!"); + return; + } + + node = new ros::NodeHandle(this->robot_namespace); + + imu_data_publisher = node->advertise(topic_name,1); + + connection = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosImuSensor::UpdateChild, this, _1)); + + last_time = sensor->LastUpdateTime(); +} + +void gazebo::GazeboRosImuSensor::UpdateChild(const gazebo::common::UpdateInfo &/*_info*/) +{ + common::Time current_time = sensor->LastUpdateTime(); + + if(update_rate>0 && (current_time-last_time).Double() < 1.0/update_rate) //update rate check + return; + + if(imu_data_publisher.getNumSubscribers() > 0) + { + orientation = offset.Rot()*sensor->Orientation(); //applying offsets to the orientation measurement + accelerometer_data = sensor->LinearAcceleration(); + gyroscope_data = sensor->AngularVelocity(); + + //Guassian noise is applied to all measurements + imu_msg.orientation.x = orientation.X() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.y = orientation.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.z = orientation.Z() + GuassianKernel(0,gaussian_noise); + imu_msg.orientation.w = orientation.W() + GuassianKernel(0,gaussian_noise); + + imu_msg.linear_acceleration.x = accelerometer_data.X() + GuassianKernel(0,gaussian_noise); + imu_msg.linear_acceleration.y = accelerometer_data.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.linear_acceleration.z = accelerometer_data.Z() + GuassianKernel(0,gaussian_noise); + + imu_msg.angular_velocity.x = gyroscope_data.X() + GuassianKernel(0,gaussian_noise); + imu_msg.angular_velocity.y = gyroscope_data.Y() + GuassianKernel(0,gaussian_noise); + imu_msg.angular_velocity.z = gyroscope_data.Z() + GuassianKernel(0,gaussian_noise); + + //covariance is related to the Gaussian noise + double gn2 = gaussian_noise*gaussian_noise; + imu_msg.orientation_covariance[0] = gn2; + imu_msg.orientation_covariance[4] = gn2; + imu_msg.orientation_covariance[8] = gn2; + imu_msg.angular_velocity_covariance[0] = gn2; + imu_msg.angular_velocity_covariance[4] = gn2; + imu_msg.angular_velocity_covariance[8] = gn2; + imu_msg.linear_acceleration_covariance[0] = gn2; + imu_msg.linear_acceleration_covariance[4] = gn2; + imu_msg.linear_acceleration_covariance[8] = gn2; + + //preparing message header + imu_msg.header.frame_id = body_name; + imu_msg.header.stamp.sec = current_time.sec; + imu_msg.header.stamp.nsec = current_time.nsec; + + //publishing data + imu_data_publisher.publish(imu_msg); + + ros::spinOnce(); + } + + last_time = current_time; +} + +double gazebo::GazeboRosImuSensor::GuassianKernel(double mu, double sigma) +{ + // generation of two normalized uniform random variables + double U1 = static_cast(rand_r(&seed)) / static_cast(RAND_MAX); + double U2 = static_cast(rand_r(&seed)) / static_cast(RAND_MAX); + + // using Box-Muller transform to obtain a varaible with a standard normal distribution + double Z0 = sqrt(-2.0 * ::log(U1)) * cos(2.0*M_PI * U2); + + // scaling + Z0 = sigma * Z0 + mu; + return Z0; +} + +bool gazebo::GazeboRosImuSensor::LoadParameters() +{ + //loading parameters from the sdf file + + //NAMESPACE + if (sdf->HasElement("robotNamespace")) + { + robot_namespace = sdf->Get("robotNamespace") +"/"; + ROS_INFO_STREAM(" set to: "<ParentName(); + std::size_t it = scoped_name.find("::"); + + robot_namespace = "/" +scoped_name.substr(0,it)+"/"; + ROS_WARN_STREAM("missing , set to default: " << robot_namespace); + } + + //TOPIC + if (sdf->HasElement("topicName")) + { + topic_name = robot_namespace + sdf->Get("topicName"); + ROS_INFO_STREAM(" set to: "<, set to /namespace/default: " << topic_name); + } + + //BODY NAME + if (sdf->HasElement("frameName")) + { + body_name = sdf->Get("frameName"); + ROS_INFO_STREAM(" set to: "<, cannot proceed"); + return false; + } + + //UPDATE RATE + if (sdf->HasElement("updateRateHZ")) + { + update_rate = sdf->Get("updateRateHZ"); + ROS_INFO_STREAM(" set to: " << update_rate); + } + else + { + update_rate = 1.0; + ROS_WARN_STREAM("missing , set to default: " << update_rate); + } + + //NOISE + if (sdf->HasElement("gaussianNoise")) + { + gaussian_noise = sdf->Get("gaussianNoise"); + ROS_INFO_STREAM(" set to: " << gaussian_noise); + } + else + { + gaussian_noise = 0.0; + ROS_WARN_STREAM("missing , set to default: " << gaussian_noise); + } + + //POSITION OFFSET, UNUSED + if (sdf->HasElement("xyzOffset")) + { + offset.Pos() = sdf->Get("xyzOffset"); + ROS_INFO_STREAM(" set to: " << offset.Pos()[0] << ' ' << offset.Pos()[1] << ' ' << offset.Pos()[2]); + } + else + { + offset.Pos() = ignition::math::Vector3d(0, 0, 0); + ROS_WARN_STREAM("missing , set to default: " << offset.Pos()[0] << ' ' << offset.Pos()[1] << ' ' << offset.Pos()[2]); + } + + //ORIENTATION OFFSET + if (sdf->HasElement("rpyOffset")) + { + offset.Rot() = ignition::math::Quaterniond(sdf->Get("rpyOffset")); + ROS_INFO_STREAM(" set to: " << offset.Rot().Roll() << ' ' << offset.Rot().Pitch() << ' ' << offset.Rot().Yaw()); + } + else + { + offset.Rot() = ignition::math::Quaterniond::Identity; + ROS_WARN_STREAM("missing , set to default: " << offset.Rot().Roll() << ' ' << offset.Rot().Pitch() << ' ' << offset.Rot().Yaw()); + } + + return true; +} + +gazebo::GazeboRosImuSensor::~GazeboRosImuSensor() +{ + if (connection.get()) + { + connection.reset(); + connection = gazebo::event::ConnectionPtr(); + } + + node->shutdown(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp new file mode 100755 index 0000000..89b3467 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_joint_pose_trajectory.cpp @@ -0,0 +1,456 @@ +/* + * 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: 3D position interface for ground truth. + * Author: Sachin Chitta and John Hsu + * Date: 1 June 2008 + */ + +#include +#include +#include + +#include + +namespace gazebo +{ +GZ_REGISTER_MODEL_PLUGIN(GazeboRosJointPoseTrajectory); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +ROS_DEPRECATED GazeboRosJointPoseTrajectory::GazeboRosJointPoseTrajectory() // replaced with GazeboROSJointPoseTrajectory +{ + + this->has_trajectory_ = false; + this->trajectory_index = 0; + this->joint_trajectory_.points.clear(); + this->physics_engine_enabled_ = true; + this->disable_physics_updates_ = true; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosJointPoseTrajectory::~GazeboRosJointPoseTrajectory() +{ + this->update_connection_.reset(); + // Finalize the controller + this->rosnode_->shutdown(); + this->queue_.clear(); + this->queue_.disable(); + this->callback_queue_thread_.join(); + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosJointPoseTrajectory::Load(physics::ModelPtr _model, + sdf::ElementPtr _sdf) +{ + // save pointers + this->model_ = _model; + this->sdf = _sdf; + this->world_ = this->model_->GetWorld(); + + // this->world_->SetGravity(ignition::math::Vector3d(0, 0, 0)); + + // load parameters + this->robot_namespace_ = ""; + if (this->sdf->HasElement("robotNamespace")) + this->robot_namespace_ = this->sdf->Get("robotNamespace") + "/"; + + if (!this->sdf->HasElement("serviceName")) + { + // default + this->service_name_ = "set_joint_trajectory"; + } + else + this->service_name_ = this->sdf->Get("serviceName"); + + if (!this->sdf->HasElement("topicName")) + { + // default + this->topic_name_ = "set_joint_trajectory"; + } + else + this->topic_name_ = this->sdf->Get("topicName"); + + if (!this->sdf->HasElement("updateRate")) + { + ROS_INFO_NAMED("joint_pose_trajectory", "joint trajectory plugin missing , defaults" + " to 0.0 (as fast as possible)"); + this->update_rate_ = 0; + } + else + this->update_rate_ = this->sdf->Get("updateRate"); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("joint_pose_trajectory", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + this->deferred_load_thread_ = boost::thread( + boost::bind(&GazeboRosJointPoseTrajectory::LoadThread, this)); + +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosJointPoseTrajectory::LoadThread() +{ + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + // resolve tf prefix + std::string prefix; + this->rosnode_->getParam(std::string("tf_prefix"), prefix); + + if (this->topic_name_ != "") + { + ros::SubscribeOptions trajectory_so = + ros::SubscribeOptions::create( + this->topic_name_, 100, boost::bind( + &GazeboRosJointPoseTrajectory::SetTrajectory, this, _1), + ros::VoidPtr(), &this->queue_); + this->sub_ = this->rosnode_->subscribe(trajectory_so); + } + +#ifdef ENABLE_SERVICE + if (this->service_name_ != "") + { + ros::AdvertiseServiceOptions srv_aso = + ros::AdvertiseServiceOptions::create( + this->service_name_, + boost::bind(&GazeboRosJointPoseTrajectory::SetTrajectory, this, _1, _2), + ros::VoidPtr(), &this->queue_); + this->srv_ = this->rosnode_->advertiseService(srv_aso); + } +#endif + +#if GAZEBO_MAJOR_VERSION >= 8 + this->last_time_ = this->world_->SimTime(); +#else + this->last_time_ = this->world_->GetSimTime(); +#endif + + // start custom queue for joint trajectory plugin ros topics + this->callback_queue_thread_ = + boost::thread(boost::bind(&GazeboRosJointPoseTrajectory::QueueThread, this)); + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosJointPoseTrajectory::UpdateStates, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// set joint trajectory +void GazeboRosJointPoseTrajectory::SetTrajectory( + const trajectory_msgs::JointTrajectory::ConstPtr& trajectory) +{ + boost::mutex::scoped_lock lock(this->update_mutex); + + this->reference_link_name_ = trajectory->header.frame_id; + // do this every time a new joint trajectory is supplied, + // use header.frame_id as the reference_link_name_ + if (this->reference_link_name_ != "world" && + this->reference_link_name_ != "/map" && + this->reference_link_name_ != "map") + { + physics::EntityPtr ent = +#if GAZEBO_MAJOR_VERSION >= 8 + this->world_->EntityByName(this->reference_link_name_); +#else + this->world_->GetEntity(this->reference_link_name_); +#endif + if (ent) + this->reference_link_ = boost::dynamic_pointer_cast(ent); + if (!this->reference_link_) + { + ROS_ERROR_NAMED("joint_pose_trajectory", "ros_joint_trajectory plugin needs a reference link [%s] as" + " frame_id, aborting.\n", this->reference_link_name_.c_str()); + return; + } + else + { + this->model_ = this->reference_link_->GetParentModel(); + ROS_DEBUG_NAMED("joint_pose_trajectory", "test: update model pose by keeping link [%s] stationary" + " inertially", this->reference_link_->GetName().c_str()); + } + } + + // copy joint configuration into a map + unsigned int chain_size = trajectory->joint_names.size(); + this->joints_.resize(chain_size); + for (unsigned int i = 0; i < chain_size; ++i) + { + this->joints_[i] = this->model_->GetJoint(trajectory->joint_names[i]); + } + + unsigned int points_size = trajectory->points.size(); + this->points_.resize(points_size); + for (unsigned int i = 0; i < points_size; ++i) + { + this->points_[i].positions.resize(chain_size); + this->points_[i].time_from_start = trajectory->points[i].time_from_start; + for (unsigned int j = 0; j < chain_size; ++j) + { + this->points_[i].positions[j] = trajectory->points[i].positions[j]; + } + } + + // trajectory start time + this->trajectory_start = gazebo::common::Time(trajectory->header.stamp.sec, + trajectory->header.stamp.nsec); +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time cur_time = this->world_->SimTime(); +#else + common::Time cur_time = this->world_->GetSimTime(); +#endif + if (this->trajectory_start < cur_time) + this->trajectory_start = cur_time; + + // update the joint trajectory to play + this->has_trajectory_ = true; + // reset trajectory_index to beginning of new trajectory + this->trajectory_index = 0; + + if (this->disable_physics_updates_) + { +#if GAZEBO_MAJOR_VERSION >= 8 + this->physics_engine_enabled_ = this->world_->PhysicsEnabled(); + this->world_->SetPhysicsEnabled(false); +#else + this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine(); + this->world_->EnablePhysicsEngine(false); +#endif + } +} + +#ifdef ENABLE_SERVICE +bool GazeboRosJointPoseTrajectory::SetTrajectory( + const gazebo_msgs::SetJointTrajectory::Request& req, + const gazebo_msgs::SetJointTrajectory::Response& res) +{ + boost::mutex::scoped_lock lock(this->update_mutex); + + this->model_pose_ = req.model_pose; + this->set_model_pose_ = req.set_model_pose; + + this->reference_link_name_ = req.joint_trajectory.header.frame_id; + // do this every time a new joint_trajectory is supplied, + // use header.frame_id as the reference_link_name_ + if (this->reference_link_name_ != "world" && + this->reference_link_name_ != "/map" && + this->reference_link_name_ != "map") + { + physics::EntityPtr ent = +#if GAZEBO_MAJOR_VERSION >= 8 + this->world_->EntityByName(this->reference_link_name_); +#else + this->world_->GetEntity(this->reference_link_name_); +#endif + if (ent) + this->reference_link_ = boost::dynamic_pointer_cast(ent); + if (!this->reference_link_) + { + ROS_ERROR_NAMED("joint_pose_trajectory", "ros_joint_trajectory plugin specified a reference link [%s]" + " that does not exist, aborting.\n", + this->reference_link_name_.c_str()); + ROS_DEBUG_NAMED("joint_pose_trajectory", "will set model [%s] configuration, keeping model root link" + " stationary.", this->model_->GetName().c_str()); + return false; + } + else + ROS_DEBUG_NAMED("joint_pose_trajectory", "test: update model pose by keeping link [%s] stationary" + " inertially", this->reference_link_->GetName().c_str()); + } + +#if GAZEBO_MAJOR_VERSION >= 8 + this->model_ = this->world_->ModelByName(req.model_name); +#else + this->model_ = this->world_->GetModel(req.model_name); +#endif + if (!this->model_) // look for it by frame_id name + { + this->model_ = this->reference_link_->GetParentModel(); + if (this->model_) + { + ROS_INFO_NAMED("joint_pose_trajectory", "found model[%s] by link name specified in frame_id[%s]", + this->model_->GetName().c_str(), + req.joint_trajectory.header.frame_id.c_str()); + } + else + { + ROS_WARN_NAMED("joint_pose_trajectory", "no model found by link name specified in frame_id[%s]," + " aborting.", req.joint_trajectory.header.frame_id.c_str()); + return false; + } + } + + // copy joint configuration into a map + this->joint_trajectory_ = req.joint_trajectory; + + // trajectory start time + this->trajectory_start = gazebo::common::Time( + req.joint_trajectory.header.stamp.sec, + req.joint_trajectory.header.stamp.nsec); + + // update the joint_trajectory to play + this->has_trajectory_ = true; + // reset trajectory_index to beginning of new trajectory + this->trajectory_index = 0; + this->disable_physics_updates_ = req.disable_physics_updates; + if (this->disable_physics_updates_) + { +#if GAZEBO_MAJOR_VERSION >= 8 + this->physics_engine_enabled_ = this->world_->PhysicsEnabled(); + this->world_->SetPhysicsEnabled(false); +#else + this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine(); + this->world_->EnablePhysicsEngine(false); +#endif + } + + return true; +} +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Play the trajectory, update states +void GazeboRosJointPoseTrajectory::UpdateStates() +{ + boost::mutex::scoped_lock lock(this->update_mutex); + if (this->has_trajectory_) + { +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time cur_time = this->world_->SimTime(); +#else + common::Time cur_time = this->world_->GetSimTime(); +#endif + // roll out trajectory via set model configuration + // gzerr << "i[" << trajectory_index << "] time " + // << trajectory_start << " now: " << cur_time << " : "<< "\n"; + if (cur_time >= this->trajectory_start) + { + // @todo: consider a while loop until the trajectory + // catches up to the current time + // gzerr << trajectory_index << " : " << this->points_.size() << "\n"; + if (this->trajectory_index < this->points_.size()) + { + ROS_INFO_NAMED("joint_pose_trajectory", "time [%f] updating configuration [%d/%lu]", + cur_time.Double(), this->trajectory_index, this->points_.size()); + + // get reference link pose before updates +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d reference_pose = this->model_->WorldPose(); +#else + ignition::math::Pose3d reference_pose = this->model_->GetWorldPose().Ign(); +#endif + if (this->reference_link_) + { +#if GAZEBO_MAJOR_VERSION >= 8 + reference_pose = this->reference_link_->WorldPose(); +#else + reference_pose = this->reference_link_->GetWorldPose().Ign(); +#endif + } + + // trajectory roll-out based on time: + // set model configuration from trajectory message + unsigned int chain_size = this->joints_.size(); + if (chain_size == + this->points_[this->trajectory_index].positions.size()) + { + for (unsigned int i = 0; i < chain_size; ++i) + { + // this is not the most efficient way to set things + if (this->joints_[i]) + { +#if GAZEBO_MAJOR_VERSION >= 9 + this->joints_[i]->SetPosition(0, + this->points_[this->trajectory_index].positions[i], true); +#else + ROS_WARN_ONCE("The joint_pose_trajectory plugin is using the Joint::SetPosition method without preserving the link velocity."); + ROS_WARN_ONCE("As a result, gravity will not be simulated correctly for your model."); + ROS_WARN_ONCE("Please upgrade to Gazebo 9."); + ROS_WARN_ONCE("For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612"); + this->joints_[i]->SetPosition(0, + this->points_[this->trajectory_index].positions[i]); +#endif + } + } + + // set model pose + if (this->reference_link_) + this->model_->SetLinkWorldPose(reference_pose, + this->reference_link_); + else + this->model_->SetWorldPose(reference_pose); + } + else + { + ROS_ERROR_NAMED("joint_pose_trajectory", "point[%u] in JointTrajectory has different number of" + " joint names[%u] and positions[%lu].", + this->trajectory_index, chain_size, + this->points_[this->trajectory_index].positions.size()); + } + + // this->world_->SetPaused(is_paused); // resume original pause-state + gazebo::common::Time duration( + this->points_[this->trajectory_index].time_from_start.sec, + this->points_[this->trajectory_index].time_from_start.nsec); + + // reset start time for next trajectory point + this->trajectory_start += duration; + this->trajectory_index++; // increment to next trajectory point + + // save last update time stamp + this->last_time_ = cur_time; + } + else // no more trajectory points + { + // trajectory finished + this->reference_link_.reset(); + this->has_trajectory_ = false; + if (this->disable_physics_updates_) + { +#if GAZEBO_MAJOR_VERSION >= 8 + this->world_->SetPhysicsEnabled(this->physics_engine_enabled_); +#else + this->world_->EnablePhysicsEngine(this->physics_engine_enabled_); +#endif + } + } + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Put laser data to the interface +void GazeboRosJointPoseTrajectory::QueueThread() +{ + static const double timeout = 0.01; + while (this->rosnode_->ok()) + { + this->queue_.callAvailable(ros::WallDuration(timeout)); + } +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp new file mode 100755 index 0000000..c125d73 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp @@ -0,0 +1,154 @@ +/* + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * 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 the 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 Antons Rebguns ''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 Antons Rebguns 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. + * + **/ +#include +#include +#include +#include + +using namespace gazebo; + +GazeboRosJointStatePublisher::GazeboRosJointStatePublisher() {} + +// Destructor +GazeboRosJointStatePublisher::~GazeboRosJointStatePublisher() { + rosnode_->shutdown(); +} + +void GazeboRosJointStatePublisher::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) { + // Store the pointer to the model + this->parent_ = _parent; + this->world_ = _parent->GetWorld(); + + this->robot_namespace_ = parent_->GetName (); + if ( !_sdf->HasElement ( "robotNamespace" ) ) { + ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin missing , defaults to \"%s\"", + this->robot_namespace_.c_str() ); + } else { + this->robot_namespace_ = _sdf->GetElement ( "robotNamespace" )->Get(); + if ( this->robot_namespace_.empty() ) this->robot_namespace_ = parent_->GetName (); + } + if ( !robot_namespace_.empty() ) this->robot_namespace_ += "/"; + rosnode_ = boost::shared_ptr ( new ros::NodeHandle ( this->robot_namespace_ ) ); + + if ( !_sdf->HasElement ( "jointName" ) ) { + ROS_ASSERT ( "GazeboRosJointStatePublisher Plugin missing jointNames" ); + } else { + sdf::ElementPtr element = _sdf->GetElement ( "jointName" ) ; + std::string joint_names = element->Get(); + boost::erase_all ( joint_names, " " ); + boost::split ( joint_names_, joint_names, boost::is_any_of ( "," ) ); + } + + this->update_rate_ = 100.0; + if ( !_sdf->HasElement ( "updateRate" ) ) { + ROS_WARN_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->update_rate_ ); + } else { + this->update_rate_ = _sdf->GetElement ( "updateRate" )->Get(); + } + + // Initialize update rate stuff + if ( this->update_rate_ > 0.0 ) { + this->update_period_ = 1.0 / this->update_rate_; + } else { + this->update_period_ = 0.0; + } +#if GAZEBO_MAJOR_VERSION >= 8 + last_update_time_ = this->world_->SimTime(); +#else + last_update_time_ = this->world_->GetSimTime(); +#endif + + for ( unsigned int i = 0; i< joint_names_.size(); i++ ) { + joints_.push_back ( this->parent_->GetJoint ( joint_names_[i] ) ); + ROS_INFO_NAMED("joint_state_publisher", "GazeboRosJointStatePublisher is going to publish joint: %s", joint_names_[i].c_str() ); + } + + ROS_INFO_NAMED("joint_state_publisher", "Starting GazeboRosJointStatePublisher Plugin (ns = %s)!, parent name: %s", this->robot_namespace_.c_str(), parent_->GetName ().c_str() ); + + tf_prefix_ = tf::getPrefixParam ( *rosnode_ ); + joint_state_publisher_ = rosnode_->advertise ( "joint_states",1000 ); + +#if GAZEBO_MAJOR_VERSION >= 8 + last_update_time_ = this->world_->SimTime(); +#else + last_update_time_ = this->world_->GetSimTime(); +#endif + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->updateConnection = event::Events::ConnectWorldUpdateBegin ( + boost::bind ( &GazeboRosJointStatePublisher::OnUpdate, this, _1 ) ); +} + +void GazeboRosJointStatePublisher::OnUpdate ( const common::UpdateInfo & _info ) { + // Apply a small linear velocity to the model. +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time current_time = this->world_->SimTime(); +#else + common::Time current_time = this->world_->GetSimTime(); +#endif + if (current_time < last_update_time_) + { + ROS_WARN_NAMED("joint_state_publisher", "Negative joint state update time difference detected."); + last_update_time_ = current_time; + } + + double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); + + if ( seconds_since_last_update > update_period_ ) { + + publishJointStates(); + + last_update_time_+= common::Time ( update_period_ ); + + } + +} + +void GazeboRosJointStatePublisher::publishJointStates() { + ros::Time current_time = ros::Time::now(); + + joint_state_.header.stamp = current_time; + joint_state_.name.resize ( joints_.size() ); + joint_state_.position.resize ( joints_.size() ); + joint_state_.velocity.resize ( joints_.size() ); + + for ( int i = 0; i < joints_.size(); i++ ) { + physics::JointPtr joint = joints_[i]; + double velocity = joint->GetVelocity( 0 ); +#if GAZEBO_MAJOR_VERSION >= 8 + double position = joint->Position ( 0 ); +#else + double position = joint->GetAngle ( 0 ).Radian(); +#endif + joint_state_.name[i] = joint->GetName(); + joint_state_.position[i] = position; + joint_state_.velocity[i] = velocity; + } + joint_state_publisher_.publish ( joint_state_ ); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp new file mode 100755 index 0000000..60187a9 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_joint_trajectory.cpp @@ -0,0 +1,458 @@ +/* + * 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. + * +*/ + + +// ************************************************************* +// DEPRECATED +// This class has been renamed to gazebo_ros_joint_pose_trajectory +// ************************************************************* + +#include +#include +#include + +#include "gazebo_plugins/gazebo_ros_joint_trajectory.h" + +namespace gazebo +{ +GZ_REGISTER_MODEL_PLUGIN(GazeboRosJointTrajectory); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +ROS_DEPRECATED GazeboRosJointTrajectory::GazeboRosJointTrajectory() // replaced with GazeboROSJointPoseTrajectory +{ + ROS_WARN_NAMED("gazebo_ros_joint_trajectory","DEPRECATED: gazebo_ros_joint_trajectory has been renamed to gazebo_ros_joint_pose_trajectory"); + + this->has_trajectory_ = false; + this->trajectory_index = 0; + this->joint_trajectory_.points.clear(); + this->physics_engine_enabled_ = true; + this->disable_physics_updates_ = true; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosJointTrajectory::~GazeboRosJointTrajectory() +{ + this->update_connection_.reset(); + // Finalize the controller + this->rosnode_->shutdown(); + this->queue_.clear(); + this->queue_.disable(); + this->callback_queue_thread_.join(); + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosJointTrajectory::Load(physics::ModelPtr _model, + sdf::ElementPtr _sdf) +{ + // save pointers + this->model_ = _model; + this->sdf = _sdf; + this->world_ = this->model_->GetWorld(); + + // this->world_->SetGravity(ignition::math::Vector3d(0, 0, 0)); + + // load parameters + this->robot_namespace_ = ""; + if (this->sdf->HasElement("robotNamespace")) + this->robot_namespace_ = this->sdf->Get("robotNamespace") + "/"; + + if (!this->sdf->HasElement("serviceName")) + { + // default + this->service_name_ = "set_joint_trajectory"; + } + else + this->service_name_ = this->sdf->Get("serviceName"); + + if (!this->sdf->HasElement("topicName")) + { + // default + this->topic_name_ = "set_joint_trajectory"; + } + else + this->topic_name_ = this->sdf->Get("topicName"); + + if (!this->sdf->HasElement("updateRate")) + { + ROS_INFO_NAMED("joint_trajectory", "joint trajectory plugin missing , defaults" + " to 0.0 (as fast as possible)"); + this->update_rate_ = 0; + } + else + this->update_rate_ = this->sdf->Get("updateRate"); + + // ros callback queue for processing subscription + if (ros::isInitialized()) + { + this->deferred_load_thread_ = boost::thread( + boost::bind(&GazeboRosJointTrajectory::LoadThread, this)); + } + else + { + gzerr << "Not loading plugin since ROS hasn't been " + << "properly initialized. Try starting gazebo with ros plugin:\n" + << " gazebo -s libgazebo_ros_api_plugin.so\n"; + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosJointTrajectory::LoadThread() +{ + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + // resolve tf prefix + std::string prefix; + this->rosnode_->getParam(std::string("tf_prefix"), prefix); + + if (this->topic_name_ != "") + { + ros::SubscribeOptions trajectory_so = + ros::SubscribeOptions::create( + this->topic_name_, 100, boost::bind( + &GazeboRosJointTrajectory::SetTrajectory, this, _1), + ros::VoidPtr(), &this->queue_); + this->sub_ = this->rosnode_->subscribe(trajectory_so); + } + +#ifdef ENABLE_SERVICE + if (this->service_name_ != "") + { + ros::AdvertiseServiceOptions srv_aso = + ros::AdvertiseServiceOptions::create( + this->service_name_, + boost::bind(&GazeboRosJointTrajectory::SetTrajectory, this, _1, _2), + ros::VoidPtr(), &this->queue_); + this->srv_ = this->rosnode_->advertiseService(srv_aso); + } +#endif + +#if GAZEBO_MAJOR_VERSION >= 8 + this->last_time_ = this->world_->SimTime(); +#else + this->last_time_ = this->world_->GetSimTime(); +#endif + + // start custom queue for joint trajectory plugin ros topics + this->callback_queue_thread_ = + boost::thread(boost::bind(&GazeboRosJointTrajectory::QueueThread, this)); + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosJointTrajectory::UpdateStates, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// set joint trajectory +void GazeboRosJointTrajectory::SetTrajectory( + const trajectory_msgs::JointTrajectory::ConstPtr& trajectory) +{ + boost::mutex::scoped_lock lock(this->update_mutex); + + this->reference_link_name_ = trajectory->header.frame_id; + // do this every time a new joint trajectory is supplied, + // use header.frame_id as the reference_link_name_ + if (this->reference_link_name_ != "world" && + this->reference_link_name_ != "/map" && + this->reference_link_name_ != "map") + { + physics::EntityPtr ent = +#if GAZEBO_MAJOR_VERSION >= 8 + this->world_->EntityByName(this->reference_link_name_); +#else + this->world_->GetEntity(this->reference_link_name_); +#endif + if (ent) + this->reference_link_ = boost::dynamic_pointer_cast(ent); + if (!this->reference_link_) + { + ROS_ERROR_NAMED("joint_trajectory", "ros_joint_trajectory plugin needs a reference link [%s] as" + " frame_id, aborting.\n", this->reference_link_name_.c_str()); + return; + } + else + { + this->model_ = this->reference_link_->GetParentModel(); + ROS_DEBUG_NAMED("joint_trajectory", "test: update model pose by keeping link [%s] stationary" + " inertially", this->reference_link_->GetName().c_str()); + } + } + + // copy joint configuration into a map + unsigned int chain_size = trajectory->joint_names.size(); + this->joints_.resize(chain_size); + for (unsigned int i = 0; i < chain_size; ++i) + { + this->joints_[i] = this->model_->GetJoint(trajectory->joint_names[i]); + } + + unsigned int points_size = trajectory->points.size(); + this->points_.resize(points_size); + for (unsigned int i = 0; i < points_size; ++i) + { + this->points_[i].positions.resize(chain_size); + this->points_[i].time_from_start = trajectory->points[i].time_from_start; + for (unsigned int j = 0; j < chain_size; ++j) + { + this->points_[i].positions[j] = trajectory->points[i].positions[j]; + } + } + + // trajectory start time + this->trajectory_start = gazebo::common::Time(trajectory->header.stamp.sec, + trajectory->header.stamp.nsec); +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time cur_time = this->world_->SimTime(); +#else + common::Time cur_time = this->world_->GetSimTime(); +#endif + if (this->trajectory_start < cur_time) + this->trajectory_start = cur_time; + + // update the joint trajectory to play + this->has_trajectory_ = true; + // reset trajectory_index to beginning of new trajectory + this->trajectory_index = 0; + + if (this->disable_physics_updates_) + { +#if GAZEBO_MAJOR_VERSION >= 8 + this->physics_engine_enabled_ = this->world_->PhysicsEnabled(); + this->world_->SetPhysicsEnabled(false); +#else + this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine(); + this->world_->EnablePhysicsEngine(false); +#endif + } +} + +#ifdef ENABLE_SERVICE +bool GazeboRosJointTrajectory::SetTrajectory( + const gazebo_msgs::SetJointTrajectory::Request& req, + const gazebo_msgs::SetJointTrajectory::Response& res) +{ + boost::mutex::scoped_lock lock(this->update_mutex); + + this->model_pose_ = req.model_pose; + this->set_model_pose_ = req.set_model_pose; + + this->reference_link_name_ = req.joint_trajectory.header.frame_id; + // do this every time a new joint_trajectory is supplied, + // use header.frame_id as the reference_link_name_ + if (this->reference_link_name_ != "world" && + this->reference_link_name_ != "/map" && + this->reference_link_name_ != "map") + { + physics::EntityPtr ent = +#if GAZEBO_MAJOR_VERSION >= 8 + this->world_->EntityByName(this->reference_link_name_); +#else + this->world_->GetEntity(this->reference_link_name_); +#endif + if (ent) + this->reference_link_ = boost::shared_dynamic_cast(ent); + if (!this->reference_link_) + { + ROS_ERROR_NAMED("joint_trajectory", "ros_joint_trajectory plugin specified a reference link [%s]" + " that does not exist, aborting.\n", + this->reference_link_name_.c_str()); + ROS_DEBUG_NAMED("joint_trajectory", "will set model [%s] configuration, keeping model root link" + " stationary.", this->model_->GetName().c_str()); + return false; + } + else + ROS_DEBUG_NAMED("joint_trajectory", "test: update model pose by keeping link [%s] stationary" + " inertially", this->reference_link_->GetName().c_str()); + } + +#if GAZEBO_MAJOR_VERSION >= 8 + this->model_ = this->world_->ModelByName(req.model_name); +#else + this->model_ = this->world_->GetModel(req.model_name); +#endif + if (!this->model_) // look for it by frame_id name + { + this->model_ = this->reference_link_->GetParentModel(); + if (this->model_) + { + ROS_INFO_NAMED("joint_trajectory", "found model[%s] by link name specified in frame_id[%s]", + this->model_->GetName().c_str(), + req.joint_trajectory.header.frame_id.c_str()); + } + else + { + ROS_WARN_NAMED("joint_trajectory", "no model found by link name specified in frame_id[%s]," + " aborting.", req.joint_trajectory.header.frame_id.c_str()); + return false; + } + } + + // copy joint configuration into a map + this->joint_trajectory_ = req.joint_trajectory; + + // trajectory start time + this->trajectory_start = gazebo::common::Time( + req.joint_trajectory.header.stamp.sec, + req.joint_trajectory.header.stamp.nsec); + + // update the joint_trajectory to play + this->has_trajectory_ = true; + // reset trajectory_index to beginning of new trajectory + this->trajectory_index = 0; + this->disable_physics_updates_ = req.disable_physics_updates; + if (this->disable_physics_updates_) + { +#if GAZEBO_MAJOR_VERSION >= 8 + this->physics_engine_enabled_ = this->world_->PhysicsEnabled(); + this->world_->SetPhysicsEnabled(false); +#else + this->physics_engine_enabled_ = this->world_->GetEnablePhysicsEngine(); + this->world_->EnablePhysicsEngine(false); +#endif + } + + return true; +} +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Play the trajectory, update states +void GazeboRosJointTrajectory::UpdateStates() +{ + boost::mutex::scoped_lock lock(this->update_mutex); + if (this->has_trajectory_) + { +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time cur_time = this->world_->SimTime(); +#else + common::Time cur_time = this->world_->GetSimTime(); +#endif + // roll out trajectory via set model configuration + // gzerr << "i[" << trajectory_index << "] time " + // << trajectory_start << " now: " << cur_time << " : "<< "\n"; + if (cur_time >= this->trajectory_start) + { + // @todo: consider a while loop until the trajectory + // catches up to the current time + // gzerr << trajectory_index << " : " << this->points_.size() << "\n"; + if (this->trajectory_index < this->points_.size()) + { + ROS_INFO_NAMED("joint_trajectory", "time [%f] updating configuration [%d/%lu]", + cur_time.Double(), this->trajectory_index, this->points_.size()); + + // get reference link pose before updates +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d reference_pose = this->model_->WorldPose(); +#else + ignition::math::Pose3d reference_pose = this->model_->GetWorldPose().Ign(); +#endif + if (this->reference_link_) + { +#if GAZEBO_MAJOR_VERSION >= 8 + reference_pose = this->reference_link_->WorldPose(); +#else + reference_pose = this->reference_link_->GetWorldPose().Ign(); +#endif + } + + // trajectory roll-out based on time: + // set model configuration from trajectory message + unsigned int chain_size = this->joints_.size(); + if (chain_size == + this->points_[this->trajectory_index].positions.size()) + { + for (unsigned int i = 0; i < chain_size; ++i) + { + // this is not the most efficient way to set things + if (this->joints_[i]) + { +#if GAZEBO_MAJOR_VERSION >= 9 + this->joints_[i]->SetPosition(0, + this->points_[this->trajectory_index].positions[i], true); +#else + ROS_WARN_ONCE("The joint_trajectory plugin is using the Joint::SetPosition method without preserving the link velocity."); + ROS_WARN_ONCE("As a result, gravity will not be simulated correctly for your model."); + ROS_WARN_ONCE("Please upgrade to Gazebo 9."); + ROS_WARN_ONCE("For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612"); + this->joints_[i]->SetPosition(0, + this->points_[this->trajectory_index].positions[i]); +#endif + } + } + + // set model pose + if (this->reference_link_) + this->model_->SetLinkWorldPose(reference_pose, + this->reference_link_); + else + this->model_->SetWorldPose(reference_pose); + } + else + { + ROS_ERROR_NAMED("joint_trajectory", "point[%u] in JointTrajectory has different number of" + " joint names[%u] and positions[%lu].", + this->trajectory_index, chain_size, + this->points_[this->trajectory_index].positions.size()); + } + + // this->world_->SetPaused(is_paused); // resume original pause-state + gazebo::common::Time duration( + this->points_[this->trajectory_index].time_from_start.sec, + this->points_[this->trajectory_index].time_from_start.nsec); + + // reset start time for next trajectory point + this->trajectory_start += duration; + this->trajectory_index++; // increment to next trajectory point + + // save last update time stamp + this->last_time_ = cur_time; + } + else // no more trajectory points + { + // trajectory finished + this->reference_link_.reset(); + this->has_trajectory_ = false; + if (this->disable_physics_updates_) + { +#if GAZEBO_MAJOR_VERSION >= 8 + this->world_->SetPhysicsEnabled(this->physics_engine_enabled_); +#else + this->world_->EnablePhysicsEngine(this->physics_engine_enabled_); +#endif + } + } + } + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Put laser data to the interface +void GazeboRosJointTrajectory::QueueThread() +{ + static const double timeout = 0.01; + while (this->rosnode_->ok()) + { + this->queue_.callAvailable(ros::WallDuration(timeout)); + } +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_laser.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_laser.cpp new file mode 100755 index 0000000..b2b42c8 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_laser.cpp @@ -0,0 +1,204 @@ +/* + * 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: Ros Laser controller. + * Author: Nathan Koenig + * Date: 01 Feb 2007 + */ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosLaser) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosLaser::GazeboRosLaser() +{ + this->seed = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosLaser::~GazeboRosLaser() +{ + this->rosnode_->shutdown(); + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosLaser::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + // load plugin + RayPlugin::Load(_parent, this->sdf); + // Get the world name. + std::string worldName = _parent->WorldName(); + this->world_ = physics::get_world(worldName); + // save pointers + this->sdf = _sdf; + + GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; + this->parent_ray_sensor_ = + dynamic_pointer_cast(_parent); + + if (!this->parent_ray_sensor_) + gzthrow("GazeboRosLaser controller requires a Ray Sensor as its parent"); + + this->robot_namespace_ = GetRobotNamespace(_parent, _sdf, "Laser"); + + if (!this->sdf->HasElement("frameName")) + { + ROS_INFO_NAMED("laser", "Laser plugin missing , defaults to /world"); + this->frame_name_ = "/world"; + } + else + this->frame_name_ = this->sdf->Get("frameName"); + + + if (!this->sdf->HasElement("topicName")) + { + ROS_INFO_NAMED("laser", "Laser plugin missing , defaults to /world"); + this->topic_name_ = "/world"; + } + else + this->topic_name_ = this->sdf->Get("topicName"); + + this->laser_connect_count_ = 0; + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("laser", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + ROS_INFO_NAMED("laser", "Starting Laser Plugin (ns = %s)", this->robot_namespace_.c_str() ); + // ros callback queue for processing subscription + this->deferred_load_thread_ = boost::thread( + boost::bind(&GazeboRosLaser::LoadThread, this)); + +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosLaser::LoadThread() +{ + this->gazebo_node_ = gazebo::transport::NodePtr(new gazebo::transport::Node()); + this->gazebo_node_->Init(this->world_name_); + + this->pmq.startServiceThread(); + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + this->tf_prefix_ = tf::getPrefixParam(*this->rosnode_); + if(this->tf_prefix_.empty()) { + this->tf_prefix_ = this->robot_namespace_; + boost::trim_right_if(this->tf_prefix_,boost::is_any_of("/")); + } + ROS_INFO_NAMED("laser", "Laser Plugin (ns = %s) , set to \"%s\"", + this->robot_namespace_.c_str(), this->tf_prefix_.c_str()); + + // resolve tf prefix + this->frame_name_ = tf::resolve(this->tf_prefix_, this->frame_name_); + + if (this->topic_name_ != "") + { + ros::AdvertiseOptions ao = + ros::AdvertiseOptions::create( + this->topic_name_, 1, + boost::bind(&GazeboRosLaser::LaserConnect, this), + boost::bind(&GazeboRosLaser::LaserDisconnect, this), + ros::VoidPtr(), NULL); + this->pub_ = this->rosnode_->advertise(ao); + this->pub_queue_ = this->pmq.addPub(); + } + + // Initialize the controller + + // sensor generation off by default + this->parent_ray_sensor_->SetActive(false); +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosLaser::LaserConnect() +{ + this->laser_connect_count_++; + if (this->laser_connect_count_ == 1) + this->laser_scan_sub_ = + this->gazebo_node_->Subscribe(this->parent_ray_sensor_->Topic(), + &GazeboRosLaser::OnScan, this); +} + +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosLaser::LaserDisconnect() +{ + this->laser_connect_count_--; + if (this->laser_connect_count_ == 0) + this->laser_scan_sub_.reset(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Convert new Gazebo message to ROS message and publish it +void GazeboRosLaser::OnScan(ConstLaserScanStampedPtr &_msg) +{ + // We got a new message from the Gazebo sensor. Stuff a + // corresponding ROS message and publish it. + sensor_msgs::LaserScan laser_msg; + laser_msg.header.stamp = ros::Time(_msg->time().sec(), _msg->time().nsec()); + laser_msg.header.frame_id = this->frame_name_; + laser_msg.angle_min = _msg->scan().angle_min(); + laser_msg.angle_max = _msg->scan().angle_max(); + laser_msg.angle_increment = _msg->scan().angle_step(); + laser_msg.time_increment = 0; // instantaneous simulator scan + laser_msg.scan_time = 0; // not sure whether this is correct + laser_msg.range_min = _msg->scan().range_min(); + laser_msg.range_max = _msg->scan().range_max(); + laser_msg.ranges.resize(_msg->scan().ranges_size()); + std::copy(_msg->scan().ranges().begin(), + _msg->scan().ranges().end(), + laser_msg.ranges.begin()); + laser_msg.intensities.resize(_msg->scan().intensities_size()); + std::copy(_msg->scan().intensities().begin(), + _msg->scan().intensities().end(), + laser_msg.intensities.begin()); + this->pub_queue_->push(laser_msg, this->pub_); +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_multicamera.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_multicamera.cpp new file mode 100755 index 0000000..0094db5 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_multicamera.cpp @@ -0,0 +1,130 @@ +/* + * 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: Syncronizes shutters across multiple cameras + * Author: John Hsu + * Date: 10 June 2013 + */ + +#include + +#include +#include +#include + +#include "gazebo_plugins/gazebo_ros_multicamera.h" + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosMultiCamera) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosMultiCamera::GazeboRosMultiCamera() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosMultiCamera::~GazeboRosMultiCamera() +{ +} + +void GazeboRosMultiCamera::Load(sensors::SensorPtr _parent, + sdf::ElementPtr _sdf) +{ + MultiCameraPlugin::Load(_parent, _sdf); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("multicamera", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + // initialize shared_ptr members + this->image_connect_count_ = boost::shared_ptr(new int(0)); + this->image_connect_count_lock_ = boost::shared_ptr(new boost::mutex); + this->was_active_ = boost::shared_ptr(new bool(false)); + + // copying from CameraPlugin into GazeboRosCameraUtils + for (unsigned i = 0; i < this->camera.size(); ++i) + { + GazeboRosCameraUtils* util = new GazeboRosCameraUtils(); + util->parentSensor_ = this->parentSensor; + util->width_ = this->width[i]; + util->height_ = this->height[i]; + util->depth_ = this->depth[i]; + util->format_ = this->format[i]; + util->camera_ = this->camera[i]; + // Set up a shared connection counter + util->image_connect_count_ = this->image_connect_count_; + util->image_connect_count_lock_ = this->image_connect_count_lock_; + util->was_active_ = this->was_active_; + if (this->camera[i]->Name().find("left") != std::string::npos) + { + // FIXME: hardcoded, left hack_baseline_ 0 + util->Load(_parent, _sdf, "/left", 0.0); + } + else if (this->camera[i]->Name().find("right") != std::string::npos) + { + double hackBaseline = 0.0; + if (_sdf->HasElement("hackBaseline")) + hackBaseline = _sdf->Get("hackBaseline"); + util->Load(_parent, _sdf, "/right", hackBaseline); + } + this->utils.push_back(util); + } +} + +//////////////////////////////////////////////////////////////////////////////// + +void GazeboRosMultiCamera::OnNewFrame(const unsigned char *_image, + GazeboRosCameraUtils* util) +{ + common::Time sensor_update_time = util->parentSensor_->LastMeasurementTime(); + + if (util->parentSensor_->IsActive()) + { + if (sensor_update_time - util->last_update_time_ >= util->update_period_) + { + util->PutCameraData(_image, sensor_update_time); + util->PublishCameraInfo(sensor_update_time); + util->last_update_time_ = sensor_update_time; + } + } +} + +// Update the controller +void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ + OnNewFrame(_image, this->utils[0]); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosMultiCamera::OnNewFrameRight(const unsigned char *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ + OnNewFrame(_image, this->utils[1]); +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp new file mode 100755 index 0000000..0b841aa --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp @@ -0,0 +1,481 @@ +/* + * 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: GazeboRosOpenniKinect plugin for simulating cameras in Gazebo + Author: John Hsu + Date: 24 Sept 2008 + */ + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + +#include + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosOpenniKinect) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosOpenniKinect::GazeboRosOpenniKinect() +{ + this->point_cloud_connect_count_ = 0; + this->depth_info_connect_count_ = 0; + this->depth_image_connect_count_ = 0; + this->last_depth_image_camera_info_update_time_ = common::Time(0); +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosOpenniKinect::~GazeboRosOpenniKinect() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosOpenniKinect::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"; + + // point cloud stuff + if (!_sdf->HasElement("pointCloudTopicName")) + this->point_cloud_topic_name_ = "points"; + else + this->point_cloud_topic_name_ = _sdf->GetElement("pointCloudTopicName")->Get(); + + // 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(); + + if (!_sdf->HasElement("pointCloudCutoff")) + this->point_cloud_cutoff_ = 0.4; + else + this->point_cloud_cutoff_ = _sdf->GetElement("pointCloudCutoff")->Get(); + if (!_sdf->HasElement("pointCloudCutoffMax")) + this->point_cloud_cutoff_max_ = 5.0; + else + this->point_cloud_cutoff_max_ = _sdf->GetElement("pointCloudCutoffMax")->Get(); + + load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboRosOpenniKinect::Advertise, this)); + GazeboRosCameraUtils::Load(_parent, _sdf); +} + +void GazeboRosOpenniKinect::Advertise() +{ + ros::AdvertiseOptions point_cloud_ao = + ros::AdvertiseOptions::create( + this->point_cloud_topic_name_,1, + boost::bind( &GazeboRosOpenniKinect::PointCloudConnect,this), + boost::bind( &GazeboRosOpenniKinect::PointCloudDisconnect,this), + ros::VoidPtr(), &this->camera_queue_); + this->point_cloud_pub_ = this->rosnode_->advertise(point_cloud_ao); + + ros::AdvertiseOptions depth_image_ao = + ros::AdvertiseOptions::create< sensor_msgs::Image >( + this->depth_image_topic_name_,1, + boost::bind( &GazeboRosOpenniKinect::DepthImageConnect,this), + boost::bind( &GazeboRosOpenniKinect::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( &GazeboRosOpenniKinect::DepthInfoConnect,this), + boost::bind( &GazeboRosOpenniKinect::DepthInfoDisconnect,this), + ros::VoidPtr(), &this->camera_queue_); + this->depth_image_camera_info_pub_ = this->rosnode_->advertise(depth_image_camera_info_ao); +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosOpenniKinect::PointCloudConnect() +{ + this->point_cloud_connect_count_++; + (*this->image_connect_count_)++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosOpenniKinect::PointCloudDisconnect() +{ + this->point_cloud_connect_count_--; + (*this->image_connect_count_)--; + if (this->point_cloud_connect_count_ <= 0) + this->parentSensor->SetActive(false); +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosOpenniKinect::DepthImageConnect() +{ + this->depth_image_connect_count_++; + this->parentSensor->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosOpenniKinect::DepthImageDisconnect() +{ + this->depth_image_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosOpenniKinect::DepthInfoConnect() +{ + this->depth_info_connect_count_++; +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosOpenniKinect::DepthInfoDisconnect() +{ + this->depth_info_connect_count_--; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosOpenniKinect::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(); + if (this->parentSensor->IsActive()) + { + if (this->point_cloud_connect_count_ <= 0 && + this->depth_image_connect_count_ <= 0 && + (*this->image_connect_count_) <= 0) + { + this->parentSensor->SetActive(false); + } + else + { + if (this->point_cloud_connect_count_ > 0) + this->FillPointdCloud(_image); + + if (this->depth_image_connect_count_ > 0) + this->FillDepthImage(_image); + } + } + else + { + if (this->point_cloud_connect_count_ > 0 || + this->depth_image_connect_count_ <= 0) + // do this first so there's chance for sensor to run 1 frame after activate + this->parentSensor->SetActive(true); + } + PublishCameraInfo(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosOpenniKinect::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; + + //ROS_ERROR_NAMED("openni_kinect", "camera_ new frame %s %s",this->parentSensor_->Name().c_str(),this->frame_name_.c_str()); + this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime(); + + if (this->parentSensor->IsActive()) + { + if (this->point_cloud_connect_count_ <= 0 && + 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) + // do this first so there's chance for sensor to run 1 frame after activate + this->parentSensor->SetActive(true); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Put point cloud data to the interface +void GazeboRosOpenniKinect::FillPointdCloud(const float *_src) +{ + this->lock_.lock(); + + this->point_cloud_msg_.header.frame_id = this->frame_name_; + this->point_cloud_msg_.header.stamp.sec = this->depth_sensor_update_time_.sec; + this->point_cloud_msg_.header.stamp.nsec = this->depth_sensor_update_time_.nsec; + this->point_cloud_msg_.width = this->width; + this->point_cloud_msg_.height = this->height; + this->point_cloud_msg_.row_step = this->point_cloud_msg_.point_step * this->width; + + ///copy from depth to point cloud message + FillPointCloudHelper(this->point_cloud_msg_, + this->height, + this->width, + this->skip_, + (void*)_src ); + + this->point_cloud_pub_.publish(this->point_cloud_msg_); + + this->lock_.unlock(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Put depth image data to the interface +void GazeboRosOpenniKinect::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 + FillDepthImageHelper(this->depth_image_msg_, + this->height, + this->width, + this->skip_, + (void*)_src ); + + this->depth_image_pub_.publish(this->depth_image_msg_); + + this->lock_.unlock(); +} + + +// Fill depth information +bool GazeboRosOpenniKinect::FillPointCloudHelper( + sensor_msgs::PointCloud2 &point_cloud_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + // convert to flat array shape, we need to reconvert later + pcd_modifier.resize(rows_arg*cols_arg); + point_cloud_msg.is_dense = true; + + sensor_msgs::PointCloud2Iterator iter_x(point_cloud_msg_, "x"); + sensor_msgs::PointCloud2Iterator iter_y(point_cloud_msg_, "y"); + sensor_msgs::PointCloud2Iterator iter_z(point_cloud_msg_, "z"); + sensor_msgs::PointCloud2Iterator iter_rgb(point_cloud_msg_, "rgb"); + + float* toCopyFrom = (float*)data_arg; + int index = 0; + + double hfov = this->parentSensor->DepthCamera()->HFOV().Radian(); + double fl = ((double)this->width) / (2.0 *tan(hfov/2.0)); + + // convert depth to point cloud + for (uint32_t j=0; j1) pAngle = atan2( (double)j - 0.5*(double)(rows_arg-1), fl); + else pAngle = 0.0; + + for (uint32_t i=0; i1) yAngle = atan2( (double)i - 0.5*(double)(cols_arg-1), fl); + else yAngle = 0.0; + + double depth = toCopyFrom[index++]; // + 0.0*this->myParent->GetNearClip(); + + if(depth > this->point_cloud_cutoff_ && + depth < this->point_cloud_cutoff_max_) + { + // in optical frame + // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in + // to urdf, where the *_optical_frame should have above relative + // rotation from the physical camera *_frame + *iter_x = depth * tan(yAngle); + *iter_y = depth * tan(pAngle); + *iter_z = depth; + } + else //point in the unseeable range + { + *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN (); + point_cloud_msg.is_dense = false; + } + + // put image color data for each point + uint8_t* image_src = (uint8_t*)(&(this->image_msg_.data[0])); + if (this->image_msg_.data.size() == rows_arg*cols_arg*3) + { + // color + iter_rgb[0] = image_src[i*3+j*cols_arg*3+0]; + iter_rgb[1] = image_src[i*3+j*cols_arg*3+1]; + iter_rgb[2] = image_src[i*3+j*cols_arg*3+2]; + } + else if (this->image_msg_.data.size() == rows_arg*cols_arg) + { + // mono (or bayer? @todo; fix for bayer) + iter_rgb[0] = image_src[i+j*cols_arg]; + iter_rgb[1] = image_src[i+j*cols_arg]; + iter_rgb[2] = image_src[i+j*cols_arg]; + } + else + { + // no image + iter_rgb[0] = 0; + iter_rgb[1] = 0; + iter_rgb[2] = 0; + } + } + } + + // reconvert to original height and width after the flat reshape + point_cloud_msg.height = rows_arg; + point_cloud_msg.width = cols_arg; + point_cloud_msg.row_step = point_cloud_msg.point_step * point_cloud_msg.width; + + return true; +} + +// Fill depth information +bool GazeboRosOpenniKinect::FillDepthImageHelper( + sensor_msgs::Image& image_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void* data_arg) +{ + 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; + + const float bad_point = std::numeric_limits::quiet_NaN(); + + float* dest = (float*)(&(image_msg.data[0])); + float* toCopyFrom = (float*)data_arg; + int index = 0; + + // convert depth to point cloud + for (uint32_t j = 0; j < rows_arg; j++) + { + for (uint32_t i = 0; i < cols_arg; i++) + { + float depth = toCopyFrom[index++]; + + if (depth > this->point_cloud_cutoff_ && + depth < this->point_cloud_cutoff_max_) + { + dest[i + j * cols_arg] = depth; + } + else //point in the unseeable range + { + dest[i + j * cols_arg] = bad_point; + } + } + } + return true; +} + +void GazeboRosOpenniKinect::PublishCameraInfo() +{ + ROS_DEBUG_NAMED("openni_kinect", "publishing default camera info, then openni kinect camera info"); + 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->PublishCameraInfo(this->depth_image_camera_info_pub_); + this->last_depth_image_camera_info_update_time_ = this->sensor_update_time_; + } + } +} + +//@todo: publish disparity similar to openni_camera_deprecated/src/nodelets/openni_nodelet.cpp. +/* +#include +pub_disparity_ = comm_nh.advertise ("depth/disparity", 5, subscriberChanged2, subscriberChanged2); + +void GazeboRosDepthCamera::PublishDisparityImage(const DepthImage& depth, ros::Time time) +{ + stereo_msgs::DisparityImagePtr disp_msg = boost::make_shared (); + disp_msg->header.stamp = time; + disp_msg->header.frame_id = device_->isDepthRegistered () ? rgb_frame_id_ : depth_frame_id_; + disp_msg->image.header = disp_msg->header; + disp_msg->image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + disp_msg->image.height = depth_height_; + disp_msg->image.width = depth_width_; + disp_msg->image.step = disp_msg->image.width * sizeof (float); + disp_msg->image.data.resize (disp_msg->image.height * disp_msg->image.step); + disp_msg->T = depth.getBaseline (); + disp_msg->f = depth.getFocalLength () * depth_width_ / depth.getWidth (); + + /// @todo Compute these values from DepthGenerator::GetDeviceMaxDepth() and the like + disp_msg->min_disparity = 0.0; + disp_msg->max_disparity = disp_msg->T * disp_msg->f / 0.3; + disp_msg->delta_d = 0.125; + + depth.fillDisparityImage (depth_width_, depth_height_, reinterpret_cast(&disp_msg->image.data[0]), disp_msg->image.step); + + pub_disparity_.publish (disp_msg); +} +*/ + +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_p3d.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_p3d.cpp new file mode 100755 index 0000000..a252d22 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_p3d.cpp @@ -0,0 +1,397 @@ +/* + * 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. + * +*/ + +#include +#include +#include + +#include "gazebo_plugins/gazebo_ros_p3d.h" + +namespace gazebo +{ +GZ_REGISTER_MODEL_PLUGIN(GazeboRosP3D); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosP3D::GazeboRosP3D() +{ + this->seed = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosP3D::~GazeboRosP3D() +{ + this->update_connection_.reset(); + // Finalize the controller + this->rosnode_->shutdown(); + this->p3d_queue_.clear(); + this->p3d_queue_.disable(); + this->callback_queue_thread_.join(); + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosP3D::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) +{ + // Get the world name. + this->world_ = _parent->GetWorld(); + this->model_ = _parent; + + // load parameters + this->robot_namespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + this->robot_namespace_ = + _sdf->GetElement("robotNamespace")->Get() + "/"; + + if (!_sdf->HasElement("bodyName")) + { + ROS_FATAL_NAMED("p3d", "p3d plugin missing , cannot proceed"); + return; + } + else + this->link_name_ = _sdf->GetElement("bodyName")->Get(); + + this->link_ = _parent->GetLink(this->link_name_); + if (!this->link_) + { + ROS_FATAL_NAMED("p3d", "gazebo_ros_p3d plugin error: bodyName: %s does not exist\n", + this->link_name_.c_str()); + return; + } + + if (!_sdf->HasElement("topicName")) + { + ROS_FATAL_NAMED("p3d", "p3d plugin missing , cannot proceed"); + return; + } + else + this->topic_name_ = _sdf->GetElement("topicName")->Get(); + + if (!_sdf->HasElement("frameName")) + { + ROS_DEBUG_NAMED("p3d", "p3d plugin missing , defaults to world"); + this->frame_name_ = "world"; + } + else + this->frame_name_ = _sdf->GetElement("frameName")->Get(); + + if (!_sdf->HasElement("xyzOffset")) + { + ROS_DEBUG_NAMED("p3d", "p3d plugin missing , defaults to 0s"); + this->offset_.Pos() = ignition::math::Vector3d(0, 0, 0); + } + else + this->offset_.Pos() = _sdf->GetElement("xyzOffset")->Get(); + + if (!_sdf->HasElement("rpyOffset")) + { + ROS_DEBUG_NAMED("p3d", "p3d plugin missing , defaults to 0s"); + this->offset_.Rot() = ignition::math::Quaterniond(ignition::math::Vector3d(0, 0, 0)); + } + else + this->offset_.Rot() = ignition::math::Quaterniond(_sdf->GetElement("rpyOffset")->Get()); + + if (!_sdf->HasElement("gaussianNoise")) + { + ROS_DEBUG_NAMED("p3d", "p3d plugin missing , defaults to 0.0"); + this->gaussian_noise_ = 0; + } + else + this->gaussian_noise_ = _sdf->GetElement("gaussianNoise")->Get(); + + if (!_sdf->HasElement("updateRate")) + { + ROS_DEBUG_NAMED("p3d", "p3d plugin missing , defaults to 0.0" + " (as fast as possible)"); + this->update_rate_ = 0; + } + else + this->update_rate_ = _sdf->GetElement("updateRate")->Get(); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("p3d", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + // publish multi queue + this->pmq.startServiceThread(); + + // resolve tf prefix + std::string prefix; + this->rosnode_->getParam(std::string("tf_prefix"), prefix); + this->tf_frame_name_ = tf::resolve(prefix, this->frame_name_); + + if (this->topic_name_ != "") + { + this->pub_Queue = this->pmq.addPub(); + this->pub_ = + this->rosnode_->advertise(this->topic_name_, 1); + } + +#if GAZEBO_MAJOR_VERSION >= 8 + this->last_time_ = this->world_->SimTime(); +#else + this->last_time_ = this->world_->GetSimTime(); +#endif + // initialize body +#if GAZEBO_MAJOR_VERSION >= 8 + this->last_vpos_ = this->link_->WorldLinearVel(); + this->last_veul_ = this->link_->WorldAngularVel(); +#else + this->last_vpos_ = this->link_->GetWorldLinearVel().Ign(); + this->last_veul_ = this->link_->GetWorldAngularVel().Ign(); +#endif + this->apos_ = 0; + this->aeul_ = 0; + + // if frameName specified is "/world", "world", "/map" or "map" report + // back inertial values in the gazebo world + if (this->frame_name_ != "/world" && + this->frame_name_ != "world" && + this->frame_name_ != "/map" && + this->frame_name_ != "map") + { + this->reference_link_ = this->model_->GetLink(this->frame_name_); + if (!this->reference_link_) + { + ROS_ERROR_NAMED("p3d", "gazebo_ros_p3d plugin: frameName: %s does not exist, will" + " not publish pose\n", this->frame_name_.c_str()); + return; + } + } + + // init reference frame state + if (this->reference_link_) + { + ROS_DEBUG_NAMED("p3d", "got body %s", this->reference_link_->GetName().c_str()); + this->frame_apos_ = 0; + this->frame_aeul_ = 0; +#if GAZEBO_MAJOR_VERSION >= 8 + this->last_frame_vpos_ = this->reference_link_->WorldLinearVel(); + this->last_frame_veul_ = this->reference_link_->WorldAngularVel(); +#else + this->last_frame_vpos_ = this->reference_link_->GetWorldLinearVel().Ign(); + this->last_frame_veul_ = this->reference_link_->GetWorldAngularVel().Ign(); +#endif + } + + + // start custom queue for p3d + this->callback_queue_thread_ = boost::thread( + boost::bind(&GazeboRosP3D::P3DQueueThread, this)); + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + this->update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosP3D::UpdateChild, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosP3D::UpdateChild() +{ + if (!this->link_) + return; + +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time cur_time = this->world_->SimTime(); +#else + common::Time cur_time = this->world_->GetSimTime(); +#endif + + if (cur_time < this->last_time_) + { + ROS_WARN_NAMED("p3d", "Negative update time difference detected."); + this->last_time_ = cur_time; + } + + // rate control + if (this->update_rate_ > 0 && + (cur_time-this->last_time_).Double() < (1.0/this->update_rate_)) + return; + + if (this->pub_.getNumSubscribers() > 0) + { + // differentiate to get accelerations + double tmp_dt = cur_time.Double() - this->last_time_.Double(); + if (tmp_dt != 0) + { + this->lock.lock(); + + if (this->topic_name_ != "") + { + // copy data into pose message + this->pose_msg_.header.frame_id = this->tf_frame_name_; + this->pose_msg_.header.stamp.sec = cur_time.sec; + this->pose_msg_.header.stamp.nsec = cur_time.nsec; + + this->pose_msg_.child_frame_id = this->link_name_; + + ignition::math::Pose3d pose, frame_pose; + ignition::math::Vector3d frame_vpos; + ignition::math::Vector3d frame_veul; + + // get inertial Rates + // Get Pose/Orientation +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Vector3d vpos = this->link_->WorldLinearVel(); + ignition::math::Vector3d veul = this->link_->WorldAngularVel(); + + pose = this->link_->WorldPose(); +#else + ignition::math::Vector3d vpos = this->link_->GetWorldLinearVel().Ign(); + ignition::math::Vector3d veul = this->link_->GetWorldAngularVel().Ign(); + + pose = this->link_->GetWorldPose().Ign(); +#endif + + // Apply Reference Frame + if (this->reference_link_) + { + // convert to relative pose, rates +#if GAZEBO_MAJOR_VERSION >= 8 + frame_pose = this->reference_link_->WorldPose(); + frame_vpos = this->reference_link_->WorldLinearVel(); + frame_veul = this->reference_link_->WorldAngularVel(); +#else + frame_pose = this->reference_link_->GetWorldPose().Ign(); + frame_vpos = this->reference_link_->GetWorldLinearVel().Ign(); + frame_veul = this->reference_link_->GetWorldAngularVel().Ign(); +#endif + pose.Pos() = pose.Pos() - frame_pose.Pos(); + pose.Pos() = frame_pose.Rot().RotateVectorReverse(pose.Pos()); + pose.Rot() *= frame_pose.Rot().Inverse(); + + vpos = frame_pose.Rot().RotateVector(vpos - frame_vpos); + veul = frame_pose.Rot().RotateVector(veul - frame_veul); + } + + // Apply Constant Offsets + // apply xyz offsets and get position and rotation components + pose.Pos() = pose.Pos() + this->offset_.Pos(); + // apply rpy offsets + pose.Rot() = this->offset_.Rot()*pose.Rot(); + pose.Rot().Normalize(); + + // compute accelerations (not used) + this->apos_ = (this->last_vpos_ - vpos) / tmp_dt; + this->aeul_ = (this->last_veul_ - veul) / tmp_dt; + this->last_vpos_ = vpos; + this->last_veul_ = veul; + + this->frame_apos_ = (this->last_frame_vpos_ - frame_vpos) / tmp_dt; + this->frame_aeul_ = (this->last_frame_veul_ - frame_veul) / tmp_dt; + this->last_frame_vpos_ = frame_vpos; + this->last_frame_veul_ = frame_veul; + + // Fill out messages + this->pose_msg_.pose.pose.position.x = pose.Pos().X(); + this->pose_msg_.pose.pose.position.y = pose.Pos().Y(); + this->pose_msg_.pose.pose.position.z = pose.Pos().Z(); + + this->pose_msg_.pose.pose.orientation.x = pose.Rot().X(); + this->pose_msg_.pose.pose.orientation.y = pose.Rot().Y(); + this->pose_msg_.pose.pose.orientation.z = pose.Rot().Z(); + this->pose_msg_.pose.pose.orientation.w = pose.Rot().W(); + + this->pose_msg_.twist.twist.linear.x = vpos.X() + + this->GaussianKernel(0, this->gaussian_noise_); + this->pose_msg_.twist.twist.linear.y = vpos.Y() + + this->GaussianKernel(0, this->gaussian_noise_); + this->pose_msg_.twist.twist.linear.z = vpos.Z() + + this->GaussianKernel(0, this->gaussian_noise_); + // pass euler angular rates + this->pose_msg_.twist.twist.angular.x = veul.X() + + this->GaussianKernel(0, this->gaussian_noise_); + this->pose_msg_.twist.twist.angular.y = veul.Y() + + this->GaussianKernel(0, this->gaussian_noise_); + this->pose_msg_.twist.twist.angular.z = veul.Z() + + this->GaussianKernel(0, this->gaussian_noise_); + + // fill in covariance matrix + /// @todo: let user set separate linear and angular covariance values. + double gn2 = this->gaussian_noise_*this->gaussian_noise_; + this->pose_msg_.pose.covariance[0] = gn2; + this->pose_msg_.pose.covariance[7] = gn2; + this->pose_msg_.pose.covariance[14] = gn2; + this->pose_msg_.pose.covariance[21] = gn2; + this->pose_msg_.pose.covariance[28] = gn2; + this->pose_msg_.pose.covariance[35] = gn2; + + this->pose_msg_.twist.covariance[0] = gn2; + this->pose_msg_.twist.covariance[7] = gn2; + this->pose_msg_.twist.covariance[14] = gn2; + this->pose_msg_.twist.covariance[21] = gn2; + this->pose_msg_.twist.covariance[28] = gn2; + this->pose_msg_.twist.covariance[35] = gn2; + + // publish to ros + this->pub_Queue->push(this->pose_msg_, this->pub_); + } + + this->lock.unlock(); + + // save last time stamp + this->last_time_ = cur_time; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +// Utility for adding noise +double GazeboRosP3D::GaussianKernel(double mu, double sigma) +{ + // using Box-Muller transform to generate two independent standard + // normally disbributed normal variables see wikipedia + + // normalized uniform random variable + double U = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + // normalized uniform random variable + double V = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V); + // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V); + + // there are 2 indep. vars, we'll just use X + // scale to our mu and sigma + X = sigma * X + mu; + return X; +} + +//////////////////////////////////////////////////////////////////////////////// +// Put laser data to the interface +void GazeboRosP3D::P3DQueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + { + this->p3d_queue_.callAvailable(ros::WallDuration(timeout)); + } +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_planar_move.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_planar_move.cpp new file mode 100755 index 0000000..fdad12b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_planar_move.cpp @@ -0,0 +1,295 @@ +/* + * 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: Simple model controller that uses a twist message to move a robot on + * the xy plane. + * Author: Piyush Khandelwal + * Date: 29 July 2013 + */ + +#include + +namespace gazebo +{ + + GazeboRosPlanarMove::GazeboRosPlanarMove() {} + + GazeboRosPlanarMove::~GazeboRosPlanarMove() {} + + // Load the controller + void GazeboRosPlanarMove::Load(physics::ModelPtr parent, + sdf::ElementPtr sdf) + { + + parent_ = parent; + + /* Parse parameters */ + + robot_namespace_ = ""; + if (!sdf->HasElement("robotNamespace")) + { + ROS_INFO_NAMED("planar_move", "PlanarMovePlugin missing , " + "defaults to \"%s\"", robot_namespace_.c_str()); + } + else + { + robot_namespace_ = + sdf->GetElement("robotNamespace")->Get(); + } + + command_topic_ = "cmd_vel"; + if (!sdf->HasElement("commandTopic")) + { + ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing , " + "defaults to \"%s\"", + robot_namespace_.c_str(), command_topic_.c_str()); + } + else + { + command_topic_ = sdf->GetElement("commandTopic")->Get(); + } + + odometry_topic_ = "odom"; + if (!sdf->HasElement("odometryTopic")) + { + ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing , " + "defaults to \"%s\"", + robot_namespace_.c_str(), odometry_topic_.c_str()); + } + else + { + odometry_topic_ = sdf->GetElement("odometryTopic")->Get(); + } + + odometry_frame_ = "odom"; + if (!sdf->HasElement("odometryFrame")) + { + ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing , " + "defaults to \"%s\"", + robot_namespace_.c_str(), odometry_frame_.c_str()); + } + else + { + odometry_frame_ = sdf->GetElement("odometryFrame")->Get(); + } + + robot_base_frame_ = "base_footprint"; + if (!sdf->HasElement("robotBaseFrame")) + { + ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing , " + "defaults to \"%s\"", + robot_namespace_.c_str(), robot_base_frame_.c_str()); + } + else + { + robot_base_frame_ = sdf->GetElement("robotBaseFrame")->Get(); + } + + odometry_rate_ = 20.0; + if (!sdf->HasElement("odometryRate")) + { + ROS_WARN_NAMED("planar_move", "PlanarMovePlugin (ns = %s) missing , " + "defaults to %f", + robot_namespace_.c_str(), odometry_rate_); + } + else + { + odometry_rate_ = sdf->GetElement("odometryRate")->Get(); + } + +#if GAZEBO_MAJOR_VERSION >= 8 + last_odom_publish_time_ = parent_->GetWorld()->SimTime(); +#else + last_odom_publish_time_ = parent_->GetWorld()->GetSimTime(); +#endif +#if GAZEBO_MAJOR_VERSION >= 8 + last_odom_pose_ = parent_->WorldPose(); +#else + last_odom_pose_ = parent_->GetWorldPose().Ign(); +#endif + x_ = 0; + y_ = 0; + rot_ = 0; + alive_ = true; + + // Ensure that ROS has been initialized and subscribe to cmd_vel + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("planar_move", "PlanarMovePlugin (ns = " << robot_namespace_ + << "). A ROS node for Gazebo has not been initialized, " + << "unable to load plugin. Load the Gazebo system plugin " + << "'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + rosnode_.reset(new ros::NodeHandle(robot_namespace_)); + + ROS_DEBUG_NAMED("planar_move", "OCPlugin (%s) has started", + robot_namespace_.c_str()); + + tf_prefix_ = tf::getPrefixParam(*rosnode_); + transform_broadcaster_.reset(new tf::TransformBroadcaster()); + + // subscribe to the odometry topic + ros::SubscribeOptions so = + ros::SubscribeOptions::create(command_topic_, 1, + boost::bind(&GazeboRosPlanarMove::cmdVelCallback, this, _1), + ros::VoidPtr(), &queue_); + + vel_sub_ = rosnode_->subscribe(so); + odometry_pub_ = rosnode_->advertise(odometry_topic_, 1); + + // start custom queue for diff drive + callback_queue_thread_ = + boost::thread(boost::bind(&GazeboRosPlanarMove::QueueThread, this)); + + // listen to the update event (broadcast every simulation iteration) + update_connection_ = + event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosPlanarMove::UpdateChild, this)); + + } + + // Update the controller + void GazeboRosPlanarMove::UpdateChild() + { + boost::mutex::scoped_lock scoped_lock(lock); +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d pose = parent_->WorldPose(); +#else + ignition::math::Pose3d pose = parent_->GetWorldPose().Ign(); +#endif + float yaw = pose.Rot().Yaw(); + parent_->SetLinearVel(ignition::math::Vector3d( + x_ * cosf(yaw) - y_ * sinf(yaw), + y_ * cosf(yaw) + x_ * sinf(yaw), + 0)); + parent_->SetAngularVel(ignition::math::Vector3d(0, 0, rot_)); + if (odometry_rate_ > 0.0) { +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time current_time = parent_->GetWorld()->SimTime(); +#else + common::Time current_time = parent_->GetWorld()->GetSimTime(); +#endif + double seconds_since_last_update = + (current_time - last_odom_publish_time_).Double(); + if (seconds_since_last_update > (1.0 / odometry_rate_)) { + publishOdometry(seconds_since_last_update); + last_odom_publish_time_ = current_time; + } + } + } + + // Finalize the controller + void GazeboRosPlanarMove::FiniChild() { + alive_ = false; + queue_.clear(); + queue_.disable(); + rosnode_->shutdown(); + callback_queue_thread_.join(); + } + + void GazeboRosPlanarMove::cmdVelCallback( + const geometry_msgs::Twist::ConstPtr& cmd_msg) + { + boost::mutex::scoped_lock scoped_lock(lock); + x_ = cmd_msg->linear.x; + y_ = cmd_msg->linear.y; + rot_ = cmd_msg->angular.z; + } + + void GazeboRosPlanarMove::QueueThread() + { + static const double timeout = 0.01; + while (alive_ && rosnode_->ok()) + { + queue_.callAvailable(ros::WallDuration(timeout)); + } + } + + void GazeboRosPlanarMove::publishOdometry(double step_time) + { + + ros::Time current_time = ros::Time::now(); + std::string odom_frame = tf::resolve(tf_prefix_, odometry_frame_); + std::string base_footprint_frame = + tf::resolve(tf_prefix_, robot_base_frame_); + + // getting data for base_footprint to odom transform +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d pose = this->parent_->WorldPose(); +#else + ignition::math::Pose3d pose = this->parent_->GetWorldPose().Ign(); +#endif + + tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W()); + tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z()); + + tf::Transform base_footprint_to_odom(qt, vt); + transform_broadcaster_->sendTransform( + tf::StampedTransform(base_footprint_to_odom, current_time, odom_frame, + base_footprint_frame)); + + // publish odom topic + odom_.pose.pose.position.x = pose.Pos().X(); + odom_.pose.pose.position.y = pose.Pos().Y(); + + odom_.pose.pose.orientation.x = pose.Rot().X(); + odom_.pose.pose.orientation.y = pose.Rot().Y(); + odom_.pose.pose.orientation.z = pose.Rot().Z(); + odom_.pose.pose.orientation.w = pose.Rot().W(); + odom_.pose.covariance[0] = 0.00001; + odom_.pose.covariance[7] = 0.00001; + odom_.pose.covariance[14] = 1000000000000.0; + odom_.pose.covariance[21] = 1000000000000.0; + odom_.pose.covariance[28] = 1000000000000.0; + odom_.pose.covariance[35] = 0.001; + + // get velocity in /odom frame + ignition::math::Vector3d linear; + linear.X() = (pose.Pos().X() - last_odom_pose_.Pos().X()) / step_time; + linear.Y() = (pose.Pos().Y() - last_odom_pose_.Pos().Y()) / step_time; + if (rot_ > M_PI / step_time) + { + // we cannot calculate the angular velocity correctly + odom_.twist.twist.angular.z = rot_; + } + else + { + float last_yaw = last_odom_pose_.Rot().Yaw(); + float current_yaw = pose.Rot().Yaw(); + while (current_yaw < last_yaw - M_PI) current_yaw += 2 * M_PI; + while (current_yaw > last_yaw + M_PI) current_yaw -= 2 * M_PI; + float angular_diff = current_yaw - last_yaw; + odom_.twist.twist.angular.z = angular_diff / step_time; + } + last_odom_pose_ = pose; + + // convert velocity to child_frame_id (aka base_footprint) + float yaw = pose.Rot().Yaw(); + odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y(); + odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X(); + + odom_.header.stamp = current_time; + odom_.header.frame_id = odom_frame; + odom_.child_frame_id = base_footprint_frame; + + odometry_pub_.publish(odom_); + } + + GZ_REGISTER_MODEL_PLUGIN(GazeboRosPlanarMove) +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_projector.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_projector.cpp new file mode 100755 index 0000000..ec05cc0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_projector.cpp @@ -0,0 +1,176 @@ +/* + * 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: GazeboRosTextureProjector plugin that controls texture projection + Author: Jared Duke + Date: 17 Jun 2010 + */ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace gazebo +{ +GZ_REGISTER_MODEL_PLUGIN(GazeboRosProjector); + +typedef std::map OgrePassMap; +typedef OgrePassMap::iterator OgrePassMapIterator; + + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosProjector::GazeboRosProjector() +{ + this->rosnode_ = NULL; +} + + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosProjector::~GazeboRosProjector() +{ + // Custom Callback Queue + this->queue_.clear(); + this->queue_.disable(); + this->rosnode_->shutdown(); + this->callback_queue_thread_.join(); + + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosProjector::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) +{ + this->world_ = _parent->GetWorld(); + + + + + // Create a new transport node for talking to the projector + this->node_.reset(new transport::Node()); + // Initialize the node with the world name +#if GAZEBO_MAJOR_VERSION >= 8 + this->node_->Init(this->world_->Name()); +#else + this->node_->Init(this->world_->GetName()); +#endif + // Setting projector topic + std::string name = std::string("~/") + _parent->GetName() + "/" + + _sdf->Get("projector"); + // Create a publisher on the ~/physics topic + this->projector_pub_ = node_->Advertise(name); + + + + // load parameters + this->robot_namespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; + + this->texture_topic_name_ = ""; + if (_sdf->HasElement("textureTopicName")) + this->texture_topic_name_ = _sdf->GetElement("textureTopicName")->Get(); + + this->projector_topic_name_ = ""; + if (_sdf->HasElement("projectorTopicName")) + this->projector_topic_name_ = _sdf->GetElement("projectorTopicName")->Get(); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("projector", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + + // Custom Callback Queue + ros::SubscribeOptions so = ros::SubscribeOptions::create( + this->projector_topic_name_,1, + boost::bind( &GazeboRosProjector::ToggleProjector,this,_1), + ros::VoidPtr(), &this->queue_); + this->projectorSubscriber_ = this->rosnode_->subscribe(so); + + ros::SubscribeOptions so2 = ros::SubscribeOptions::create( + this->texture_topic_name_,1, + boost::bind( &GazeboRosProjector::LoadImage,this,_1), + ros::VoidPtr(), &this->queue_); + this->imageSubscriber_ = this->rosnode_->subscribe(so2); + + + // Custom Callback Queue + this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosProjector::QueueThread,this ) ); + +} + + +//////////////////////////////////////////////////////////////////////////////// +// Load a texture into the projector +void GazeboRosProjector::LoadImage(const std_msgs::String::ConstPtr& imageMsg) +{ + msgs::Projector msg; + msg.set_name("texture_projector"); + msg.set_texture(imageMsg->data); + this->projector_pub_->Publish(msg); +} + +//////////////////////////////////////////////////////////////////////////////// +// Toggle the activation of the projector +void GazeboRosProjector::ToggleProjector(const std_msgs::Int32::ConstPtr& projectorMsg) +{ + msgs::Projector msg; + msg.set_name("texture_projector"); + msg.set_enabled(projectorMsg->data); + this->projector_pub_->Publish(msg); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Custom callback queue thread +void GazeboRosProjector::QueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + { + this->queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_prosilica.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_prosilica.cpp new file mode 100755 index 0000000..cb25c41 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_prosilica.cpp @@ -0,0 +1,346 @@ +/* + * 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. + * +*/ + +/* + @mainpage + Desc: GazeboRosProsilica plugin for simulating Prosilica cameras in Gazebo + Author: John Hsu + Date: 24 Sept 2008 +*/ + +#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 + +namespace gazebo +{ + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosProsilica::GazeboRosProsilica() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosProsilica::~GazeboRosProsilica() +{ + // Finalize the controller + this->poll_srv_.shutdown(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosProsilica::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + + CameraPlugin::Load(_parent, _sdf); + this->parentSensor_ = this->parentSensor; + this->width_ = this->width; + this->height_ = this->height; + this->depth_ = this->depth; + this->format_ = this->format; + this->camera_ = this->camera; + GazeboRosCameraUtils::Load(_parent, _sdf); + + this->load_connection_ = GazeboRosCameraUtils::OnLoad(boost::bind(&GazeboRosProsilica::Advertise, this)); +} + +void GazeboRosProsilica::Advertise() +{ + // camera mode for prosilica: + // prosilica::AcquisitionMode mode_; /// @todo Make this property of Camera + + //ROS_ERROR_NAMED("prosilica", "before trigger_mode %s %s",this->mode_param_name.c_str(),this->mode_.c_str()); + + if (!this->rosnode_->searchParam("trigger_mode",this->mode_param_name)) ///\@todo: hardcoded per prosilica_camera wiki api, make this an urdf parameter + this->mode_param_name = "trigger_mode"; + + if (!this->rosnode_->getParam(this->mode_param_name,this->mode_)) + this->mode_ = "streaming"; + + ROS_INFO_NAMED("prosilica", "trigger_mode %s %s",this->mode_param_name.c_str(),this->mode_.c_str()); + + + if (this->mode_ == "polled") + { + poll_srv_ = polled_camera::advertise(*this->rosnode_,this->pollServiceName,&GazeboRosProsilica::pollCallback,this); + } + else if (this->mode_ == "streaming") + { + ROS_DEBUG_NAMED("prosilica", "do nothing here,mode: %s",this->mode_.c_str()); + } + else + { + ROS_ERROR_NAMED("prosilica", "trigger_mode is invalid: %s, using streaming mode",this->mode_.c_str()); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosProsilica::OnNewImageFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ + if (!this->rosnode_->getParam(this->mode_param_name,this->mode_)) + this->mode_ = "streaming"; + + // should do nothing except turning camera on/off, as we are using service. + /// @todo: consider adding thumbnailing feature here if subscribed. + common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime(); + + // as long as ros is connected, parent is active + //ROS_ERROR_NAMED("prosilica", "debug image count %d",this->image_connect_count_); + if (!this->parentSensor->IsActive()) + { + if ((*this->image_connect_count_) > 0) + // do this first so there's chance for sensor to run 1 frame after activate + this->parentSensor->SetActive(true); + } + else + { + //ROS_ERROR_NAMED("prosilica", "camera_ new frame %s %s",this->parentSensor_->Name().c_str(),this->frame_name_.c_str()); + + if (this->mode_ == "streaming") + { + if ((*this->image_connect_count_) > 0) + { + if (sensor_update_time - this->last_update_time_ >= this->update_period_) + { + this->PutCameraData(_image, sensor_update_time); + this->PublishCameraInfo(sensor_update_time); + this->last_update_time_ = sensor_update_time; + } + } + } + + } +} + + +//////////////////////////////////////////////////////////////////////////////// +// new prosilica interface. +void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& req, + polled_camera::GetPolledImage::Response& rsp, + sensor_msgs::Image& image, sensor_msgs::CameraInfo& info) +{ + if (!this->rosnode_->getParam(this->mode_param_name,this->mode_)) + this->mode_ = "streaming"; + + /// @todo Support binning (maybe just cv::resize) + /// @todo Don't adjust K, P for ROI, set CameraInfo.roi fields instead + /// @todo D parameter order is k1, k2, t1, t2, k3 + + if (this->mode_ != "polled") + { + rsp.success = false; + rsp.status_message = "Camera is not in triggered mode"; + return; + } + + if (req.binning_x > 1 || req.binning_y > 1) + { + rsp.success = false; + rsp.status_message = "Gazebo Prosilica plugin does not support binning"; + return; + } + + // get region from request + if (req.roi.x_offset <= 0 || req.roi.y_offset <= 0 || req.roi.width <= 0 || req.roi.height <= 0) + { + req.roi.x_offset = 0; + req.roi.y_offset = 0; + req.roi.width = this->width_; + req.roi.height = this->height; + } + const unsigned char *src = NULL; + ROS_ERROR_NAMED("prosilica", "roidebug %d %d %d %d", req.roi.x_offset, req.roi.y_offset, req.roi.width, req.roi.height); + + // signal sensor to start update + (*this->image_connect_count_)++; + + // wait until an image has been returned + while(!src) + { + { + // Get a pointer to image data + src = this->parentSensor->Camera()->ImageData(0); + + if (src) + { + + // fill CameraInfo + this->roiCameraInfoMsg = &info; + this->roiCameraInfoMsg->header.frame_id = this->frame_name_; + + common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime(); + this->roiCameraInfoMsg->header.stamp.sec = roiLastRenderTime.sec; + this->roiCameraInfoMsg->header.stamp.nsec = roiLastRenderTime.nsec; + + this->roiCameraInfoMsg->width = req.roi.width; //this->parentSensor->ImageWidth() ; + this->roiCameraInfoMsg->height = req.roi.height; //this->parentSensor->ImageHeight(); + // distortion +#if ROS_VERSION_MINIMUM(1, 3, 0) + this->roiCameraInfoMsg->distortion_model = "plumb_bob"; + this->roiCameraInfoMsg->D.resize(5); +#endif + this->roiCameraInfoMsg->D[0] = this->distortion_k1_; + this->roiCameraInfoMsg->D[1] = this->distortion_k2_; + this->roiCameraInfoMsg->D[2] = this->distortion_k3_; + this->roiCameraInfoMsg->D[3] = this->distortion_t1_; + this->roiCameraInfoMsg->D[4] = this->distortion_t2_; + // original camera matrix + this->roiCameraInfoMsg->K[0] = this->focal_length_; + this->roiCameraInfoMsg->K[1] = 0.0; + this->roiCameraInfoMsg->K[2] = this->cx_ - req.roi.x_offset; + this->roiCameraInfoMsg->K[3] = 0.0; + this->roiCameraInfoMsg->K[4] = this->focal_length_; + this->roiCameraInfoMsg->K[5] = this->cy_ - req.roi.y_offset; + this->roiCameraInfoMsg->K[6] = 0.0; + this->roiCameraInfoMsg->K[7] = 0.0; + this->roiCameraInfoMsg->K[8] = 1.0; + // rectification + this->roiCameraInfoMsg->R[0] = 1.0; + this->roiCameraInfoMsg->R[1] = 0.0; + this->roiCameraInfoMsg->R[2] = 0.0; + this->roiCameraInfoMsg->R[3] = 0.0; + this->roiCameraInfoMsg->R[4] = 1.0; + this->roiCameraInfoMsg->R[5] = 0.0; + this->roiCameraInfoMsg->R[6] = 0.0; + this->roiCameraInfoMsg->R[7] = 0.0; + this->roiCameraInfoMsg->R[8] = 1.0; + // camera projection matrix (same as camera matrix due to lack of distortion/rectification) (is this generated?) + this->roiCameraInfoMsg->P[0] = this->focal_length_; + this->roiCameraInfoMsg->P[1] = 0.0; + this->roiCameraInfoMsg->P[2] = this->cx_ - req.roi.x_offset; + this->roiCameraInfoMsg->P[3] = -this->focal_length_ * this->hack_baseline_; + this->roiCameraInfoMsg->P[4] = 0.0; + this->roiCameraInfoMsg->P[5] = this->focal_length_; + this->roiCameraInfoMsg->P[6] = this->cy_ - req.roi.y_offset; + this->roiCameraInfoMsg->P[7] = 0.0; + this->roiCameraInfoMsg->P[8] = 0.0; + this->roiCameraInfoMsg->P[9] = 0.0; + this->roiCameraInfoMsg->P[10] = 1.0; + this->roiCameraInfoMsg->P[11] = 0.0; + this->camera_info_pub_.publish(*this->roiCameraInfoMsg); + + // copy data into image_msg_, then convert to roiImageMsg(image) + this->image_msg_.header.frame_id = this->frame_name_; + + common::Time lastRenderTime = this->parentSensor_->LastMeasurementTime(); + this->image_msg_.header.stamp.sec = lastRenderTime.sec; + this->image_msg_.header.stamp.nsec = lastRenderTime.nsec; + + //unsigned char dst[this->width_*this->height]; + + /// @todo: don't bother if there are no subscribers + + // copy from src to image_msg_ + fillImage(this->image_msg_, + this->type_, + this->height_, + this->width_, + this->skip_*this->width_, + (void*)src ); + + /// @todo: publish to ros, thumbnails and rect image in the Update call? + + this->image_pub_.publish(this->image_msg_); + + { + // copy data into ROI image + this->roiImageMsg = ℑ + this->roiImageMsg->header.frame_id = this->frame_name_; + common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime(); + this->roiImageMsg->header.stamp.sec = roiLastRenderTime.sec; + this->roiImageMsg->header.stamp.nsec = roiLastRenderTime.nsec; + + // convert image_msg_ to a CvImage using cv_bridge + boost::shared_ptr img_bridge_; + img_bridge_ = cv_bridge::toCvCopy(this->image_msg_); + + // for debug + //cvNamedWindow("showme",CV_WINDOW_AUTOSIZE); + //cvSetMouseCallback("showme", &GazeboRosProsilica::mouse_cb, this); + //cvStartWindowThread(); + //cvShowImage("showme",img_bridge_.toIpl()); + + // crop image to roi + cv::Mat roi(img_bridge_->image, + cv::Rect(req.roi.x_offset, req.roi.y_offset, + req.roi.width, req.roi.height)); + img_bridge_->image = roi; + + // copy roi'd image into roiImageMsg + img_bridge_->toImageMsg(*this->roiImageMsg); + } + } + } + usleep(100000); + } + (*this->image_connect_count_)--; + rsp.success = true; + return; +} + + +/* +void GazeboRosProsilica::OnStats( const boost::shared_ptr &_msg) +{ + this->simTime = msgs::Convert( _msg->sim_time() ); + + ignition::math::Pose3d pose; + pose.Pos().X() = 0.5*sin(0.01*this->simTime.Double()); + gzdbg << "plugin simTime [" << this->simTime.Double() << "] update pose [" << pose.Pos().X() << "]\n"; +} +*/ + +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosProsilica) + + +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_range.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_range.cpp new file mode 100755 index 0000000..0439552 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_range.cpp @@ -0,0 +1,348 @@ +/* + * Software License Agreement (Modified BSD License) + * + * Copyright (c) 2013-2015, PAL Robotics, S.L. + * All rights reserved. + * + * 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 PAL Robotics, S.L. 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 Jose Capriles, Bence Magyar. */ + +#include "gazebo_plugins/gazebo_ros_range.h" +#include "gazebo_plugins/gazebo_ros_utils.h" + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosRange) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosRange::GazeboRosRange() +{ + this->seed = 0; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosRange::~GazeboRosRange() +{ + this->range_queue_.clear(); + this->range_queue_.disable(); + this->rosnode_->shutdown(); + this->callback_queue_thread_.join(); + + delete this->rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosRange::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + // load plugin + RayPlugin::Load(_parent, this->sdf); + // Get then name of the parent sensor + this->parent_sensor_ = _parent; + // Get the world name. + std::string worldName = _parent->WorldName(); + this->world_ = physics::get_world(worldName); + // save pointers + this->sdf = _sdf; + + this->last_update_time_ = common::Time(0); + + GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST; + this->parent_ray_sensor_ = + dynamic_pointer_cast(this->parent_sensor_); + + if (!this->parent_ray_sensor_) + gzthrow("GazeboRosRange controller requires a Ray Sensor as its parent"); + + this->robot_namespace_ = ""; + if (this->sdf->HasElement("robotNamespace")) + this->robot_namespace_ = this->sdf->Get("robotNamespace") + "/"; + + if (!this->sdf->HasElement("frameName")) + { + ROS_INFO_NAMED("range", "Range plugin missing , defaults to /world"); + this->frame_name_ = "/world"; + } + else + this->frame_name_ = this->sdf->Get("frameName"); + + if (!this->sdf->HasElement("topicName")) + { + ROS_INFO_NAMED("range", "Range plugin missing , defaults to /range"); + this->topic_name_ = "/range"; + } + else + this->topic_name_ = this->sdf->Get("topicName"); + + if (!this->sdf->HasElement("radiation")) + { + ROS_WARN_NAMED("range", "Range plugin missing , defaults to ultrasound"); + this->radiation_ = "ultrasound"; + + } + else + this->radiation_ = _sdf->GetElement("radiation")->Get(); + + if (!this->sdf->HasElement("fov")) + { + ROS_WARN_NAMED("range", "Range plugin missing , defaults to 0.05"); + this->fov_ = 0.05; + } + else + this->fov_ = _sdf->GetElement("fov")->Get(); + if (!this->sdf->HasElement("gaussianNoise")) + { + ROS_INFO_NAMED("range", "Range plugin missing , defaults to 0.0"); + this->gaussian_noise_ = 0; + } + else + this->gaussian_noise_ = this->sdf->Get("gaussianNoise"); + + if (!this->sdf->HasElement("updateRate")) + { + ROS_INFO_NAMED("range", "Range plugin missing , defaults to 0"); + this->update_rate_ = 0; + } + else + this->update_rate_ = this->sdf->Get("updateRate"); + + // prepare to throttle this plugin at the same rate + // ideally, we should invoke a plugin update when the sensor updates, + // have to think about how to do that properly later + if (this->update_rate_ > 0.0) + this->update_period_ = 1.0/this->update_rate_; + else + this->update_period_ = 0.0; + + this->range_connect_count_ = 0; + + this->range_msg_.header.frame_id = this->frame_name_; + if (this->radiation_==std::string("ultrasound")) + this->range_msg_.radiation_type = sensor_msgs::Range::ULTRASOUND; + else + this->range_msg_.radiation_type = sensor_msgs::Range::INFRARED; + + this->range_msg_.field_of_view = fov_; + this->range_msg_.max_range = this->parent_ray_sensor_->RangeMax(); + this->range_msg_.min_range = this->parent_ray_sensor_->RangeMin(); + + // Init ROS + if (ros::isInitialized()) + { + // ros callback queue for processing subscription + this->deferred_load_thread_ = boost::thread( + boost::bind(&GazeboRosRange::LoadThread, this)); + } + else + { + gzerr << "Not loading plugin since ROS hasn't been " + << "properly initialized. Try starting gazebo with ros plugin:\n" + << " gazebo -s libgazebo_ros_api_plugin.so\n"; + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosRange::LoadThread() +{ + this->rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + // resolve tf prefix + std::string prefix; + this->rosnode_->getParam(std::string("tf_prefix"), prefix); + this->frame_name_ = tf::resolve(prefix, this->frame_name_); + + if (this->topic_name_ != "") + { + ros::AdvertiseOptions ao = + ros::AdvertiseOptions::create( + this->topic_name_, 1, + boost::bind(&GazeboRosRange::RangeConnect, this), + boost::bind(&GazeboRosRange::RangeDisconnect, this), + ros::VoidPtr(), &this->range_queue_); + this->pub_ = this->rosnode_->advertise(ao); + } + + + // Initialize the controller + + // sensor generation off by default + this->parent_ray_sensor_->SetActive(false); + // start custom queue for range + this->callback_queue_thread_ = + boost::thread(boost::bind(&GazeboRosRange::RangeQueueThread, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Increment count +void GazeboRosRange::RangeConnect() +{ + this->range_connect_count_++; + this->parent_ray_sensor_->SetActive(true); +} +//////////////////////////////////////////////////////////////////////////////// +// Decrement count +void GazeboRosRange::RangeDisconnect() +{ + this->range_connect_count_--; + + if (this->range_connect_count_ == 0) + this->parent_ray_sensor_->SetActive(false); +} + + +//////////////////////////////////////////////////////////////////////////////// +// Update the plugin +void GazeboRosRange::OnNewLaserScans() +{ + if (this->topic_name_ != "") + { +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time cur_time = this->world_->SimTime(); +#else + common::Time cur_time = this->world_->GetSimTime(); +#endif + if (cur_time < this->last_update_time_) + { + ROS_WARN_NAMED("range", "Negative sensor update time difference detected."); + this->last_update_time_ = cur_time; + } + + if (cur_time - this->last_update_time_ >= this->update_period_) + { + common::Time sensor_update_time = + this->parent_sensor_->LastUpdateTime(); + this->PutRangeData(sensor_update_time); + this->last_update_time_ = cur_time; + } + } + else + { + ROS_INFO_NAMED("range", "gazebo_ros_range topic name not set"); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Put range data to the interface +void GazeboRosRange::PutRangeData(common::Time &_updateTime) +{ + this->parent_ray_sensor_->SetActive(false); + + /***************************************************************/ + /* */ + /* point scan from ray sensor */ + /* */ + /***************************************************************/ + { + boost::mutex::scoped_lock lock(this->lock_); + // Add Frame Name + this->range_msg_.header.frame_id = this->frame_name_; + this->range_msg_.header.stamp.sec = _updateTime.sec; + this->range_msg_.header.stamp.nsec = _updateTime.nsec; + + // find ray with minimal range + range_msg_.range = std::numeric_limits::max(); + + int num_ranges = parent_ray_sensor_->LaserShape()->GetSampleCount() * parent_ray_sensor_->LaserShape()->GetVerticalSampleCount(); + + for(int i = 0; i < num_ranges; ++i) + { + double ray = parent_ray_sensor_->LaserShape()->GetRange(i); + if (ray < range_msg_.range) + range_msg_.range = ray; + } + + // add Gaussian noise and limit to min/max range + if (range_msg_.range < range_msg_.max_range) + range_msg_.range = std::min(range_msg_.range + this->GaussianKernel(0,gaussian_noise_), parent_ray_sensor_->RangeMax()); + + this->parent_ray_sensor_->SetActive(true); + + // send data out via ros message + if (this->range_connect_count_ > 0 && this->topic_name_ != "") + this->pub_.publish(this->range_msg_); + } +} + +////////////////////////////////////////////////////////////////////////////// +// Utility for adding noise +double GazeboRosRange::GaussianKernel(double mu, double sigma) +{ + // using Box-Muller transform to generate two independent standard + // normally disbributed normal variables see wikipedia + + // normalized uniform random variable + double U = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + // normalized uniform random variable + double V = static_cast(rand_r(&this->seed)) / + static_cast(RAND_MAX); + + double X = sqrt(-2.0 * ::log(U)) * cos(2.0*M_PI * V); + // double Y = sqrt(-2.0 * ::log(U)) * sin(2.0*M_PI * V); + + // there are 2 indep. vars, we'll just use X + // scale to our mu and sigma + X = sigma * X + mu; + return X; +} + +//////////////////////////////////////////////////////////////////////////////// +// Put range data to the interface +void GazeboRosRange::RangeQueueThread() +{ + static const double timeout = 0.01; + + while (this->rosnode_->ok()) + { + this->range_queue_.callAvailable(ros::WallDuration(timeout)); + } +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp new file mode 100755 index 0000000..61d23ab --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_skid_steer_drive.cpp @@ -0,0 +1,478 @@ +/* + * Copyright (c) 2010, Daniel Hewlett, Antons Rebguns + * All rights reserved. + * + * 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 the 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 Antons Rebguns ''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 Antons Rebguns 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 gazebo_ros_skid_steer_drive.cpp + * + * \brief A skid steering drive plugin. Inspired by gazebo_ros_diff_drive and SkidSteerDrivePlugin + * + * \author Zdenek Materna (imaterna@fit.vutbr.cz) + * + * $ Id: 06/25/2013 11:23:40 AM materna $ + */ + + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo { + + enum { + RIGHT_FRONT=0, + LEFT_FRONT=1, + RIGHT_REAR=2, + LEFT_REAR=3, + }; + + GazeboRosSkidSteerDrive::GazeboRosSkidSteerDrive() {} + + // Destructor + GazeboRosSkidSteerDrive::~GazeboRosSkidSteerDrive() { + delete rosnode_; + delete transform_broadcaster_; + } + + // Load the controller + void GazeboRosSkidSteerDrive::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { + + this->parent = _parent; + this->world = _parent->GetWorld(); + + this->robot_namespace_ = ""; + if (!_sdf->HasElement("robotNamespace")) { + ROS_INFO_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin missing , defaults to \"%s\"", + this->robot_namespace_.c_str()); + } else { + this->robot_namespace_ = + _sdf->GetElement("robotNamespace")->Get() + "/"; + } + + this->broadcast_tf_ = false; + if (!_sdf->HasElement("broadcastTF")) { + if (!this->broadcast_tf_) + ROS_INFO_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to false.",this->robot_namespace_.c_str()); + else ROS_INFO_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to true.",this->robot_namespace_.c_str()); + + } else { + this->broadcast_tf_ = _sdf->GetElement("broadcastTF")->Get(); + } + + // TODO write error if joint doesn't exist! + this->left_front_joint_name_ = "left_front_joint"; + if (!_sdf->HasElement("leftFrontJoint")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->left_front_joint_name_.c_str()); + } else { + this->left_front_joint_name_ = _sdf->GetElement("leftFrontJoint")->Get(); + } + + this->right_front_joint_name_ = "right_front_joint"; + if (!_sdf->HasElement("rightFrontJoint")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->right_front_joint_name_.c_str()); + } else { + this->right_front_joint_name_ = _sdf->GetElement("rightFrontJoint")->Get(); + } + + this->left_rear_joint_name_ = "left_rear_joint"; + if (!_sdf->HasElement("leftRearJoint")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->left_rear_joint_name_.c_str()); + } else { + this->left_rear_joint_name_ = _sdf->GetElement("leftRearJoint")->Get(); + } + + this->right_rear_joint_name_ = "right_rear_joint"; + if (!_sdf->HasElement("rightRearJoint")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->right_rear_joint_name_.c_str()); + } else { + this->right_rear_joint_name_ = _sdf->GetElement("rightRearJoint")->Get(); + } + + + // This assumes that front and rear wheel spacing is identical + /*this->wheel_separation_ = this->parent->GetJoint(left_front_joint_name_)->GetAnchor(0).Distance( + this->parent->GetJoint(right_front_joint_name_)->GetAnchor(0));*/ + + this->wheel_separation_ = 0.4; + + if (!_sdf->HasElement("wheelSeparation")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to value from robot_description: %f", + this->robot_namespace_.c_str(), this->wheel_separation_); + } else { + this->wheel_separation_ = + _sdf->GetElement("wheelSeparation")->Get(); + } + + // TODO get this from robot_description + this->wheel_diameter_ = 0.15; + if (!_sdf->HasElement("wheelDiameter")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->wheel_diameter_); + } else { + this->wheel_diameter_ = _sdf->GetElement("wheelDiameter")->Get(); + } + + this->torque = 5.0; + if (!_sdf->HasElement("torque")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->torque); + } else { + this->torque = _sdf->GetElement("torque")->Get(); + } + + this->command_topic_ = "cmd_vel"; + if (!_sdf->HasElement("commandTopic")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->command_topic_.c_str()); + } else { + this->command_topic_ = _sdf->GetElement("commandTopic")->Get(); + } + + this->odometry_topic_ = "odom"; + if (!_sdf->HasElement("odometryTopic")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->odometry_topic_.c_str()); + } else { + this->odometry_topic_ = _sdf->GetElement("odometryTopic")->Get(); + } + + this->odometry_frame_ = "odom"; + if (!_sdf->HasElement("odometryFrame")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->odometry_frame_.c_str()); + } else { + this->odometry_frame_ = _sdf->GetElement("odometryFrame")->Get(); + } + + this->robot_base_frame_ = "base_footprint"; + if (!_sdf->HasElement("robotBaseFrame")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to \"%s\"", + this->robot_namespace_.c_str(), this->robot_base_frame_.c_str()); + } else { + this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get(); + } + + this->update_rate_ = 100.0; + if (!_sdf->HasElement("updateRate")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), this->update_rate_); + } else { + this->update_rate_ = _sdf->GetElement("updateRate")->Get(); + } + + this->covariance_x_ = 0.0001; + if (!_sdf->HasElement("covariance_x")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), covariance_x_); + } else { + covariance_x_ = _sdf->GetElement("covariance_x")->Get(); + } + + this->covariance_y_ = 0.0001; + if (!_sdf->HasElement("covariance_y")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), covariance_y_); + } else { + covariance_y_ = _sdf->GetElement("covariance_y")->Get(); + } + + this->covariance_yaw_ = 0.01; + if (!_sdf->HasElement("covariance_yaw")) { + ROS_WARN_NAMED("skid_steer_drive", "GazeboRosSkidSteerDrive Plugin (ns = %s) missing , defaults to %f", + this->robot_namespace_.c_str(), covariance_yaw_); + } else { + covariance_yaw_ = _sdf->GetElement("covariance_yaw")->Get(); + } + + // Initialize update rate stuff + if (this->update_rate_ > 0.0) { + this->update_period_ = 1.0 / this->update_rate_; + } else { + this->update_period_ = 0.0; + } +#if GAZEBO_MAJOR_VERSION >= 8 + last_update_time_ = this->world->SimTime(); +#else + last_update_time_ = this->world->GetSimTime(); +#endif + + // Initialize velocity stuff + wheel_speed_[RIGHT_FRONT] = 0; + wheel_speed_[LEFT_FRONT] = 0; + wheel_speed_[RIGHT_REAR] = 0; + wheel_speed_[LEFT_REAR] = 0; + + x_ = 0; + rot_ = 0; + alive_ = true; + + joints[LEFT_FRONT] = this->parent->GetJoint(left_front_joint_name_); + joints[RIGHT_FRONT] = this->parent->GetJoint(right_front_joint_name_); + joints[LEFT_REAR] = this->parent->GetJoint(left_rear_joint_name_); + joints[RIGHT_REAR] = this->parent->GetJoint(right_rear_joint_name_); + + if (!joints[LEFT_FRONT]) { + char error[200]; + snprintf(error, 200, + "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get left front hinge joint named \"%s\"", + this->robot_namespace_.c_str(), this->left_front_joint_name_.c_str()); + gzthrow(error); + } + + if (!joints[RIGHT_FRONT]) { + char error[200]; + snprintf(error, 200, + "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get right front hinge joint named \"%s\"", + this->robot_namespace_.c_str(), this->right_front_joint_name_.c_str()); + gzthrow(error); + } + + if (!joints[LEFT_REAR]) { + char error[200]; + snprintf(error, 200, + "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get left rear hinge joint named \"%s\"", + this->robot_namespace_.c_str(), this->left_rear_joint_name_.c_str()); + gzthrow(error); + } + + if (!joints[RIGHT_REAR]) { + char error[200]; + snprintf(error, 200, + "GazeboRosSkidSteerDrive Plugin (ns = %s) couldn't get right rear hinge joint named \"%s\"", + this->robot_namespace_.c_str(), this->right_rear_joint_name_.c_str()); + gzthrow(error); + } + +#if GAZEBO_MAJOR_VERSION > 2 + joints[LEFT_FRONT]->SetParam("fmax", 0, torque); + joints[RIGHT_FRONT]->SetParam("fmax", 0, torque); + joints[LEFT_REAR]->SetParam("fmax", 0, torque); + joints[RIGHT_REAR]->SetParam("fmax", 0, torque); +#else + joints[LEFT_FRONT]->SetMaxForce(0, torque); + joints[RIGHT_FRONT]->SetMaxForce(0, torque); + joints[LEFT_REAR]->SetMaxForce(0, torque); + joints[RIGHT_REAR]->SetMaxForce(0, torque); +#endif + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("skid_steer_drive", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + rosnode_ = new ros::NodeHandle(this->robot_namespace_); + + ROS_INFO_NAMED("skid_steer_drive", "Starting GazeboRosSkidSteerDrive Plugin (ns = %s)", this->robot_namespace_.c_str()); + + tf_prefix_ = tf::getPrefixParam(*rosnode_); + transform_broadcaster_ = new tf::TransformBroadcaster(); + + // ROS: Subscribe to the velocity command topic (usually "cmd_vel") + ros::SubscribeOptions so = + ros::SubscribeOptions::create(command_topic_, 1, + boost::bind(&GazeboRosSkidSteerDrive::cmdVelCallback, this, _1), + ros::VoidPtr(), &queue_); + + cmd_vel_subscriber_ = rosnode_->subscribe(so); + + odometry_publisher_ = rosnode_->advertise(odometry_topic_, 1); + + // start custom queue for diff drive + this->callback_queue_thread_ = + boost::thread(boost::bind(&GazeboRosSkidSteerDrive::QueueThread, this)); + + // listen to the update event (broadcast every simulation iteration) + this->update_connection_ = + event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosSkidSteerDrive::UpdateChild, this)); + + } + + // Update the controller + void GazeboRosSkidSteerDrive::UpdateChild() { +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time current_time = this->world->SimTime(); +#else + common::Time current_time = this->world->GetSimTime(); +#endif + double seconds_since_last_update = + (current_time - last_update_time_).Double(); + if (seconds_since_last_update > update_period_) { + + publishOdometry(seconds_since_last_update); + + // Update robot in case new velocities have been requested + getWheelVelocities(); +#if GAZEBO_MAJOR_VERSION > 2 + joints[LEFT_FRONT]->SetParam("vel", 0, wheel_speed_[LEFT_FRONT] / (wheel_diameter_ / 2.0)); + joints[RIGHT_FRONT]->SetParam("vel", 0, wheel_speed_[RIGHT_FRONT] / (wheel_diameter_ / 2.0)); + joints[LEFT_REAR]->SetParam("vel", 0, wheel_speed_[LEFT_REAR] / (wheel_diameter_ / 2.0)); + joints[RIGHT_REAR]->SetParam("vel", 0, wheel_speed_[RIGHT_REAR] / (wheel_diameter_ / 2.0)); +#else + joints[LEFT_FRONT]->SetVelocity(0, wheel_speed_[LEFT_FRONT] / (wheel_diameter_ / 2.0)); + joints[RIGHT_FRONT]->SetVelocity(0, wheel_speed_[RIGHT_FRONT] / (wheel_diameter_ / 2.0)); + joints[LEFT_REAR]->SetVelocity(0, wheel_speed_[LEFT_REAR] / (wheel_diameter_ / 2.0)); + joints[RIGHT_REAR]->SetVelocity(0, wheel_speed_[RIGHT_REAR] / (wheel_diameter_ / 2.0)); +#endif + + last_update_time_+= common::Time(update_period_); + + } + } + + // Finalize the controller + void GazeboRosSkidSteerDrive::FiniChild() { + alive_ = false; + queue_.clear(); + queue_.disable(); + rosnode_->shutdown(); + callback_queue_thread_.join(); + } + + void GazeboRosSkidSteerDrive::getWheelVelocities() { + boost::mutex::scoped_lock scoped_lock(lock); + + double vr = x_; + double va = rot_; + + wheel_speed_[RIGHT_FRONT] = vr + va * wheel_separation_ / 2.0; + wheel_speed_[RIGHT_REAR] = vr + va * wheel_separation_ / 2.0; + + wheel_speed_[LEFT_FRONT] = vr - va * wheel_separation_ / 2.0; + wheel_speed_[LEFT_REAR] = vr - va * wheel_separation_ / 2.0; + + } + + void GazeboRosSkidSteerDrive::cmdVelCallback( + const geometry_msgs::Twist::ConstPtr& cmd_msg) { + + boost::mutex::scoped_lock scoped_lock(lock); + x_ = cmd_msg->linear.x; + rot_ = cmd_msg->angular.z; + } + + void GazeboRosSkidSteerDrive::QueueThread() { + static const double timeout = 0.01; + + while (alive_ && rosnode_->ok()) { + queue_.callAvailable(ros::WallDuration(timeout)); + } + } + + void GazeboRosSkidSteerDrive::publishOdometry(double step_time) { + ros::Time current_time = ros::Time::now(); + std::string odom_frame = + tf::resolve(tf_prefix_, odometry_frame_); + std::string base_footprint_frame = + tf::resolve(tf_prefix_, robot_base_frame_); + + // TODO create some non-perfect odometry! + // getting data for base_footprint to odom transform +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d pose = this->parent->WorldPose(); +#else + ignition::math::Pose3d pose = this->parent->GetWorldPose().Ign(); +#endif + + tf::Quaternion qt(pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W()); + tf::Vector3 vt(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z()); + + tf::Transform base_footprint_to_odom(qt, vt); + if (this->broadcast_tf_) { + + transform_broadcaster_->sendTransform( + tf::StampedTransform(base_footprint_to_odom, current_time, + odom_frame, base_footprint_frame)); + + } + + // publish odom topic + odom_.pose.pose.position.x = pose.Pos().X(); + odom_.pose.pose.position.y = pose.Pos().Y(); + + odom_.pose.pose.orientation.x = pose.Rot().X(); + odom_.pose.pose.orientation.y = pose.Rot().Y(); + odom_.pose.pose.orientation.z = pose.Rot().Z(); + odom_.pose.pose.orientation.w = pose.Rot().W(); + odom_.pose.covariance[0] = this->covariance_x_; + odom_.pose.covariance[7] = this->covariance_y_; + odom_.pose.covariance[14] = 1000000000000.0; + odom_.pose.covariance[21] = 1000000000000.0; + odom_.pose.covariance[28] = 1000000000000.0; + odom_.pose.covariance[35] = this->covariance_yaw_; + + // get velocity in /odom frame + ignition::math::Vector3d linear; +#if GAZEBO_MAJOR_VERSION >= 8 + linear = this->parent->WorldLinearVel(); + odom_.twist.twist.angular.z = this->parent->WorldAngularVel().Z(); +#else + linear = this->parent->GetWorldLinearVel().Ign(); + odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().Ign().Z(); +#endif + + // convert velocity to child_frame_id (aka base_footprint) + float yaw = pose.Rot().Yaw(); + odom_.twist.twist.linear.x = cosf(yaw) * linear.X() + sinf(yaw) * linear.Y(); + odom_.twist.twist.linear.y = cosf(yaw) * linear.Y() - sinf(yaw) * linear.X(); + odom_.twist.covariance[0] = this->covariance_x_; + odom_.twist.covariance[7] = this->covariance_y_; + odom_.twist.covariance[14] = 1000000000000.0; + odom_.twist.covariance[21] = 1000000000000.0; + odom_.twist.covariance[28] = 1000000000000.0; + odom_.twist.covariance[35] = this->covariance_yaw_; + + odom_.header.stamp = current_time; + odom_.header.frame_id = odom_frame; + odom_.child_frame_id = base_footprint_frame; + + odometry_publisher_.publish(odom_); + } + + GZ_REGISTER_MODEL_PLUGIN(GazeboRosSkidSteerDrive) +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_template.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_template.cpp new file mode 100755 index 0000000..2aeb34e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_template.cpp @@ -0,0 +1,80 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * All rights reserved. + * + * 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 the Open Source Robotics Foundation + * 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 Dave Coleman + * \desc Example ROS plugin for Gazebo + */ + +#include +#include + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboRosTemplate); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosTemplate::GazeboRosTemplate() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosTemplate::~GazeboRosTemplate() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosTemplate::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) +{ + // Make sure the ROS node for Gazebo has already been initalized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("template", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosTemplate::UpdateChild() +{ +} + +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp new file mode 100755 index 0000000..3e05df0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_tricycle_drive.cpp @@ -0,0 +1,485 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * 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 the copyright holder 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 gazebo_ros_tricycle_drive.cpp + * \brief A tricycle drive plugin for gazebo. + * \author Markus Bader + * \date 22th of June 2014 + */ + + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace gazebo +{ + +enum { + RIGHT, + LEFT, +}; + +GazeboRosTricycleDrive::GazeboRosTricycleDrive() {} + +// Destructor +GazeboRosTricycleDrive::~GazeboRosTricycleDrive() {} + +// Load the controller +void GazeboRosTricycleDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) +{ + parent = _parent; + gazebo_ros_ = GazeboRosPtr ( new GazeboRos ( _parent, _sdf, "TricycleDrive" ) ); + // Make sure the ROS node for Gazebo has already been initialized + gazebo_ros_->isInitialized(); + + gazebo_ros_->getParameter ( diameter_actuated_wheel_, "actuatedWheelDiameter", 0.15 ); + gazebo_ros_->getParameter ( diameter_encoder_wheel_, "encoderWheelDiameter", 0.15 ); + gazebo_ros_->getParameter ( wheel_torque_, "wheelTorque", 5.0 ); + gazebo_ros_->getParameter ( command_topic_, "commandTopic", "cmd_vel" ); + gazebo_ros_->getParameter ( odometry_topic_, "odometryTopic", "odom" ); + gazebo_ros_->getParameter ( odometry_frame_, "odometryFrame", "odom" ); + gazebo_ros_->getParameter ( robot_base_frame_, "robotBaseFrame", "base_link" ); + + gazebo_ros_->getParameter ( update_rate_, "updateRate", 100.0 ); + gazebo_ros_->getParameter ( wheel_acceleration_, "wheelAcceleration", 0 ); + gazebo_ros_->getParameter ( wheel_deceleration_, "wheelDeceleration", wheel_acceleration_ ); + gazebo_ros_->getParameter ( wheel_speed_tolerance_, "wheelSpeedTolerance", 0.01 ); + gazebo_ros_->getParameter ( steering_speed_, "steeringSpeed", 0 ); + gazebo_ros_->getParameter ( steering_angle_tolerance_, "steeringAngleTolerance", 0.01 ); + gazebo_ros_->getParameter ( separation_encoder_wheel_, "encoderWheelSeparation", 0.5 ); + + gazebo_ros_->getParameterBoolean ( publishWheelTF_, "publishWheelTF", false ); + gazebo_ros_->getParameterBoolean ( publishWheelJointState_, "publishWheelJointState", false ); + + gazebo_ros_->getParameterBoolean ( publishWheelJointState_, "publishWheelJointState", false ); + + joint_steering_ = gazebo_ros_->getJoint ( parent, "steeringJoint", "front_steering_joint" ); + joint_wheel_actuated_ = gazebo_ros_->getJoint ( parent, "actuatedWheelJoint", "front_wheel_joint" ); + joint_wheel_encoder_left_ = gazebo_ros_->getJoint ( parent, "encoderWheelLeftJoint", "left_wheel_joint" ); + joint_wheel_encoder_right_ = gazebo_ros_->getJoint ( parent, "encoderWheelRightJoint", "right_wheel_joint" ); + + std::map odomOptions; + odomOptions["encoder"] = ENCODER; + odomOptions["world"] = WORLD; + gazebo_ros_->getParameter ( odom_source_, "odometrySource", odomOptions, WORLD ); + + joint_wheel_actuated_->SetParam ( "fmax", 0, wheel_torque_ ); + joint_steering_->SetParam ( "fmax", 0, wheel_torque_ ); + + + // Initialize update rate stuff + if ( this->update_rate_ > 0.0 ) this->update_period_ = 1.0 / this->update_rate_; + else this->update_period_ = 0.0; +#if GAZEBO_MAJOR_VERSION >= 8 + last_actuator_update_ = parent->GetWorld()->SimTime(); +#else + last_actuator_update_ = parent->GetWorld()->GetSimTime(); +#endif + + // Initialize velocity stuff + alive_ = true; + + if ( this->publishWheelJointState_ ) { + joint_state_publisher_ = gazebo_ros_->node()->advertise ( "joint_states", 1000 ); + ROS_INFO_NAMED("tricycle_drive", "%s: Advertise joint_states", gazebo_ros_->info() ); + } + + transform_broadcaster_ = boost::shared_ptr ( new tf::TransformBroadcaster() ); + + // ROS: Subscribe to the velocity command topic (usually "cmd_vel") + ROS_INFO_NAMED("tricycle_drive", "%s: Try to subscribe to %s", gazebo_ros_->info(), command_topic_.c_str() ); + + ros::SubscribeOptions so = + ros::SubscribeOptions::create ( command_topic_, 1, + boost::bind ( &GazeboRosTricycleDrive::cmdVelCallback, this, _1 ), + ros::VoidPtr(), &queue_ ); + + cmd_vel_subscriber_ = gazebo_ros_->node()->subscribe ( so ); + ROS_INFO_NAMED("tricycle_drive", "%s: Subscribe to %s", gazebo_ros_->info(), command_topic_.c_str() ); + + odometry_publisher_ = gazebo_ros_->node()->advertise ( odometry_topic_, 1 ); + ROS_INFO_NAMED("tricycle_drive", "%s: Advertise odom on %s ", gazebo_ros_->info(), odometry_topic_.c_str() ); + + // start custom queue for diff drive + this->callback_queue_thread_ = boost::thread ( boost::bind ( &GazeboRosTricycleDrive::QueueThread, this ) ); + + // listen to the update event (broadcast every simulation iteration) + this->update_connection_ = event::Events::ConnectWorldUpdateBegin ( boost::bind ( &GazeboRosTricycleDrive::UpdateChild, this ) ); + +} + +void GazeboRosTricycleDrive::publishWheelJointState() +{ + std::vector joints; + joints.push_back ( joint_steering_ ); + joints.push_back ( joint_wheel_actuated_ ); + joints.push_back ( joint_wheel_encoder_left_ ); + joints.push_back ( joint_wheel_encoder_right_ ); + + ros::Time current_time = ros::Time::now(); + joint_state_.header.stamp = current_time; + joint_state_.name.resize ( joints.size() ); + joint_state_.position.resize ( joints.size() ); + joint_state_.velocity.resize ( joints.size() ); + joint_state_.effort.resize ( joints.size() ); + for ( std::size_t i = 0; i < joints.size(); i++ ) { + joint_state_.name[i] = joints[i]->GetName(); +#if GAZEBO_MAJOR_VERSION >= 8 + joint_state_.position[i] = joints[i]->Position ( 0 ); +#else + joint_state_.position[i] = joints[i]->GetAngle ( 0 ).Radian(); +#endif + joint_state_.velocity[i] = joints[i]->GetVelocity ( 0 ); + joint_state_.effort[i] = joints[i]->GetForce ( 0 ); + } + joint_state_publisher_.publish ( joint_state_ ); +} + +void GazeboRosTricycleDrive::publishWheelTF() +{ + ros::Time current_time = ros::Time::now(); + std::vector joints; + joints.push_back ( joint_steering_ ); + joints.push_back ( joint_wheel_actuated_ ); + joints.push_back ( joint_wheel_encoder_left_ ); + joints.push_back ( joint_wheel_encoder_right_ ); + for ( std::size_t i = 0; i < joints.size(); i++ ) { + std::string frame = gazebo_ros_->resolveTF ( joints[i]->GetName() ); + std::string parent_frame = gazebo_ros_->resolveTF ( joints[i]->GetParent()->GetName() ); + +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d pose = joints[i]->GetChild()->RelativePose(); +#else + ignition::math::Pose3d pose = joints[i]->GetChild()->GetRelativePose().Ign(); +#endif + + tf::Quaternion qt ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() ); + tf::Vector3 vt ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() ); + + tf::Transform transform ( qt, vt ); + transform_broadcaster_->sendTransform ( tf::StampedTransform ( transform, current_time, parent_frame, frame ) ); + } + +} +// Update the controller +void GazeboRosTricycleDrive::UpdateChild() +{ + if ( odom_source_ == ENCODER ) UpdateOdometryEncoder(); +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time current_time = parent->GetWorld()->SimTime(); +#else + common::Time current_time = parent->GetWorld()->GetSimTime(); +#endif + double seconds_since_last_update = ( current_time - last_actuator_update_ ).Double(); + if ( seconds_since_last_update > update_period_ ) { + + publishOdometry ( seconds_since_last_update ); + if ( publishWheelTF_ ) publishWheelTF(); + if ( publishWheelJointState_ ) publishWheelJointState(); + + double target_wheel_roation_speed = cmd_.speed / ( diameter_actuated_wheel_ / 2.0 ); + double target_steering_angle = cmd_.angle; + + motorController ( target_wheel_roation_speed, target_steering_angle, seconds_since_last_update ); + + //ROS_INFO_NAMED("tricycle_drive", "v = %f, w = %f ", target_wheel_roation_speed, target_steering_angle); + + last_actuator_update_ += common::Time ( update_period_ ); + } +} + + +void GazeboRosTricycleDrive::motorController ( double target_speed, double target_angle, double dt ) +{ + double applied_speed = target_speed; + double applied_angle = target_angle; + + double current_speed = joint_wheel_actuated_->GetVelocity ( 0 ); + if (wheel_acceleration_ > 0) + { + double diff_speed = current_speed - target_speed; + if ( fabs ( diff_speed ) < wheel_speed_tolerance_ ) + { + applied_speed = current_speed; + } + else if ( fabs(diff_speed) > wheel_acceleration_ * dt ) + { + if(diff_speed > 0) + { + applied_speed = current_speed - wheel_acceleration_ * dt; + } + else + { + applied_speed = current_speed + wheel_deceleration_ * dt; + } + } + } + joint_wheel_actuated_->SetParam ( "vel", 0, applied_speed ); + +#if GAZEBO_MAJOR_VERSION >= 8 + double current_angle = joint_steering_->Position ( 0 ); +#else + double current_angle = joint_steering_->GetAngle ( 0 ).Radian(); +#endif + + // truncate target angle + if (target_angle > +M_PI / 2.0) + { + target_angle = +M_PI / 2.0; + } + else if (target_angle < -M_PI / 2.0) + { + target_angle = -M_PI / 2.0; + } + + // if steering_speed_ is > 0, use speed control, otherwise use position control + // With position control, one cannot expect dynamics to work correctly. + double diff_angle = current_angle - target_angle; + if ( steering_speed_ > 0 ) { + // this means we will steer using steering speed + double applied_steering_speed = 0; + if (fabs(diff_angle) < steering_angle_tolerance_ ) { + // we're withing angle tolerance + applied_steering_speed = 0; + } else if ( diff_angle < target_speed ) { + // steer toward target angle + applied_steering_speed = steering_speed_; + } else { + // steer toward target angle + applied_steering_speed = -steering_speed_; + } + + // use speed control, not recommended, for better dynamics use force control + joint_steering_->SetParam ( "vel", 0, applied_steering_speed ); + } + else { + // steering_speed_ is zero, use position control. + // This is not a good idea if we want dynamics to work. + if (fabs(diff_angle) < steering_speed_ * dt) + { + // we can take a step and still not overshoot target + if(diff_angle > 0) + { + applied_angle = current_angle - steering_speed_ * dt; + } + else + { + applied_angle = current_angle + steering_speed_ * dt; + } + } + else + { + applied_angle = target_angle; + } +#if GAZEBO_MAJOR_VERSION >= 9 + joint_steering_->SetPosition(0, applied_angle, true); +#else + ROS_WARN_ONCE("The tricycle_drive plugin is using the Joint::SetPosition method without preserving the link velocity."); + ROS_WARN_ONCE("As a result, gravity will not be simulated correctly for your model."); + ROS_WARN_ONCE("Please upgrade to Gazebo 9."); + ROS_WARN_ONCE("For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612"); + joint_steering_->SetPosition(0, applied_angle); +#endif + } + //ROS_INFO_NAMED("tricycle_drive", "target: [%3.2f, %3.2f], current: [%3.2f, %3.2f], applied: [%3.2f, %3.2f/%3.2f] ", + // target_speed, target_angle, current_speed, current_angle, applied_speed, applied_angle, applied_steering_speed ); +} + +// Finalize the controller +void GazeboRosTricycleDrive::FiniChild() +{ + alive_ = false; + queue_.clear(); + queue_.disable(); + gazebo_ros_->node()->shutdown(); + callback_queue_thread_.join(); +} + +void GazeboRosTricycleDrive::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr& cmd_msg ) +{ + boost::mutex::scoped_lock scoped_lock ( lock ); + cmd_.speed = cmd_msg->linear.x; + cmd_.angle = cmd_msg->angular.z; +} + +void GazeboRosTricycleDrive::QueueThread() +{ + static const double timeout = 0.01; + + while ( alive_ && gazebo_ros_->node()->ok() ) { + queue_.callAvailable ( ros::WallDuration ( timeout ) ); + } +} + +void GazeboRosTricycleDrive::UpdateOdometryEncoder() +{ + double vl = joint_wheel_encoder_left_->GetVelocity ( 0 ); + double vr = joint_wheel_encoder_right_->GetVelocity ( 0 ); +#if GAZEBO_MAJOR_VERSION >= 8 + common::Time current_time = parent->GetWorld()->SimTime(); +#else + common::Time current_time = parent->GetWorld()->GetSimTime(); +#endif + double seconds_since_last_update = ( current_time - last_odom_update_ ).Double(); + last_odom_update_ = current_time; + + double b = separation_encoder_wheel_; + + // Book: Sigwart 2011 Autonompus Mobile Robots page:337 + double sl = vl * ( diameter_encoder_wheel_ / 2.0 ) * seconds_since_last_update; + double sr = vr * ( diameter_encoder_wheel_ / 2.0 ) * seconds_since_last_update; + double theta = ( sl - sr ) /b; + + + double dx = ( sl + sr ) /2.0 * cos ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) ); + double dy = ( sl + sr ) /2.0 * sin ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) ); + double dtheta = ( sl - sr ) /b; + + pose_encoder_.x += dx; + pose_encoder_.y += dy; + pose_encoder_.theta += dtheta; + + double w = dtheta/seconds_since_last_update; + double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update; + + tf::Quaternion qt; + tf::Vector3 vt; + qt.setRPY ( 0,0,pose_encoder_.theta ); + vt = tf::Vector3 ( pose_encoder_.x, pose_encoder_.y, 0 ); + + odom_.pose.pose.position.x = vt.x(); + odom_.pose.pose.position.y = vt.y(); + odom_.pose.pose.position.z = vt.z(); + + odom_.pose.pose.orientation.x = qt.x(); + odom_.pose.pose.orientation.y = qt.y(); + odom_.pose.pose.orientation.z = qt.z(); + odom_.pose.pose.orientation.w = qt.w(); + + odom_.twist.twist.angular.z = w; + odom_.twist.twist.linear.x = dx/seconds_since_last_update; + odom_.twist.twist.linear.y = dy/seconds_since_last_update; +} + +void GazeboRosTricycleDrive::publishOdometry ( double step_time ) +{ + + ros::Time current_time = ros::Time::now(); + std::string odom_frame = gazebo_ros_->resolveTF ( odometry_frame_ ); + std::string base_footprint_frame = gazebo_ros_->resolveTF ( robot_base_frame_ ); + + tf::Quaternion qt; + tf::Vector3 vt; + + if ( odom_source_ == ENCODER ) { + // getting data form encoder integration + qt = tf::Quaternion ( odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w ); + vt = tf::Vector3 ( odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z ); + + } + if ( odom_source_ == WORLD ) { + // getting data form gazebo world +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d pose = parent->WorldPose(); +#else + ignition::math::Pose3d pose = parent->GetWorldPose().Ign(); +#endif + qt = tf::Quaternion ( pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z(), pose.Rot().W() ); + vt = tf::Vector3 ( pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z() ); + + odom_.pose.pose.position.x = vt.x(); + odom_.pose.pose.position.y = vt.y(); + odom_.pose.pose.position.z = vt.z(); + + odom_.pose.pose.orientation.x = qt.x(); + odom_.pose.pose.orientation.y = qt.y(); + odom_.pose.pose.orientation.z = qt.z(); + odom_.pose.pose.orientation.w = qt.w(); + + // get velocity in /odom frame + ignition::math::Vector3d linear; +#if GAZEBO_MAJOR_VERSION >= 8 + linear = parent->WorldLinearVel(); + odom_.twist.twist.angular.z = parent->WorldAngularVel().Z(); +#else + linear = parent->GetWorldLinearVel().Ign(); + odom_.twist.twist.angular.z = parent->GetWorldAngularVel().Ign().Z(); +#endif + + // convert velocity to child_frame_id (aka base_footprint) + float yaw = pose.Rot().Yaw(); + odom_.twist.twist.linear.x = cosf ( yaw ) * linear.X() + sinf ( yaw ) * linear.Y(); + odom_.twist.twist.linear.y = cosf ( yaw ) * linear.Y() - sinf ( yaw ) * linear.X(); + } + + tf::Transform base_footprint_to_odom ( qt, vt ); + transform_broadcaster_->sendTransform ( + tf::StampedTransform ( base_footprint_to_odom, current_time, + odom_frame, base_footprint_frame ) ); + + + // set covariance + odom_.pose.covariance[0] = 0.00001; + odom_.pose.covariance[7] = 0.00001; + odom_.pose.covariance[14] = 1000000000000.0; + odom_.pose.covariance[21] = 1000000000000.0; + odom_.pose.covariance[28] = 1000000000000.0; + odom_.pose.covariance[35] = 0.001; + + + // set header + odom_.header.stamp = current_time; + odom_.header.frame_id = odom_frame; + odom_.child_frame_id = base_footprint_frame; + + odometry_publisher_.publish ( odom_ ); +} + +GZ_REGISTER_MODEL_PLUGIN ( GazeboRosTricycleDrive ) +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp new file mode 100755 index 0000000..7ea48d0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_triggered_camera.cpp @@ -0,0 +1,133 @@ +/* + * Copyright 2017 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 "gazebo_plugins/gazebo_ros_triggered_camera.h" + +#include +#include + +#include +#include +#include + +namespace gazebo +{ + +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosTriggeredCamera) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosTriggeredCamera::GazeboRosTriggeredCamera() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosTriggeredCamera::~GazeboRosTriggeredCamera() +{ + ROS_DEBUG_STREAM_NAMED("camera","Unloaded"); +} + +void GazeboRosTriggeredCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf) +{ + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + CameraPlugin::Load(_parent, _sdf); + // copying from CameraPlugin into GazeboRosTriggeredCameraUtils + this->parentSensor_ = this->parentSensor; + this->width_ = this->width; + this->height_ = this->height; + this->depth_ = this->depth; + this->format_ = this->format; + this->camera_ = this->camera; + + GazeboRosCameraUtils::Load(_parent, _sdf); + + this->SetCameraEnabled(false); + this->preRenderConnection_ = + event::Events::ConnectPreRender( + std::bind(&GazeboRosTriggeredCamera::PreRender, this)); +} + +void GazeboRosTriggeredCamera::Load(sensors::SensorPtr _parent, + sdf::ElementPtr _sdf, + const std::string &_camera_name_suffix, + double _hack_baseline) +{ + GazeboRosCameraUtils::Load(_parent, _sdf, _camera_name_suffix, _hack_baseline); + + this->SetCameraEnabled(false); + this->preRenderConnection_ = + event::Events::ConnectPreRender( + std::bind(&GazeboRosTriggeredCamera::PreRender, this)); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosTriggeredCamera::OnNewFrame(const unsigned char *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ + this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime(); + + if ((*this->image_connect_count_) > 0) + { + this->PutCameraData(_image); + this->PublishCameraInfo(); + } + this->SetCameraEnabled(false); + + std::lock_guard lock(this->mutex); + this->triggered = std::max(this->triggered-1, 0); +} + +void GazeboRosTriggeredCamera::TriggerCamera() +{ + std::lock_guard lock(this->mutex); + if (!this->parentSensor_) + return; + this->triggered++; +} + +bool GazeboRosTriggeredCamera::CanTriggerCamera() +{ + return true; +} + +void GazeboRosTriggeredCamera::PreRender() +{ + std::lock_guard lock(this->mutex); + if (this->triggered > 0) + { + this->SetCameraEnabled(true); + } +} + +void GazeboRosTriggeredCamera::SetCameraEnabled(const bool _enabled) +{ + this->parentSensor_->SetActive(_enabled); + this->parentSensor_->SetUpdateRate(_enabled ? 0.0 : DBL_MIN); +} + +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_triggered_multicamera.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_triggered_multicamera.cpp new file mode 100755 index 0000000..11cbff5 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_triggered_multicamera.cpp @@ -0,0 +1,110 @@ +/* + * Copyright 2017 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 +#include + +#include "gazebo_plugins/gazebo_ros_triggered_multicamera.h" + +namespace gazebo +{ +// Register this plugin with the simulator +GZ_REGISTER_SENSOR_PLUGIN(GazeboRosTriggeredMultiCamera) + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosTriggeredMultiCamera::GazeboRosTriggeredMultiCamera() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosTriggeredMultiCamera::~GazeboRosTriggeredMultiCamera() +{ +} + +void GazeboRosTriggeredMultiCamera::Load(sensors::SensorPtr _parent, + sdf::ElementPtr _sdf) +{ + MultiCameraPlugin::Load(_parent, _sdf); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + // initialize shared_ptr members + this->image_connect_count_ = boost::shared_ptr(new int(0)); + this->image_connect_count_lock_ = boost::shared_ptr(new boost::mutex); + this->was_active_ = boost::shared_ptr(new bool(false)); + + // copying from CameraPlugin into GazeboRosCameraUtils + for (unsigned i = 0; i < this->camera.size(); ++i) + { + GazeboRosTriggeredCamera * cam = new GazeboRosTriggeredCamera(); + cam->parentSensor_ = this->parentSensor; + cam->width_ = this->width[i]; + cam->height_ = this->height[i]; + cam->depth_ = this->depth[i]; + cam->format_ = this->format[i]; + cam->camera_ = this->camera[i]; + // Set up a shared connection counter + cam->image_connect_count_ = this->image_connect_count_; + cam->image_connect_count_lock_ = this->image_connect_count_lock_; + cam->was_active_ = this->was_active_; + if (this->camera[i]->Name().find("left") != std::string::npos) + { + // FIXME: hardcoded, left hack_baseline_ 0 + cam->Load(_parent, _sdf, "/left", 0.0); + } + else if (this->camera[i]->Name().find("right") != std::string::npos) + { + double hackBaseline = 0.0; + if (_sdf->HasElement("hackBaseline")) + hackBaseline = _sdf->Get("hackBaseline"); + cam->Load(_parent, _sdf, "/right", hackBaseline); + } + this->triggered_cameras.push_back(cam); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosTriggeredMultiCamera::OnNewFrameLeft(const unsigned char *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ + GazeboRosTriggeredCamera * cam = this->triggered_cameras[0]; + cam->OnNewFrame(_image, _width, _height, _depth, _format); +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosTriggeredMultiCamera::OnNewFrameRight(const unsigned char *_image, + unsigned int _width, unsigned int _height, unsigned int _depth, + const std::string &_format) +{ + GazeboRosTriggeredCamera * cam = this->triggered_cameras[1]; + cam->OnNewFrame(_image, _width, _height, _depth, _format); +} +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_utils.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_utils.cpp new file mode 100755 index 0000000..15e2741 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_utils.cpp @@ -0,0 +1,152 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Markus Bader + * All rights reserved. + * + * 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 the copyright holder 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. + *********************************************************************/ + + +#include + +#include +#include + +using namespace gazebo; + +const char* GazeboRos::info() const { + return info_text.c_str(); +} +boost::shared_ptr& GazeboRos ::node() { + return rosnode_; +} +const boost::shared_ptr& GazeboRos ::node() const { + return rosnode_; +} + +std::string GazeboRos ::resolveTF(const std::string &name) { + return tf::resolve(tf_prefix_, name); +} +void GazeboRos ::readCommonParameter() { + + ROS_INFO_NAMED("utils", "Starting plugin %s", info()); + + std::string debugLevel; + getParameter(debugLevel, "rosDebugLevel", "na"); + if(boost::iequals(debugLevel, std::string("Debug"))) { + if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug) ) { + ros::console::notifyLoggerLevelsChanged(); + } + } else if(boost::iequals(debugLevel, std::string("Info"))) { + if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info) ) { + ros::console::notifyLoggerLevelsChanged(); + } + } else if(boost::iequals(debugLevel, std::string("Warn"))) { + if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn) ) { + ros::console::notifyLoggerLevelsChanged(); + } + } else if(boost::iequals(debugLevel, std::string("Error"))) { + if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Error) ) { + ros::console::notifyLoggerLevelsChanged(); + } + } else if(boost::iequals(debugLevel, std::string("Fatal"))) { + if( ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Fatal) ) { + ros::console::notifyLoggerLevelsChanged(); + } + } + if (sdf_->HasElement("rosDebugLevel")) { + ROS_INFO_NAMED("utils", "%s: = %s", info(), debugLevel.c_str()); + } + + + tf_prefix_ = tf::getPrefixParam(*rosnode_); + if(tf_prefix_.empty()) + { + tf_prefix_ = namespace_; + boost::trim_right_if(tf_prefix_,boost::is_any_of("/")); + } + ROS_INFO_NAMED("utils", "%s: = %s", info(), tf_prefix_.c_str()); +} + + +void GazeboRos ::getParameterBoolean(bool &_value, const char *_tag_name, const bool &_default) { + _value = _default; + if (!sdf_->HasElement(_tag_name)) { + ROS_WARN_NAMED("utils", "%s: missing <%s> default is %s", + info(), _tag_name, (_default?"ture":"false")); + } else { + getParameterBoolean(_value, _tag_name); + } + +} +void GazeboRos ::getParameterBoolean(bool &_value, const char *_tag_name) { + + if (sdf_->HasElement(_tag_name)) { + std::string value = sdf_->GetElement(_tag_name)->Get(); + if(boost::iequals(value, std::string("true"))) + { + _value = true; + } + else if(boost::iequals(value, std::string("false"))) + { + _value = false; + } + else + { + ROS_WARN_NAMED("utils", "%s: <%s> must be either true or false", + info(), _tag_name); + } + } + ROS_DEBUG_NAMED("utils", "%s: <%s> = %s", + info(), _tag_name, (_value?"ture":"false")); + +} + +physics::JointPtr GazeboRos::getJoint(physics::ModelPtr &_parent, const char *_tag_name, const std::string &_joint_default_name) { + std::string joint_name; + getParameter(joint_name, _tag_name, _joint_default_name); + physics::JointPtr joint = _parent->GetJoint(joint_name); + if (!joint) + { + char error[200]; + snprintf(error, 200, + "%s: couldn't get wheel hinge joint named %s", + info() , joint_name.c_str()); + gzthrow(error); + } + return joint; +} + +void GazeboRos::isInitialized() { + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM(info() << "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp new file mode 100755 index 0000000..fee4d38 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_vacuum_gripper.cpp @@ -0,0 +1,259 @@ +/* + * Copyright 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. + * +*/ + +/* + Desc: GazeboVacuumGripper plugin for manipulating objects in Gazebo + Author: Kentaro Wada + Date: 7 Dec 2015 + */ + +#include +#include + +#include +#include + + +namespace gazebo +{ +GZ_REGISTER_MODEL_PLUGIN(GazeboRosVacuumGripper); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GazeboRosVacuumGripper::GazeboRosVacuumGripper() +{ + connect_count_ = 0; + status_ = false; +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +GazeboRosVacuumGripper::~GazeboRosVacuumGripper() +{ + update_connection_.reset(); + + // Custom Callback Queue + queue_.clear(); + queue_.disable(); + rosnode_->shutdown(); + callback_queue_thread_.join(); + + delete rosnode_; +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void GazeboRosVacuumGripper::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +{ + ROS_INFO_NAMED("vacuum_gripper", "Loading gazebo_ros_vacuum_gripper"); + + // Set attached model; + parent_ = _model; + + // Get the world name. + world_ = _model->GetWorld(); + + // load parameters + robot_namespace_ = ""; + if (_sdf->HasElement("robotNamespace")) + robot_namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; + + if (!_sdf->HasElement("bodyName")) + { + ROS_FATAL_NAMED("vacuum_gripper", "vacuum_gripper plugin missing , cannot proceed"); + return; + } + else + link_name_ = _sdf->GetElement("bodyName")->Get(); + + link_ = _model->GetLink(link_name_); + if (!link_) + { + std::string found; + physics::Link_V links = _model->GetLinks(); + for (size_t i = 0; i < links.size(); i++) { + found += std::string(" ") + links[i]->GetName(); + } + ROS_FATAL_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper plugin error: link named: %s does not exist", link_name_.c_str()); + ROS_FATAL_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper plugin error: You should check it exists and is not connected with fixed joint"); + ROS_FATAL_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper plugin error: Found links are: %s", found.c_str()); + return; + } + + if (!_sdf->HasElement("topicName")) + { + ROS_FATAL_NAMED("vacuum_gripper", "vacuum_gripper plugin missing , cannot proceed"); + return; + } + else + topic_name_ = _sdf->GetElement("topicName")->Get(); + + // Make sure the ROS node for Gazebo has already been initialized + if (!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("vacuum_gripper", "A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + rosnode_ = new ros::NodeHandle(robot_namespace_); + + // Custom Callback Queue + ros::AdvertiseOptions ao = ros::AdvertiseOptions::create( + topic_name_, 1, + boost::bind(&GazeboRosVacuumGripper::Connect, this), + boost::bind(&GazeboRosVacuumGripper::Disconnect, this), + ros::VoidPtr(), &queue_); + pub_ = rosnode_->advertise(ao); + + // Custom Callback Queue + ros::AdvertiseServiceOptions aso1 = + ros::AdvertiseServiceOptions::create( + "on", boost::bind(&GazeboRosVacuumGripper::OnServiceCallback, + this, _1, _2), ros::VoidPtr(), &queue_); + srv1_ = rosnode_->advertiseService(aso1); + ros::AdvertiseServiceOptions aso2 = + ros::AdvertiseServiceOptions::create( + "off", boost::bind(&GazeboRosVacuumGripper::OffServiceCallback, + this, _1, _2), ros::VoidPtr(), &queue_); + srv2_ = rosnode_->advertiseService(aso2); + + // Custom Callback Queue + callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosVacuumGripper::QueueThread,this ) ); + + // New Mechanism for Updating every World Cycle + // Listen to the update event. This event is broadcast every + // simulation iteration. + update_connection_ = event::Events::ConnectWorldUpdateBegin( + boost::bind(&GazeboRosVacuumGripper::UpdateChild, this)); + + ROS_INFO_NAMED("vacuum_gripper", "Loaded gazebo_ros_vacuum_gripper"); +} + +bool GazeboRosVacuumGripper::OnServiceCallback(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res) +{ + if (status_) { + ROS_WARN_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: already status is 'on'"); + } else { + status_ = true; + ROS_INFO_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: status: off -> on"); + } + return true; +} +bool GazeboRosVacuumGripper::OffServiceCallback(std_srvs::Empty::Request &req, + std_srvs::Empty::Response &res) +{ + if (status_) { + status_ = false; + ROS_INFO_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: status: on -> off"); + } else { + ROS_WARN_NAMED("vacuum_gripper", "gazebo_ros_vacuum_gripper: already status is 'off'"); + } + return true; +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void GazeboRosVacuumGripper::UpdateChild() +{ + std_msgs::Bool grasping_msg; + grasping_msg.data = false; + if (!status_) { + pub_.publish(grasping_msg); + return; + } + // apply force + lock_.lock(); +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d parent_pose = link_->WorldPose(); + physics::Model_V models = world_->Models(); +#else + ignition::math::Pose3d parent_pose = link_->GetWorldPose().Ign(); + physics::Model_V models = world_->GetModels(); +#endif + for (size_t i = 0; i < models.size(); i++) { + if (models[i]->GetName() == link_->GetName() || + models[i]->GetName() == parent_->GetName()) + { + continue; + } + physics::Link_V links = models[i]->GetLinks(); + for (size_t j = 0; j < links.size(); j++) { +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d link_pose = links[j]->WorldPose(); +#else + ignition::math::Pose3d link_pose = links[j]->GetWorldPose().Ign(); +#endif + ignition::math::Pose3d diff = parent_pose - link_pose; + double norm = diff.Pos().Length(); + if (norm < 0.05) { +#if GAZEBO_MAJOR_VERSION >= 8 + links[j]->SetLinearVel(link_->WorldLinearVel()); + links[j]->SetAngularVel(link_->WorldAngularVel()); +#else + links[j]->SetLinearVel(link_->GetWorldLinearVel()); + links[j]->SetAngularVel(link_->GetWorldAngularVel()); +#endif + double norm_force = 1 / norm; + if (norm < 0.01) { + // apply friction like force + // TODO(unknown): should apply friction actually + link_pose.Set(parent_pose.Pos(), link_pose.Rot()); + links[j]->SetWorldPose(link_pose); + } + if (norm_force > 20) { + norm_force = 20; // max_force + } + ignition::math::Vector3d force = norm_force * diff.Pos().Normalize(); + links[j]->AddForce(force); + grasping_msg.data = true; + } + } + } + pub_.publish(grasping_msg); + lock_.unlock(); +} + +// Custom Callback Queue +//////////////////////////////////////////////////////////////////////////////// +// custom callback queue thread +void GazeboRosVacuumGripper::QueueThread() +{ + static const double timeout = 0.01; + + while (rosnode_->ok()) + { + queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +//////////////////////////////////////////////////////////////////////////////// +// Someone subscribes to me +void GazeboRosVacuumGripper::Connect() +{ + this->connect_count_++; +} + +//////////////////////////////////////////////////////////////////////////////// +// Someone subscribes to me +void GazeboRosVacuumGripper::Disconnect() +{ + this->connect_count_--; +} + +} // namespace gazebo diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_video.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_video.cpp new file mode 100755 index 0000000..4e49711 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_video.cpp @@ -0,0 +1,254 @@ +/* + * 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: Video plugin for displaying ROS image topics on Ogre textures + * Author: Piyush Khandelwal + * Date: 26 July 2013 + */ + +#include +#include + +namespace gazebo +{ + + VideoVisual::VideoVisual( + const std::string &name, rendering::VisualPtr parent, + int height, int width) : + rendering::Visual(name, parent), height_(height), width_(width) + { + + texture_ = Ogre::TextureManager::getSingleton().createManual( + name + "__VideoTexture__", + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, + width_, height_, + 0, + Ogre::PF_BYTE_BGRA, + Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE); + + Ogre::MaterialPtr material = + Ogre::MaterialManager::getSingleton().create( + name + "__VideoMaterial__", "General"); + material->getTechnique(0)->getPass(0)->createTextureUnitState( + name + "__VideoTexture__"); + material->setReceiveShadows(false); + + double factor = 1.0; + + Ogre::ManualObject mo(name + "__VideoObject__"); + mo.begin(name + "__VideoMaterial__", + Ogre::RenderOperation::OT_TRIANGLE_LIST); + + mo.position(-factor / 2, factor / 2, 0.51); + mo.textureCoord(0, 0); + + mo.position(factor / 2, factor / 2, 0.51); + mo.textureCoord(1, 0); + + mo.position(factor / 2, -factor / 2, 0.51); + mo.textureCoord(1, 1); + + mo.position(-factor / 2, -factor / 2, 0.51); + mo.textureCoord(0, 1); + + mo.triangle(0, 3, 2); + mo.triangle(2, 1, 0); + mo.end(); + + mo.convertToMesh(name + "__VideoMesh__"); + + Ogre::MovableObject *obj = (Ogre::MovableObject*) + GetSceneNode()->getCreator()->createEntity( + name + "__VideoEntity__", + name + "__VideoMesh__"); + obj->setCastShadows(false); + AttachObject(obj); + } + + VideoVisual::~VideoVisual() {} + + void VideoVisual::render(const cv::Mat& image) + { + + // Fix image size if necessary + const cv::Mat* image_ptr = ℑ + cv::Mat converted_image; + if (image_ptr->rows != height_ || image_ptr->cols != width_) + { + cv::resize(*image_ptr, converted_image, cv::Size(width_, height_)); + image_ptr = &converted_image; + } + + // Get the pixel buffer + Ogre::HardwarePixelBufferSharedPtr pixelBuffer = + texture_->getBuffer(); + + // Lock the pixel buffer and get a pixel box + pixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD); + const Ogre::PixelBox& pixelBox = pixelBuffer->getCurrentLock(); + uint8_t* pDest = static_cast(pixelBox.data); + + memcpy(pDest, image_ptr->data, height_ * width_ * 4); + + // Unlock the pixel buffer + pixelBuffer->unlock(); + } + + // Constructor + GazeboRosVideo::GazeboRosVideo() {} + + // Destructor + GazeboRosVideo::~GazeboRosVideo() { + update_connection_.reset(); + + // Custom Callback Queue + queue_.clear(); + queue_.disable(); + rosnode_->shutdown(); + callback_queue_thread_.join(); + + delete rosnode_; + } + + // Load the controller + void GazeboRosVideo::Load( + rendering::VisualPtr parent, sdf::ElementPtr sdf) + { + + model_ = parent; + sdf::ElementPtr p_sdf; + if (sdf->HasElement("sdf")) + { + p_sdf = sdf->GetElement("sdf"); + } + else + { + p_sdf = sdf; + } + + robot_namespace_ = ""; + if (!p_sdf->HasElement("robotNamespace")) + { + ROS_WARN_NAMED("video", "GazeboRosVideo plugin missing , " + "defaults to \"%s\".", robot_namespace_.c_str()); + } + else + { + robot_namespace_ = + p_sdf->GetElement("robotNamespace")->Get(); + } + + topic_name_ = "image_raw"; + if (!p_sdf->HasElement("topicName")) + { + ROS_WARN_NAMED("video", "GazeboRosVideo Plugin (ns = %s) missing , " + "defaults to \"%s\".", robot_namespace_.c_str(), topic_name_.c_str()); + } + else + { + topic_name_ = p_sdf->GetElement("topicName")->Get(); + } + + int height = 240; + if (!p_sdf->HasElement("height")) { + ROS_WARN_NAMED("video", "GazeboRosVideo Plugin (ns = %s) missing , " + "defaults to %i.", robot_namespace_.c_str(), height); + } + else + { + height = p_sdf->GetElement("height")->Get(); + } + + int width = 320; + if (!p_sdf->HasElement("width")) { + ROS_WARN_NAMED("video", "GazeboRosVideo Plugin (ns = %s) missing , " + "defaults to %i", robot_namespace_.c_str(), width); + } + else + { + width = p_sdf->GetElement("width")->Get(); + } + + std::string name = robot_namespace_ + "_visual"; + video_visual_.reset( + new VideoVisual(name, parent, height, width)); + + // Initialize the ROS node for the gazebo client if necessary + if (!ros::isInitialized()) + { + int argc = 0; + char** argv = NULL; + ros::init(argc, argv, "gazebo_client", + ros::init_options::NoSigintHandler); + } + std::string gazebo_source = + (ros::this_node::getName() == "/gazebo_client") ? "gzclient" : "gzserver"; + rosnode_ = new ros::NodeHandle(robot_namespace_); + + // Subscribe to the image topic + ros::SubscribeOptions so = + ros::SubscribeOptions::create(topic_name_, 1, + boost::bind(&GazeboRosVideo::processImage, this, _1), + ros::VoidPtr(), &queue_); + camera_subscriber_ = rosnode_->subscribe(so); + + new_image_available_ = false; + + callback_queue_thread_ = + boost::thread(boost::bind(&GazeboRosVideo::QueueThread, this)); + + update_connection_ = + event::Events::ConnectPreRender( + boost::bind(&GazeboRosVideo::UpdateChild, this)); + + ROS_INFO_NAMED("video", "GazeboRosVideo (%s, ns = %s) has started", + gazebo_source.c_str(), robot_namespace_.c_str()); + } + + // Update the controller + void GazeboRosVideo::UpdateChild() + { + boost::mutex::scoped_lock scoped_lock(m_image_); + if (new_image_available_) + { + video_visual_->render(image_->image); + } + new_image_available_ = false; + } + + void GazeboRosVideo::processImage(const sensor_msgs::ImageConstPtr &msg) + { + // Get a reference to the image from the image message pointer + boost::mutex::scoped_lock scoped_lock(m_image_); + // We get image with alpha channel as it allows memcpy onto ogre texture + image_ = cv_bridge::toCvCopy(msg, "bgra8"); + new_image_available_ = true; + } + + void GazeboRosVideo::QueueThread() + { + static const double timeout = 0.01; + while (rosnode_->ok()) + { + queue_.callAvailable(ros::WallDuration(timeout)); + } + } + + GZ_REGISTER_VISUAL_PLUGIN(GazeboRosVideo); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/hokuyo_node.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/hokuyo_node.cpp new file mode 100755 index 0000000..c4b09f9 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/hokuyo_node.cpp @@ -0,0 +1,60 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009-2010, Willow Garage, Inc. +* All rights reserved. +* +* 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 the Willow Garage 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. +*********************************************************************/ + +#include +#include + +void callback(gazebo_plugins::HokuyoConfig &config, uint32_t level) +{ + ROS_INFO_NAMED("hokuyo_node", "Reconfigure request : %f %f %i %i %i %s %i %s %f %i", + config.min_ang, config.max_ang, (int)config.intensity, config.cluster, config.skip, + config.port.c_str(), (int)config.calibrate_time, config.frame_id.c_str(), config.time_offset, (int)config.allow_unsafe_settings); + + // do nothing for now + + ROS_INFO_NAMED("hokuyo_node", "Reconfigure to : %f %f %i %i %i %s %i %s %f %i", + config.min_ang, config.max_ang, (int)config.intensity, config.cluster, config.skip, + config.port.c_str(), (int)config.calibrate_time, config.frame_id.c_str(), config.time_offset, (int)config.allow_unsafe_settings); +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "hokuyo_node"); + dynamic_reconfigure::Server srv; + dynamic_reconfigure::Server::CallbackType f = boost::bind(&callback, _1, _2); + srv.setCallback(f); + ROS_INFO_NAMED("hokuyo_node", "Starting to spin..."); + ros::spin(); + return 0; +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/src/vision_reconfigure.cpp b/gazebo_ros_pkgs/gazebo_plugins/src/vision_reconfigure.cpp new file mode 100755 index 0000000..9934e72 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/src/vision_reconfigure.cpp @@ -0,0 +1,144 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009-2010, Willow Garage, Inc. +* All rights reserved. +* +* 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 the Willow Garage 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. +*********************************************************************/ + +#include + +VisionReconfigure::VisionReconfigure() : nh_("") +{ + this->nh_.setCallbackQueue(&this->queue_); + + // Custom Callback Queue + this->callback_queue_thread_ = boost::thread( boost::bind( &VisionReconfigure::QueueThread,this ) ); + + // this code needs to be rewritten + // for now, it publishes on pub_projector_ which is used by gazebo_ros_projector plugin directly + // and it publishes pub_header_, which is published by projector_controller in ethercat_trigger_controllers package in real life + this->pub_projector_ = this->nh_.advertise("/projector_wg6802418_controller/projector", 1,true); // publish latched for sim + this->pub_header_ = this->nh_.advertise("/projector_controller/rising_edge_timestamps", 1,true); // publish latched for sim + dynamic_reconfigure::Server::CallbackType f = boost::bind(&VisionReconfigure::ReconfigureCallback, this, _1, _2); + this->srv_.setCallback(f); + + + // initialize from relevant params on server + gazebo_plugins::CameraSynchronizerConfig config; + this->nh_.getParam("/camera_synchronizer_node/projector_mode",config.projector_mode); + this->nh_.getParam("/camera_synchronizer_node/forearm_l_trig_mode",config.forearm_l_trig_mode); + this->nh_.getParam("/camera_synchronizer_node/forearm_r_trig_mode",config.forearm_r_trig_mode); + this->nh_.getParam("/camera_synchronizer_node/narrow_stereo_trig_mode",config.narrow_stereo_trig_mode); + this->nh_.getParam("/camera_synchronizer_node/wide_stereo_trig_mode",config.wide_stereo_trig_mode); + this->ReconfigureCallback(config,0); + +} + +VisionReconfigure::~VisionReconfigure() +{ + this->nh_.shutdown(); + this->callback_queue_thread_.join(); +} + +void VisionReconfigure::ReconfigureCallback(gazebo_plugins::CameraSynchronizerConfig &config, uint32_t level) +{ + + // turn on or off projector in gazebo plugin + if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorOff) + { + this->projector_msg_.data = 0; + } + else if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorAuto) + { + // turn on or off projector based on narrow stereo trigger mode + if (config.wide_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector || + config.narrow_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector || + config.forearm_r_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector || + config.forearm_l_trig_mode == gazebo_plugins::CameraSynchronizer_WithProjector) + { + this->projector_msg_.data = 1; + } + else if (config.wide_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector || + config.narrow_stereo_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector || + config.forearm_r_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector || + config.forearm_l_trig_mode == gazebo_plugins::CameraSynchronizer_AlternateProjector) + { + ROS_WARN_NAMED("vision_reconfigure", "Alternate Projector Mode not supported in simulation, setting projector to on for now"); + this->projector_msg_.data = 1; + } + else + { + ROS_DEBUG_NAMED("vision_reconfigure", "Projector only supported for modes: WithProjector and AlternateProjector"); + this->projector_msg_.data = 0; + } + } + else if (config.projector_mode == gazebo_plugins::CameraSynchronizer_ProjectorOn) + { + this->projector_msg_.data = 1; + } + else + { + ROS_ERROR_NAMED("vision_reconfigure", "projector_mode is not in any recognized state [%d]",config.projector_mode); + } + + this->pub_projector_.publish(projector_msg_); +} + +void VisionReconfigure::QueueThread() +{ + // FIXME: hardcoded to 100Hz update rate for ros callback queue + static const double timeout = 0.01; + while (this->nh_.ok()) + { + this->queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +void VisionReconfigure::spinOnce() +{ + if (projector_msg_.data == 1) + { + std_msgs::Header rh; + rh.stamp = ros::Time::now(); + rh.frame_id = "projector_wg6802418_frame"; + this->pub_header_.publish(rh); + } +} + +void VisionReconfigure::spin(double spin_frequency) +{ + ros::Rate loop_rate(spin_frequency); + while(this->nh_.ok()) + { + ros::spinOnce(); + this->spinOnce(); + loop_rate.sleep(); + } +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/bumper_test/gazebo_ros_bumper.world b/gazebo_ros_pkgs/gazebo_plugins/test/bumper_test/gazebo_ros_bumper.world new file mode 100755 index 0000000..2c60189 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/bumper_test/gazebo_ros_bumper.world @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + true + 100.0 + world + test_bumper + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/bumper_test/test_bumper.launch b/gazebo_ros_pkgs/gazebo_plugins/test/bumper_test/test_bumper.launch new file mode 100755 index 0000000..83808d4 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/bumper_test/test_bumper.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/bumper_test/test_bumper.py b/gazebo_ros_pkgs/gazebo_plugins/test/bumper_test/test_bumper.py new file mode 100755 index 0000000..00a5915 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/bumper_test/test_bumper.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# 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 the Willow Garage 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. +# + +## Gazebo test bumper for a box + +PKG = 'gazebo_plugins' +NAME = 'test_bumper' + +import math +import roslib +roslib.load_manifest(PKG) + +import sys, unittest +import os, os.path, threading, time +import rospy, rostest +from nav_msgs.msg import * +from geometry_msgs.msg import Pose +from geometry_msgs.msg import Twist +from geometry_msgs.msg import Wrench +from geometry_msgs.msg import Vector3 +from gazebo_msgs.msg import ContactsState +from gazebo_msgs.msg import ContactState + +class BumperTest(unittest.TestCase): + def __init__(self, *args): + super(BumperTest, self).__init__(*args) + self.success = False + + self.sample_count = 0 + + self.min_samples_topic = "~min_samples" + self.min_samples = 1000 + + # in seconds + self.test_duration_topic = "~test_duration" + self.test_duration = 10.0 + + # test start time in seconds + self.test_start_time_topic = "~test_start_time" + self.test_start_time = 0.0 + + self.bumper_topic_name = "~bumper_topic_name" + self.bumper_topic = "/test_bumper" + self.bumper_state = ContactsState() + + self.fz_sum = 0 + self.fz_avg = 0 + + def bumperStateInput(self, contacts_state): + self.bumper_state = contacts_state + self.sample_count+=1 + self.fz_sum += contacts_state.states[0].total_wrench.force.z + self.fz_avg = self.fz_sum / self.sample_count + + def checkContact(self): + # see if total wrench is close to 98.1N + if self.sample_count > 20: + if abs(self.fz_avg - 98.1) < 0.01: + print "z force ",self.fz_avg + self.success = True + + + def test_bumper(self): + print "LNK\n" + rospy.init_node(NAME, anonymous=True) + self.bumper_topic = rospy.get_param(self.bumper_topic_name,self.bumper_topic); + self.min_samples = rospy.get_param(self.min_samples_topic,self.min_samples); + self.test_duration = rospy.get_param(self.test_duration_topic,self.test_duration); + self.test_start_time = rospy.get_param(self.test_start_time_topic,self.test_start_time); + + while not rospy.is_shutdown() and time.time() < self.test_start_time: + rospy.stdinfo("Waiting for test to start at time [%s]"% self.test_start_time) + time.sleep(0.1) + + print "subscribe" + rospy.Subscriber(self.bumper_topic, ContactsState, self.bumperStateInput) + + start_t = time.time() + timeout_t = start_t + self.test_duration + while not rospy.is_shutdown() and not self.success and time.time() < timeout_t: + self.checkContact() + time.sleep(0.1) + self.assert_(self.success) + +if __name__ == '__main__': + print "Waiting for test to start at time " + rostest.run(PKG, sys.argv[0], BumperTest, sys.argv) #, text_mode=True) + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.cpp b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.cpp new file mode 100755 index 0000000..92d6078 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.cpp @@ -0,0 +1,15 @@ +#include "camera.h" + +// Test if the camera image is published at all, and that the timestamp +// is not too long in the past. +TEST_F(CameraTest, cameraSubscribeTest) +{ + subscribeTest(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gazebo_camera_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.h b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.h new file mode 100755 index 0000000..a2f997b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.h @@ -0,0 +1,87 @@ +#ifndef GAZEBO_PLUGINS_TEST_CAMERA_CAMERA_H +#define GAZEBO_PLUGINS_TEST_CAMERA_CAMERA_H + +#include +#include +#include + +class CameraTest : public testing::Test +{ +protected: + virtual void SetUp() + { + has_new_image_ = false; + } + + ros::NodeHandle nh_; + image_transport::Subscriber cam_sub_; + bool has_new_image_; + ros::Time image_stamp_; +public: + void subscribeTest(); + + void imageCallback(const sensor_msgs::ImageConstPtr& msg) + { + image_stamp_ = msg->header.stamp; + has_new_image_ = true; + } +}; + +// Test if the camera image is published at all, and that the timestamp +// is not too long in the past. +void CameraTest::subscribeTest() +{ + image_transport::ImageTransport it(nh_); + cam_sub_ = it.subscribe("camera1/image_raw", 1, + &CameraTest::imageCallback, + dynamic_cast(this)); + +#if 0 + // wait for gazebo to start publishing + // TODO(lucasw) this isn't really necessary since this test + // is purely passive + bool wait_for_topic = true; + while (wait_for_topic) + { + // @todo this fails without the additional 0.5 second sleep after the + // publisher comes online, which means on a slower or more heavily + // loaded system it may take longer than 0.5 seconds, and the test + // would hang until the timeout is reached and fail. + if (cam_sub_.getNumPublishers() > 0) + wait_for_topic = false; + ros::Duration(0.5).sleep(); + } +#endif + + while (!has_new_image_) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + + // This check depends on the update period being much longer + // than the expected difference between now and the received image time + // TODO(lucasw) + // this likely isn't that robust - what if the testing system is really slow? + double time_diff = (ros::Time::now() - image_stamp_).toSec(); + ROS_INFO_STREAM(time_diff); + EXPECT_LT(time_diff, 1.0); + cam_sub_.shutdown(); + + // make sure nothing is subscribing to image_trigger topic + // there is no easy API, so call getSystemState + XmlRpc::XmlRpcValue args, result, payload; + args[0] = ros::this_node::getName(); + EXPECT_TRUE(ros::master::execute("getSystemState", args, result, payload, true)); + // [publishers, subscribers, services] + // subscribers in index 1 of payload + for (int i = 0; i < payload[1].size(); ++i) + { + // [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ] + // topic name i is in index 0 + std::string topic = payload[1][i][0]; + EXPECT_EQ(topic.find("image_trigger"), std::string::npos); + } +} + +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.test b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.test new file mode 100755 index 0000000..4de8fd0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.world b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.world new file mode 100755 index 0000000..895c60f --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera.world @@ -0,0 +1,138 @@ + + + + + + model://ground_plane + + + + + model://sun + + + + + + 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 + orbit + + + + + false + 2.0 0.0 4.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.5 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.5 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + + + + true + 0.0 0.0 0.5 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.5 + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + true + + 0.0 + camera1 + image_raw + camera_info + camera_link + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera16bit.cpp b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera16bit.cpp new file mode 100755 index 0000000..bea1b00 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera16bit.cpp @@ -0,0 +1,15 @@ +#include "camera.h" + +// Test if the camera image is published at all, and that the timestamp +// is not too long in the past. +TEST_F(CameraTest, camera16bitSubscribeTest) +{ + subscribeTest(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gazebo_camera_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera16bit.test b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera16bit.test new file mode 100755 index 0000000..ba9c0af --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera16bit.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera16bit.world b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera16bit.world new file mode 100755 index 0000000..a6c455a --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/camera16bit.world @@ -0,0 +1,138 @@ + + + + + + model://ground_plane + + + + + model://sun + + + + + + 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 + orbit + + + + + false + 2.0 0.0 4.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.5 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.5 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + + + + true + 0.0 0.0 0.5 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.5 + + 1.3962634 + + 640 + 480 + R16G16B16 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + true + + 0.0 + camera1 + image_raw + camera_info + camera_link + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.cpp b/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.cpp new file mode 100755 index 0000000..0c0ce12 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.cpp @@ -0,0 +1,128 @@ +#include +#include +#include +#include + +class DepthCameraTest : public testing::Test +{ +protected: + virtual void SetUp() + { + has_new_image_ = false; + has_new_depth_ = false; + has_new_points_ = false; + } + + ros::NodeHandle nh_; + image_transport::Subscriber cam_sub_; + image_transport::Subscriber depth_sub_; + ros::Subscriber points_sub_; + bool has_new_image_; + ros::Time image_stamp_; + bool has_new_depth_; + ros::Time depth_stamp_; + bool has_new_points_; + ros::Time points_stamp_; +public: + void imageCallback(const sensor_msgs::ImageConstPtr& msg) + { + image_stamp_ = msg->header.stamp; + has_new_image_ = true; + } + void depthCallback(const sensor_msgs::ImageConstPtr& msg) + { + depth_stamp_ = msg->header.stamp; + has_new_depth_ = true; + } + void pointsCallback(const sensor_msgs::PointCloud2ConstPtr& msg) + { + points_stamp_ = msg->header.stamp; + has_new_points_ = true; + } +}; + +// Test if the camera image is published at all, and that the timestamp +// is not too long in the past. +TEST_F(DepthCameraTest, cameraSubscribeTest) +{ + image_transport::ImageTransport it(nh_); + cam_sub_ = it.subscribe("camera1/image_raw", 1, + &DepthCameraTest::imageCallback, + dynamic_cast(this)); + depth_sub_ = it.subscribe("camera1/depth/image_raw", 1, + &DepthCameraTest::depthCallback, + dynamic_cast(this)); + points_sub_ = nh_.subscribe("camera1/points", 1, + &DepthCameraTest::pointsCallback, + dynamic_cast(this)); +#if 0 + // wait for gazebo to start publishing + // TODO(lucasw) this isn't really necessary since this test + // is purely passive + bool wait_for_topic = true; + while (wait_for_topic) + { + // @todo this fails without the additional 0.5 second sleep after the + // publisher comes online, which means on a slower or more heavily + // loaded system it may take longer than 0.5 seconds, and the test + // would hang until the timeout is reached and fail. + if (cam_sub_.getNumPublishers() > 0) + wait_for_topic = false; + ros::Duration(0.5).sleep(); + } +#endif + + while (!has_new_image_ || !has_new_depth_ || !has_new_points_) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + + EXPECT_EQ(depth_stamp_.toSec(), image_stamp_.toSec()); + EXPECT_EQ(depth_stamp_.toSec(), points_stamp_.toSec()); + // This check depends on the update period (currently 1.0/update_rate = 2.0 seconds) + // being much longer than the expected difference between now and the + // received image time. + const double max_time = 1.0; + const ros::Time current_time = ros::Time::now(); + // TODO(lucasw) + // this likely isn't that robust - what if the testing system is really slow? + double time_diff; + time_diff = (current_time - image_stamp_).toSec(); + ROS_INFO_STREAM(time_diff); + EXPECT_LT(time_diff, max_time); + + time_diff = (current_time - depth_stamp_).toSec(); + ROS_INFO_STREAM(time_diff); + EXPECT_LT(time_diff, max_time); + + time_diff = (current_time - points_stamp_).toSec(); + ROS_INFO_STREAM(time_diff); + EXPECT_LT(time_diff, max_time); + + cam_sub_.shutdown(); + depth_sub_.shutdown(); + points_sub_.shutdown(); + + // make sure nothing is subscribing to image_trigger topic + // there is no easy API, so call getSystemState + XmlRpc::XmlRpcValue args, result, payload; + args[0] = ros::this_node::getName(); + EXPECT_TRUE(ros::master::execute("getSystemState", args, result, payload, true)); + // [publishers, subscribers, services] + // subscribers in index 1 of payload + for (int i = 0; i < payload[1].size(); ++i) + { + // [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ] + // topic name i is in index 0 + std::string topic = payload[1][i][0]; + EXPECT_EQ(topic.find("image_trigger"), std::string::npos); + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gazebo_depth_camera_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.test b/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.test new file mode 100755 index 0000000..439129b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.world b/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.world new file mode 100755 index 0000000..a1502f0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/depth_camera.world @@ -0,0 +1,144 @@ + + + + + + model://ground_plane + + + + + model://sun + + + + + + 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 + orbit + + + + + false + 2.0 0.0 4.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.5 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.5 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + + + + true + 0.0 0.0 0.5 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.5 + + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + + true + + 0.0 + camera1 + image_raw + camera_info + depth/image_raw + + depth/camera_info + points + camera_link + + 0.07 + 0.001 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion.h b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion.h new file mode 100755 index 0000000..f08ad79 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion.h @@ -0,0 +1,183 @@ +#ifndef GAZEBO_PLUGINS_TEST_CAMERA_DISTORTION_H +#define GAZEBO_PLUGINS_TEST_CAMERA_DISTORTION_H + +// OpenCV includes +#include +#include + +// Test includes +#include + +// ROS includes +#include +#include +#include + +using namespace cv; + +void diffBetween(Mat& orig, Mat& diff, long& total_diff) +{ + MatIterator_ it, end; + Vec3b orig_pixel, diff_pixel; + total_diff = 0; + + for(int i=0; i(i,j); + diff_pixel = diff.at(i,j); + total_diff += abs(orig_pixel[0] - diff_pixel[0]) + + abs(orig_pixel[1] - diff_pixel[1]) + + abs(orig_pixel[2] - diff_pixel[2]); + } + } +} + +class DistortionTest : public testing::Test +{ + protected: + ros::NodeHandle nh_; + + // Used to listen for images + image_transport::Subscriber cam_sub_distorted_; + image_transport::Subscriber cam_sub_undistorted_; + + // Stores found images + sensor_msgs::ImageConstPtr cam_image_distorted_; + sensor_msgs::ImageConstPtr cam_image_undistorted_; + + // Listens for camera metadata to be published + ros::Subscriber cam_info_distorted_sub_; + // Stores received camera metadata + sensor_msgs::CameraInfoConstPtr cam_info_distorted_; + + public: + void cameraDistortionTest(); + + void imageCallback(const sensor_msgs::ImageConstPtr& msg, int cam_index) + { + // for now, only support 2 cameras + assert(cam_index == 0 || cam_index == 1); + if(cam_index == 0) + { + cam_image_undistorted_ = msg; + } + else + { + cam_image_distorted_ = msg; + } + } + void camInfoCallback(const sensor_msgs::CameraInfoConstPtr& msg) + { + cam_info_distorted_ = msg; + } +}; + +void DistortionTest::cameraDistortionTest() +{ + ros::AsyncSpinner spinner(2); + spinner.start(); + + image_transport::ImageTransport trans(nh_); + cam_sub_undistorted_ = + trans.subscribe("/camera_undistorted/image_raw", + 1, + boost::bind(&DistortionTest::imageCallback, + dynamic_cast(this), _1, 0) + ); + + cam_info_distorted_ = nullptr; + cam_image_distorted_ = nullptr; + // acquire information from ROS topics + cam_sub_distorted_ = + trans.subscribe("/camera_distorted/image_raw", + 1, + boost::bind(&DistortionTest::imageCallback, + dynamic_cast(this), _1, 1) + ); + cam_info_distorted_sub_ = + nh_.subscribe("/camera_distorted/camera_info", + 1, + &DistortionTest::camInfoCallback, + dynamic_cast(this) + ); + + // keep waiting until we have an image + + if(cam_info_distorted_ && cam_image_distorted_) { + std::cerr << "available immediately" << std::endl; + } + while (!cam_info_distorted_ || + !cam_image_distorted_ || + !cam_image_undistorted_) + { + ros::Duration(0.1).sleep(); + } + cam_sub_undistorted_.shutdown(); + cam_sub_distorted_.shutdown(); + cam_info_distorted_sub_.shutdown(); + + // load camera coefficients from published ROS information + Mat intrinsic_distorted_matrix = Mat(3, 3, CV_64F); + if(cam_info_distorted_->K.size() == 9) + { + memcpy(intrinsic_distorted_matrix.data, cam_info_distorted_->K.data(), + cam_info_distorted_->K.size()*sizeof(double)); + } + Mat distortion_coeffs = Mat(5, 1, CV_64F); + if(cam_info_distorted_->D.size() == 5) + { + memcpy(distortion_coeffs.data, cam_info_distorted_->D.data(), + cam_info_distorted_->D.size()*sizeof(double)); + } + + // Information acquired, now test the quality of the undistortion + + Mat distorted = Mat(cv_bridge::toCvCopy(cam_image_distorted_)->image); + Mat fixed = distorted.clone(); + Mat undistorted = Mat(cv_bridge::toCvCopy(cam_image_undistorted_)->image); + + //crop the image to remove black borders leftover from (un)distortion + int cropBorder = 50; + cv::Rect myROI(cropBorder, cropBorder, + fixed.rows - 2 * cropBorder, fixed.cols - 2 * cropBorder); + cv::Mat fixed_crop = fixed(myROI); + cv::Mat undistorted_crop = undistorted(myROI); + + + undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); + + //Ensure that we didn't crop away everything + ASSERT_GT(distorted.rows, 0); + ASSERT_GT(distorted.cols, 0); + ASSERT_GT(undistorted.rows, 0); + ASSERT_GT(undistorted.cols, 0); + ASSERT_GT(fixed.rows, 0); + ASSERT_GT(fixed.cols, 0); + + // The difference between the undistorted image and the no-distortion camera + // image should be the lowest when we use the correct distortion parameters. + long diff1 = 0, diff2 = 0; + diffBetween(fixed_crop, undistorted_crop, diff1); + + const double OFFSET = 0.01; + + // test each parameter, varying one at a time + for(size_t i = 0; i < 5; ++i) + { + distortion_coeffs.at(i,0) += OFFSET; + undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); + diffBetween(fixed_crop, undistorted_crop, diff2); + EXPECT_GE(diff2, diff1); + distortion_coeffs.at(i,0) -= OFFSET; + + distortion_coeffs.at(i,0) -= OFFSET; + undistort(distorted, fixed, intrinsic_distorted_matrix, distortion_coeffs); + diffBetween(fixed_crop, undistorted_crop, diff2); + EXPECT_GE(diff2, diff1); + distortion_coeffs.at(i,0) += OFFSET; + } +} + +#endif diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_barrel.cpp b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_barrel.cpp new file mode 100755 index 0000000..b136164 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_barrel.cpp @@ -0,0 +1,13 @@ +#include "distortion.h" + +TEST_F(DistortionTest, barrelDistortion) +{ + cameraDistortionTest(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gazebo_camera_barrel_distortion_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_barrel.test b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_barrel.test new file mode 100755 index 0000000..7187b93 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_barrel.test @@ -0,0 +1,12 @@ + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_barrel.world b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_barrel.world new file mode 100755 index 0000000..ddb7d54 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_barrel.world @@ -0,0 +1,127 @@ + + + + + + + model://sun + + + + true + 0.0 0.0 2.5 0.0 1.5707 0.0 + + 0 0 0 0 0 0 + + + + 0.1 0.1 0.1 + + + + + 30.0 + + 0.927295218 + + 1000 + 1000 + R8G8B8 + + + 0.02 + 300 + + + + -0.1 + -0.1 + 0 + 0 + -0.1 +
0.5 0.5
+ + + + true + 0.0 + camera_distorted + image_raw + camera_info + camera_distorted_link + 0.07 + + -0.1 + -0.1 + 0.0 + 0.0 + -0.1 + false + + + + + + + true + 0.0 0.0 2.5 0.0 1.5707 0.0 + + 0 0 0 0 0 0 + + + + 0.1 0.1 0.1 + + + + + 30.0 + + 0.927295218 + + 1000 + 1000 + R8G8B8 + + + 0.02 + 300 + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 +
0.5 0.5
+ + + + true + 0.0 + camera_undistorted + image_raw + camera_info + camera_undistorted_link + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + model://checkerboard_plane + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_pincushion.cpp b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_pincushion.cpp new file mode 100755 index 0000000..79cff4c --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_pincushion.cpp @@ -0,0 +1,13 @@ +#include "distortion.h" + +TEST_F(DistortionTest, pincushionDistortion) +{ + cameraDistortionTest(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gazebo_camera_pincushion_distortion_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_pincushion.test b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_pincushion.test new file mode 100755 index 0000000..2361fe9 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_pincushion.test @@ -0,0 +1,12 @@ + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_pincushion.world b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_pincushion.world new file mode 100755 index 0000000..0b9f49c --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/distortion_pincushion.world @@ -0,0 +1,127 @@ + + + + + + + model://sun + + + + true + 0.0 0.0 2.5 0.0 1.5707 0.0 + + 0 0 0 0 0 0 + + + + 0.1 0.1 0.1 + + + + + 30.0 + + 0.927295218 + + 1000 + 1000 + R8G8B8 + + + 0.02 + 300 + + + + 0.1 + 0.1 + 0 + 0 + 0.1 +
0.5 0.5
+ + + + true + 0.0 + camera_distorted + image_raw + camera_info + camera_distorted_link + 0.07 + + 0.1 + 0.1 + 0.0 + 0.0 + 0.1 + false + + + + + + + true + 0.0 0.0 2.5 0.0 1.5707 0.0 + + 0 0 0 0 0 0 + + + + 0.1 0.1 0.1 + + + + + 30.0 + + 0.927295218 + + 1000 + 1000 + R8G8B8 + + + 0.02 + 300 + + + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 +
0.5 0.5
+ + + + true + 0.0 + camera_undistorted + image_raw + camera_info + camera_undistorted_link + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + model://checkerboard_plane + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/multicamera.cpp b/gazebo_ros_pkgs/gazebo_plugins/test/camera/multicamera.cpp new file mode 100755 index 0000000..b0e68ac --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/multicamera.cpp @@ -0,0 +1,116 @@ +#include +// #include +// #include +// #include +#include +#include +#include +#include + +class MultiCameraTest : public testing::Test +{ +protected: + virtual void SetUp() + { + has_new_image_ = false; + } + + ros::NodeHandle nh_; + // image_transport::SubscriberFilter cam_left_sub_; + // image_transport::SubscriberFilter cam_right_sub_; + message_filters::Subscriber cam_left_sub_; + message_filters::Subscriber cam_right_sub_; + + bool has_new_image_; + ros::Time image_left_stamp_; + ros::Time image_right_stamp_; + + // typedef message_filters::sync_policies::ApproximateTime< + // sensor_msgs::Image, sensor_msgs::Image + // > MySyncPolicy; + // message_filters::Synchronizer< MySyncPolicy > sync_; + + +public: + void imageCallback( + const sensor_msgs::ImageConstPtr& left_msg, + const sensor_msgs::ImageConstPtr& right_msg) + { + image_left_stamp_ = left_msg->header.stamp; + image_right_stamp_ = right_msg->header.stamp; + has_new_image_ = true; + } +}; + +// Test if the camera image is published at all, and that the timestamp +// is not too long in the past. +TEST_F(MultiCameraTest, cameraSubscribeTest) +{ + // image_transport::ImageTransport it(nh_); + // cam_left_sub_.subscribe(it, "stereo/camera/left/image_raw", 1); + // cam_right_sub_.subscribe(it, "stereo/camera/right/image_raw", 1); + // sync_ = message_filters::Synchronizer( + // MySyncPolicy(4), cam_left_sub_, cam_right_sub_); + cam_left_sub_.subscribe(nh_, "stereo/camera/left/image_raw", 1); + cam_right_sub_.subscribe(nh_, "stereo/camera/right/image_raw", 1); + message_filters::TimeSynchronizer sync( + cam_left_sub_, cam_right_sub_, 4); + sync.registerCallback(boost::bind(&MultiCameraTest::imageCallback, + dynamic_cast(this), _1, _2)); +#if 0 + // wait for gazebo to start publishing + // TODO(lucasw) this isn't really necessary since this test + // is purely passive + bool wait_for_topic = true; + while (wait_for_topic) + { + // @todo this fails without the additional 0.5 second sleep after the + // publisher comes online, which means on a slower or more heavily + // loaded system it may take longer than 0.5 seconds, and the test + // would hang until the timeout is reached and fail. + if (cam_sub_.getNumPublishers() > 0) + wait_for_topic = false; + ros::Duration(0.5).sleep(); + } +#endif + + while (!has_new_image_) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + + double sync_diff = (image_left_stamp_ - image_right_stamp_).toSec(); + EXPECT_EQ(sync_diff, 0.0); + + // This check depends on the update period being much longer + // than the expected difference between now and the received image time + // TODO(lucasw) + // this likely isn't that robust - what if the testing system is really slow? + double time_diff = (ros::Time::now() - image_left_stamp_).toSec(); + ROS_INFO_STREAM(time_diff); + EXPECT_LT(time_diff, 1.0); + // cam_sub_.shutdown(); + + // make sure nothing is subscribing to image_trigger topic + // there is no easy API, so call getSystemState + XmlRpc::XmlRpcValue args, result, payload; + args[0] = ros::this_node::getName(); + EXPECT_TRUE(ros::master::execute("getSystemState", args, result, payload, true)); + // [publishers, subscribers, services] + // subscribers in index 1 of payload + for (int i = 0; i < payload[1].size(); ++i) + { + // [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ] + // topic name i is in index 0 + std::string topic = payload[1][i][0]; + EXPECT_EQ(topic.find("image_trigger"), std::string::npos); + } +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gazebo_multicamera_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/multicamera.test b/gazebo_ros_pkgs/gazebo_plugins/test/camera/multicamera.test new file mode 100755 index 0000000..3ec022a --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/multicamera.test @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/multicamera.world b/gazebo_ros_pkgs/gazebo_plugins/test/camera/multicamera.world new file mode 100755 index 0000000..4c0b176 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/multicamera.world @@ -0,0 +1,146 @@ + + + + + + model://ground_plane + + + + + model://sun + + + + + + 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 + orbit + + + + + false + 0.0 2.0 2.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.5 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.5 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + + 0.1 + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + 0.0 + 0.007 + + + + 0 -0.07 0 0 0 0 + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + 0.0 + 0.007 + + + + true + 0.0 + stereo/camera + image_raw + camera_info + left_camera_optical_frame + + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/triggered_camera.cpp b/gazebo_ros_pkgs/gazebo_plugins/test/camera/triggered_camera.cpp new file mode 100755 index 0000000..9d7a51a --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/triggered_camera.cpp @@ -0,0 +1,114 @@ +#include +#include +#include +#include + +class TriggeredCameraTest : public testing::Test +{ +protected: + virtual void SetUp() + { + images_received_ = 0; + } + + ros::NodeHandle nh_; + image_transport::Subscriber cam_sub_; + int images_received_; + ros::Time image_stamp_; +public: + void imageCallback(const sensor_msgs::ImageConstPtr& msg) + { + image_stamp_ = msg->header.stamp; + images_received_++; + } +}; + +// Test if the camera image is published at all, and that the timestamp +// is not too long in the past. +TEST_F(TriggeredCameraTest, cameraSubscribeTest) +{ + image_transport::ImageTransport it(nh_); + cam_sub_ = it.subscribe("camera1/image_raw", 5, + &TriggeredCameraTest::imageCallback, + dynamic_cast(this)); + + // wait for 3 seconds to confirm that we don't receive any images + for (unsigned int i = 0; i < 30; ++i) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + EXPECT_EQ(images_received_, 0); + + // make sure something is subscribing to image_trigger topic + // there is no easy API, so call getSystemState + XmlRpc::XmlRpcValue args, result, payload; + args[0] = ros::this_node::getName(); + EXPECT_TRUE(ros::master::execute("getSystemState", args, result, payload, true)); + // [publishers, subscribers, services] + // subscribers in index 1 of payload + int trigger_listeners = 0; + for (int i = 0; i < payload[1].size(); ++i) + { + // [ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ] + // topic name i is in index 0 + std::string topic = payload[1][i][0]; + if (topic.find("image_trigger") != std::string::npos) + { + trigger_listeners++; + } + } + EXPECT_EQ(trigger_listeners, 1); + + // publish to trigger topic and expect an update within one second: + ros::Publisher trigger_pub = nh_.advertise("camera1/image_trigger", 1, true); + std_msgs::Empty msg; + + trigger_pub.publish(msg); + for (unsigned int i = 0; i < 10 && !images_received_; ++i) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + EXPECT_EQ(images_received_, 1); + + // then wait for 3 seconds to confirm that we don't receive any more images + for (unsigned int i = 0; i < 30; ++i) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + EXPECT_EQ(images_received_, 1); + + // then send two trigger messages very close together, and expect two more + // images + trigger_pub.publish(msg); + ros::spinOnce(); + ros::Duration(0.01).sleep(); + trigger_pub.publish(msg); + ros::spinOnce(); + ros::Duration(0.01).sleep(); + for (unsigned int i = 0; i < 10 && images_received_ < 2; ++i) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + EXPECT_EQ(images_received_, 3); + + // then wait for 3 seconds to confirm that we don't receive any more images + for (unsigned int i = 0; i < 30; ++i) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + EXPECT_EQ(images_received_, 3); + + cam_sub_.shutdown(); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "gazebo_camera_test"); + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/triggered_camera.test b/gazebo_ros_pkgs/gazebo_plugins/test/camera/triggered_camera.test new file mode 100755 index 0000000..e9f6ec5 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/triggered_camera.test @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/camera/triggered_camera.world b/gazebo_ros_pkgs/gazebo_plugins/test/camera/triggered_camera.world new file mode 100755 index 0000000..30cb340 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/camera/triggered_camera.world @@ -0,0 +1,140 @@ + + + + + + model://ground_plane + + + + + model://sun + + + + + + 4.927360 -4.376610 3.740080 0.000000 0.275643 2.356190 + orbit + + + + + false + 2.0 0.0 4.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.5 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.5 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + + + + true + 0.0 0.0 0.5 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0 + 1 + + 1.3962634 + + 640 + 480 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + true + image_trigger + + 0.0 + camera1 + image_raw + camera_info + camera_link + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/config/example_models.yaml b/gazebo_ros_pkgs/gazebo_plugins/test/config/example_models.yaml new file mode 100755 index 0000000..754bac1 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/config/example_models.yaml @@ -0,0 +1,31 @@ +gazebo_models: # everything here is namespaced under "gazebo_models" + pr2: # model name used inside gazebo + specification: robot_description # potentially contains: + # 1. a verbatim string with URDF or Gazebo Model XML or + # 2. a parameter name that refers to a parameter containing (1.) + # the spawner will differentiate automatically + namespace: / # namespace for gazebo_ros_plugins related to this model + initial_pose: # initial pose for the model + x: 0 + y: 0 + z: 0 + rx: 0 + ry: 0 + rz: 0 + joint_name: r_forearm_link # name of the joint to set initial position + position: 1.57 # initial joint position + joint_name: head_pan_link # name of the joint to set initial position + position: 0.78 # initial joint position + + # another example + table: + specification: table_description + namespace: /table + initial_pose: + x: 1 + y: 0 + z: 0 + rx: 0 + ry: 0 + rz: 0 + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/multi_robot_scenario.launch b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/multi_robot_scenario.launch new file mode 100755 index 0000000..ee83cab --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/multi_robot_scenario.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.gazebo.launch b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.gazebo.launch new file mode 100755 index 0000000..b13c1fe --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.rviz b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.rviz new file mode 100755 index 0000000..4300d22 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.rviz @@ -0,0 +1,482 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /RobotModel1 + - /TF1/Tree1 + - /LaserScan1 + - /LaserScan2 + - /Camera1 + Splitter Ratio: 0.528662 + Tree Height: 344 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: LaserScan +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + battery0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + center_hubcap: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + center_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + chassis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + swivel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: r1/robot_description + TF Prefix: r1 + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + r1/base_link: + Value: true + r1/battery0: + Value: true + r1/center_hubcap: + Value: true + r1/center_wheel: + Value: true + r1/chassis: + Value: true + r1/front_camera: + Value: true + r1/front_laser: + Value: true + r1/left_hub: + Value: true + r1/left_wheel: + Value: true + r1/odom: + Value: true + r1/right_hub: + Value: true + r1/right_wheel: + Value: true + r1/sonar: + Value: true + r1/swivel: + Value: true + r1/top: + Value: true + r2/base_link: + Value: true + r2/battery0: + Value: true + r2/center_hubcap: + Value: true + r2/center_wheel: + Value: true + r2/chassis: + Value: true + r2/front_camera: + Value: true + r2/front_laser: + Value: true + r2/left_hub: + Value: true + r2/left_wheel: + Value: true + r2/odom: + Value: true + r2/right_hub: + Value: true + r2/right_wheel: + Value: true + r2/sonar: + Value: true + r2/swivel: + Value: true + r2/top: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + r1/odom: + r1/base_link: + r1/chassis: + r1/battery0: + {} + r1/front_camera: + {} + r1/front_laser: + {} + r1/left_hub: + r1/left_wheel: + {} + r1/right_hub: + r1/right_wheel: + {} + r1/sonar: + {} + r1/swivel: + r1/center_wheel: + r1/center_hubcap: + {} + r1/top: + {} + r2/odom: + r2/base_link: + r2/chassis: + r2/battery0: + {} + r2/front_camera: + {} + r2/front_laser: + {} + r2/left_hub: + r2/left_wheel: + {} + r2/right_hub: + r2/right_wheel: + {} + r2/sonar: + {} + r2/swivel: + r2/center_wheel: + r2/center_hubcap: + {} + r2/top: + {} + Update Interval: 0 + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + battery0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + center_hubcap: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + center_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + chassis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_camera: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_laser: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sonar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + swivel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: r2/robot_description + TF Prefix: r2 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 85; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 999999 + Min Color: 0; 0; 0 + Min Intensity: -999999 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.03 + Style: Flat Squares + Topic: /r1/front_laser/scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 85; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 3.49902e-38 + Min Color: 0; 0; 0 + Min Intensity: -999999 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.03 + Style: Flat Squares + Topic: /r2/front_laser/scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /r1/front_camera/image_raw + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Value: true + Visibility: + Grid: true + LaserScan: true + RobotModel: true + TF: true + Value: true + Zoom Factor: 1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.00511 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.243573 + Y: -0.560009 + Z: 0.448538 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.585398 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.665372 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000013c000002affc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006a00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000001ed000000e900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000232000000bc0000001600ffffff000000010000010f000002affc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002af000000b600fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000029a00fffffffb0000000800540069006d0065010000000000000450000000000000000000000259000002af00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 2265 + Y: 97 diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.urdf.launch b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.urdf.launch new file mode 100755 index 0000000..02822f0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/launch/pioneer3dx.urdf.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/laser/hokuyo.dae b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/laser/hokuyo.dae new file mode 100755 index 0000000..bc65151 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/laser/hokuyo.dae @@ -0,0 +1,253 @@ + + + + + Blender User + Blender 2.64.0 r51232 + + 2013-03-22T08:19:53 + 2013-03-22T08:19:53 + + Z_UP + + + + + + + 49.13434 + 1.777778 + 0.1 + 100 + + + + + + + + + + 1 1 1 + 1 + 0 + 0.00111109 + + + + + 0.000999987 + 0 + 1 + 1 + 1 + 1 + 1 + 2 + 0 + 1 + 1 + 1 + 1 + 1 + 0 + 2880 + 2 + 30.002 + 1.000799 + 0.04999995 + 29.99998 + 1 + 2 + 0 + 0 + 1 + 1 + 1 + 1 + 8192 + 1 + 1 + 0 + 1 + 1 + 1 + 3 + 0 + 0 + 0 + 0 + 45 + 0 + 1 + 1 + 1 + 3 + 0.15 + 75 + 1 + 1 + 0 + 1 + 1 + 0 + + + + + + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.020312 0.020312 0.020312 1 + + + 0.25 0.25 0.25 1 + + + 12 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + 0 0 0 1 + + + 0 0 0 1 + + + 0.695112 0.695112 0.695112 1 + + + 0.0625 0.0625 0.0625 1 + + + 0 + + + 1 + + + + + + 1 + + + + 1 + + + + + + + + + + + + + + + -0.02213996 0.005949974 -0.004869997 -0.02213996 0.005949974 0.004869997 -0.02164 0.009949982 -0.004999995 -0.02164 0.009949982 -0.005999982 -0.02164 0.009949982 0.004999995 -0.02164 0.009949982 0.005999982 -0.02149999 0.009949982 -0.006499946 -0.02149999 0.009949982 0.006499946 -0.02113997 0.009949982 -0.006869971 -0.02113997 0.009949982 0.006869971 -0.02063995 0.005949974 0.006999969 -0.02063995 0.009949982 -0.006999969 -0.02063995 0.009949982 0.006999969 -0.01979994 0.009949982 -0.006999969 -0.01979994 0.005949974 -0.006999969 -0.01979994 0.005949974 0.006999969 -0.01979994 0.009949982 0.006999969 -0.01838999 0.005949974 0.01014 -0.02063995 0.005949974 -0.006999969 -0.02113997 0.005949974 0.006869971 -0.02113997 0.005949974 -0.006869971 -0.02149999 0.005949974 0.006499946 -0.02149999 0.005949974 -0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02164 0.005949974 -0.004999995 -0.02164 0.005949974 -0.005999982 -0.01920998 0.01990997 0.004999995 -0.01920998 0.01990997 -0.004999995 -0.005279958 0.02695 -0.01798999 -0.009719967 0.02695 -0.01595997 -0.01014 0.02695 -0.01576995 -0.01365 0.02695 -0.01273 -0.01416999 0.02695 -0.01227998 -0.01655995 0.02695 -0.008569955 -0.01705998 0.02695 -0.007789969 -0.01822996 0.02695 -0.003779947 -0.01855999 0.02695 -0.00266999 -0.01855999 0.02695 0.00127995 -0.01855999 0.02695 0.00266999 -0.01750999 0.02695 0.006229996 -0.01705998 0.02695 0.007789969 -0.01516997 0.02695 0.01071995 -0.01416999 0.02695 0.01227998 -0.01170998 0.02695 0.01440995 -0.01014 0.02695 0.01576995 -0.007389962 0.02695 0.01703 -0.005279958 0.02695 0.01798999 -0.002529978 0.02695 0.01838999 0 0.02695 0.01874995 0.002529978 0.02695 0.01838999 0.005279958 0.02695 0.01798999 0.007389962 0.02695 0.01703 0.01014 0.02695 0.01576995 0.01170998 0.02695 0.01440995 0.01416999 0.02695 0.01227998 0.01516997 0.02695 0.01071995 0.01705998 0.02695 0.007789969 0.01750999 0.02695 0.006229996 0.01855999 0.02695 0.00266999 0.01855999 0.02695 0.00127995 0.01855999 0.02695 -0.00266999 0.01822996 0.02695 -0.003779947 0.01705998 0.02695 -0.007789969 0.01655995 0.02695 -0.008569955 0.01416999 0.02695 -0.01227998 0.01365 0.02695 -0.01273 0.01014 0.02695 -0.01576995 0.009719967 0.02695 -0.01595997 0.005279958 0.02695 -0.01798999 0.005049943 0.02695 -0.01801997 -0.005049943 0.02695 -0.01801997 0 0.02695 -0.01874995 -0.01885998 0.02594995 0.001289963 -0.01850998 0.02594995 -0.003849983 -0.02039998 0.009949982 -0.004999995 -0.02039998 0.009949982 0.004999995 -0.01848 0.02594995 0.003999948 -0.01836997 0.02572995 0.004629969 -0.01840996 0.02588999 0.004329979 -0.01838999 0.02579998 0.004489958 -0.01835 0.02544999 0.004859983 -0.01838999 0.02494996 0.004999995 -0.01835995 0.02517998 0.004969954 -0.02164 0.02494996 0.004999995 -0.02164 0.02544999 0.004869997 -0.02188998 0.02537995 0.004869997 -0.02206999 0.02519994 0.004869997 -0.02213996 0.02494996 0.004869997 -0.02164 0.02581995 0.004499971 -0.02206999 0.02569997 0.004499971 -0.02238994 0.02537995 0.004499971 -0.02249997 0.02494996 0.004499971 -0.02164 0.02594995 0.003999948 -0.02213996 0.02581995 0.003999948 -0.02263998 0.02494996 0.003999948 -0.02249997 0.02544999 0.003999948 -0.02213996 0.02581995 -0.003999948 -0.02249997 0.02544999 -0.003999948 -0.02263998 0.02494996 -0.003999948 -0.02206999 0.02569997 -0.004499971 -0.02188998 0.02537995 -0.004869997 -0.02238994 0.02537995 -0.004499971 -0.02249997 0.02494996 -0.004499971 -0.02206999 0.02519994 -0.004869997 -0.02213996 0.02494996 -0.004869997 -0.01838999 0.02494996 -0.004999995 -0.02164 0.02494996 -0.004999995 -0.01835 0.02555996 -0.004789948 -0.01835995 0.02526998 -0.004949986 -0.01835 0.02543997 -0.00484997 -0.01836997 0.02577996 -0.004559993 -0.02164 0.02544999 -0.004869997 -0.02164 0.02581995 -0.004499971 -0.01848 0.02594995 -0.003999948 -0.02164 0.02594995 -0.003999948 -0.01841998 0.02590996 -0.004269957 -0.02001994 0.01871997 -0.004999995 -0.01986998 0.01879996 -0.004999995 -0.02025997 0.01844996 -0.004999995 -0.02039998 0.01794999 -0.004999995 -0.02037996 0.01813995 -0.004999995 -0.02039998 0.01794999 0.004999995 -0.02033996 0.01826995 0.004999995 -0.01993995 0.01877999 0.004999995 -0.01936 0.01894998 0.004999995 -0.01962 0.01892 0.004999995 -0.01879996 0.01894998 0.006679952 -0.01845997 0.01894998 0.007699966 -0.01936 0.01894998 -0.004999995 -0.01971 0.01888996 -0.004999995 -0.01845997 0.01894998 -0.007699966 -0.01668 0.01894998 -0.01102995 -0.01141995 0.01894998 -0.01641994 -7.8e-4 0.01894998 -0.01997995 0 0.01894998 -0.01993995 0.002989947 0.01894998 -0.01976996 0.01315999 0.01894998 -0.01505994 0.01454997 0.01894998 -0.01358997 0.01576 0.01894998 -0.01230996 0.01990997 0.01894998 0.001369953 0.01990997 0.01894998 -0.001889944 0.01919996 0.01894998 0.005609989 0.01779997 0.01894998 0.009119987 0.00792998 0.01894998 0.01826995 0.01008999 0.01894998 0.01726996 -0.007959961 0.01894998 0.01832997 -0.004529953 0.01894998 0.01947999 -0.01668 0.01894998 0.01102995 -0.01892 0.01881998 0.007889986 -0.02024996 0.01843994 0.004999995 -0.02017998 0.01855999 0.004999995 -0.00812 0.01894998 -0.01827996 0.006659984 0.01894998 -0.01885998 0.01576 0.01894998 0.01230996 0.002989947 0.01894998 0.01976996 0.002719998 0.01894998 0.01978999 -0.01141995 0.01894998 0.01641994 -0.01256996 0.01894998 0.01545 -0.01767998 0.01894998 -0.009159982 -0.01892 0.01881998 -0.007889986 -0.01937997 0.01794999 -0.008079946 -0.01830995 0.01794999 -0.01009994 -0.01925998 0.01844996 -0.008029997 -0.01458996 0.01894998 -0.01362997 -0.01709997 0.01881998 -0.01130998 -0.01750999 0.01794999 -0.01159 -0.0151 0.01794999 -0.01457995 -0.01739996 0.01844996 -0.01150995 -0.01431 0.01894998 -0.01397997 -0.01466 0.01881998 -0.01432996 -0.01501995 0.01794999 -0.01467996 -0.01491999 0.01844996 -0.01457995 -0.01034998 0.01894998 -0.01701998 -0.008319973 0.01881998 -0.01872998 -0.01169997 0.01881998 -0.01682996 -0.01198995 0.01794999 -0.01723998 -0.01190996 0.01844996 -0.01712995 -8.2e-4 0.01794999 -0.02098 -0.004759967 0.01794999 -0.02044999 -0.004729986 0.01844996 -0.02031999 -0.005689978 0.01794999 -0.02013999 -0.01073998 0.01794999 -0.01794999 -0.008529961 0.01794999 -0.01918995 -0.008469998 0.01844996 -0.01906996 -2.5e-4 0.01794999 -0.02094995 -8.2e-4 0.01844996 -0.02085 -8.1e-4 0.01881998 -0.02047997 -0.005379974 0.01894998 -0.01919996 -0.004649996 0.01881998 -0.01996999 -0.004529953 0.01894998 -0.01947999 0.005379974 0.01894998 -0.01917999 0.006829977 0.01881998 -0.01932996 0.003069996 0.01881998 -0.02026998 0.003139972 0.01794999 -0.02075999 0.003119945 0.01844996 -0.02063 0.01381999 0.01794999 -0.01580995 0.01059997 0.01794999 -0.01813 0.01052999 0.01844996 -0.01800996 0.01034998 0.01794999 -0.01824998 0.005209982 0.01794999 -0.02024996 0.006989955 0.01794999 -0.01979994 0.006949961 0.01844996 -0.01966995 0.01469999 0.01794999 -0.01488 0.01372998 0.01844996 -0.01570999 0.01348996 0.01881998 -0.01542997 0.01008999 0.01894998 -0.01726996 0.01034998 0.01881998 -0.01769995 0.01036995 0.01894998 -0.01705998 0.01774996 0.01894998 -0.009199976 0.01823997 0.01881998 -0.009349942 0.01615995 0.01881998 -0.01261997 0.01655 0.01794999 -0.01291996 0.01644998 0.01844996 -0.01283997 0.02090996 0.01794999 -0.00198996 0.02023994 0.01794999 -0.005469977 0.02002996 0.01844996 -0.005849957 0.02015995 0.01794999 -0.005889952 0.01807999 0.01794999 -0.01052999 0.01868999 0.01794999 -0.009579956 0.01857 0.01844996 -0.009519994 0.02090996 0.01794999 0 0.02076995 0.01844996 -0.001969993 0.02041 0.01881998 -0.001939952 0.01919996 0.01894998 -0.005609989 0.01967996 0.01881998 -0.00575 0.01949995 0.01894998 -0.004049956 0.01990997 0.01894998 0.001889944 0.02041 0.01881998 0.001939952 0.02090996 0.01794999 0.00198996 0.02023994 0.01794999 0.005469977 0.02076995 0.01844996 0.001969993 0.01876997 0.01894998 0.006669998 0.01967996 0.01881998 0.00575 0.02015995 0.01794999 0.005889952 0.02002996 0.01844996 0.005849957 0.01627999 0.01894998 0.0115 0.01615995 0.01881998 0.01261997 0.01823997 0.01881998 0.009349942 0.01868999 0.01794999 0.009579956 0.01857 0.01844996 0.009519994 0.01059997 0.01794999 0.01813 0.01381999 0.01794999 0.01580995 0.01372998 0.01844996 0.01570999 0.01469999 0.01794999 0.01488 0.01807999 0.01794999 0.01052999 0.01655 0.01794999 0.01291996 0.01644998 0.01844996 0.01283997 0.01034998 0.01794999 0.01824998 0.01052999 0.01844996 0.01800996 0.01034998 0.01881998 0.01769995 0.01315999 0.01894998 0.01505994 0.01348996 0.01881998 0.01542997 0.01258999 0.01894998 0.01546996 0.006659984 0.01894998 0.01885998 0.003069996 0.01881998 0.02026998 0.006829977 0.01881998 0.01932996 0.006989955 0.01794999 0.01979994 0.006949961 0.01844996 0.01966995 -0.004759967 0.01794999 0.02044999 -8.2e-4 0.01794999 0.02098 -8.2e-4 0.01844996 0.02085 -2.5e-4 0.01794999 0.02094995 0.005209982 0.01794999 0.02024996 0.003139972 0.01794999 0.02075999 0.003119945 0.01844996 0.02063 -0.005689978 0.01794999 0.02013999 -0.004729986 0.01844996 0.02031999 -0.004649996 0.01881998 0.01996999 -7.8e-4 0.01894998 0.01997995 -8.1e-4 0.01881998 0.02047997 -0.002709984 0.01894998 0.01972997 -0.00812 0.01894998 0.01827996 -0.01169997 0.01881998 0.01682996 -0.008319973 0.01881998 0.01872998 -0.008529961 0.01794999 0.01918995 -0.008469998 0.01844996 0.01906996 -0.01750999 0.01794999 0.01159 -0.0151 0.01794999 0.01457995 -0.01491999 0.01844996 0.01457995 -0.01501995 0.01794999 0.01467996 -0.01073998 0.01794999 0.01794999 -0.01198995 0.01794999 0.01723998 -0.01190996 0.01844996 0.01712995 -0.01937997 0.01794999 0.008079946 -0.01830995 0.01794999 0.01009994 -0.01925998 0.01844996 0.008029997 -0.01739996 0.01844996 0.01150995 -0.01709997 0.01881998 0.01130998 -0.01431 0.01894998 0.01397997 -0.01466 0.01881998 0.01432996 -0.01629996 0.01894998 0.0115 -0.01014 0.03504997 -0.01576995 -0.005279958 0.03504997 0.01798999 0 0.03504997 0.01874995 0.005279958 0.03504997 0.01798999 0.01014 0.03504997 0.01576995 0.01416999 0.03504997 0.01227998 0.01705998 0.03504997 0.007789969 0.01855999 0.03504997 0.00266999 0.01855999 0.03504997 -0.00266999 -0.01014 0.03504997 0.01576995 -0.01416999 0.03504997 0.01227998 -0.01705998 0.03504997 0.007789969 -0.01855999 0.03504997 0.00266999 -0.01855999 0.03504997 -0.00266999 -0.01705998 0.03504997 -0.007789969 -0.01416999 0.03504997 -0.01227998 -0.005279958 0.03504997 -0.01798999 0 0.03504997 -0.01874995 0.005279958 0.03504997 -0.01798999 0.01014 0.03504997 -0.01576995 0.01416999 0.03504997 -0.01227998 0.01705998 0.03504997 -0.007789969 0.001609981 0.03494995 -0.01717996 -0.003309965 0.03504997 -0.01692998 -0.003309965 0.03494995 -0.01692998 0.001759946 0.03494995 -0.01713997 0.001969993 0.03494995 -0.01708996 0.001609981 0.03504997 -0.01717996 0.004629969 0.03494995 -0.01644998 0.006399989 0.03494995 -0.01601999 0.007199943 0.03494995 -0.01555997 0.007989943 0.03494995 -0.0151 0.006399989 0.03504997 -0.01601999 0.009659945 0.03494995 -0.01413995 0.01066994 0.03494995 -0.01354998 0.01066994 0.03504997 -0.01354998 0.01512998 0.03494995 -0.007909953 0.01104998 0.03494995 -0.01314997 0.01406997 0.03504997 -0.009979963 0.01406997 0.03494995 -0.009979963 0.01695996 0.03494995 -0.002209961 0.01631999 0.03494995 -0.005589962 0.01631999 0.03504997 -0.005589962 0.01652997 0.03494995 -0.004479944 -0.007969975 0.03494995 -0.01529997 -0.007969975 0.03504997 -0.01529997 -0.009269952 0.03494995 -0.01454997 -0.01002997 0.03494995 0.01388996 -0.01088994 0.03494995 0.01314997 -0.01299995 0.03494995 0.01133996 -0.01299995 0.03504997 0.01133996 -0.01378995 0.03494995 0.01010996 -0.01567995 0.03494995 0.007189989 -0.01567995 0.03504997 0.007189989 -0.01626998 0.03494995 0.005199968 -0.01706999 0.03494995 0.002459943 -0.01706999 0.03494995 0.002019941 -0.01706999 0.03494995 9.7e-4 -0.01706999 0.03494995 6.9e-4 -0.01706999 0.03504997 0.002459943 -0.01706999 0.03494995 -0.002209961 -0.01706999 0.03494995 -0.002459943 -0.01706999 0.03504997 -0.002459943 -0.01647996 0.03494995 -0.004479944 -0.01567995 0.03494995 -0.007189989 -0.01567995 0.03504997 -0.007189989 -0.01521998 0.03494995 -0.007909953 -0.01299995 0.03504997 -0.01133996 -0.01299995 0.03494995 -0.01133996 -0.01002997 0.03494995 -0.01388996 -0.01088994 0.03494995 -0.01314997 -0.009269952 0.03504997 -0.01454997 -0.009269952 0.03494995 0.01454997 -0.009269952 0.03504997 0.01454997 0.01718997 0.03494995 9.7e-4 0.01699 0.03494995 0.002019941 0.01638996 0.03494995 0.005199968 0.01631999 0.03494995 0.005589962 0.01631999 0.03504997 0.005589962 0.01104998 0.03494995 0.01314997 0.01406997 0.03494995 0.009979963 0.01406997 0.03504997 0.009979963 0.01394999 0.03494995 0.01010996 0.01066994 0.03494995 0.01354998 0.009659945 0.03494995 0.01413995 0.01066994 0.03504997 0.01354998 0.007199943 0.03494995 0.01555997 0.006399989 0.03494995 0.01601999 0.006399989 0.03504997 0.01601999 0.004629969 0.03494995 0.01644998 0.001969993 0.03494995 0.01708996 0.001759946 0.03494995 0.01713997 0.001609981 0.03494995 0.01717996 0.001609981 0.03504997 0.01717996 -0.003309965 0.03494995 0.01692998 -0.007969975 0.03504997 0.01529997 -0.007969975 0.03494995 0.01529997 -0.003309965 0.03504997 0.01692998 0.01722997 0.03504997 7.5e-4 0.01722997 0.03494995 7.5e-4 0.01722997 0.03494995 -7.5e-4 0.01722997 0.03504997 -7.5e-4 -0.02063995 0.005949974 0.006999969 -0.02213996 0.005949974 0.004869997 -0.02213996 0.005949974 -0.004869997 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.02499997 -0.03504997 -0.023 -0.02499997 -0.03504997 0.023 -0.02472996 -0.03504997 -0.02399998 -0.02472996 -0.03504997 0.02399998 -0.02399998 -0.03504997 -0.02472996 -0.02399998 -0.03504997 0.02472996 -0.02363997 0.003949999 -0.02164 -0.02363997 0.003949999 0.02164 -0.02363997 0.00544995 -0.02164 -0.02363997 0.00544995 0.02164 -0.02357 0.005699992 -0.02164 -0.02357 0.005699992 0.02164 -0.02338999 0.005879998 -0.02164 -0.02338999 0.005879998 0.02164 -0.02336996 0.003959953 0.02263998 -0.02336996 0.00544995 -0.02263998 -0.02336996 0.00544995 0.02263998 -0.02336996 0.003959953 -0.02263998 -0.02331 0.005699992 -0.02260994 -0.02331 0.005699992 0.02260994 -0.02314996 0.005879998 -0.02250999 -0.02314996 0.005879998 0.02250999 -0.02313995 0.005949974 -0.02164 -0.02313995 0.005949974 0 -0.02313995 0.005949974 0.02164 -0.023 -0.03504997 -0.02499997 -0.023 -0.03504997 0.02499997 -0.02293998 0.005949974 -0.02238994 -0.02293998 0.005949974 0.02238994 0 -0.03504997 0 -0.02263998 0.003959953 0.02336996 -0.02263998 0.003959953 -0.02336996 -0.02263998 0.00544995 -0.02336996 -0.02263998 0.00544995 0.02336996 -0.02260994 0.005699992 -0.02331 -0.02260994 0.005699992 0.02331 -0.02250999 0.005879998 -0.02314996 -0.02250999 0.005879998 0.02314996 0 0.005949974 -0.02313995 -0.02164 0.005949974 -0.02313995 -0.02238994 0.005949974 -0.02293998 -0.02238994 0.005949974 0.02293998 -0.02164 0.003949999 -0.02363997 -0.02164 0.003949999 0.02363997 -0.02164 0.00544995 -0.02363997 -0.02164 0.00544995 0.02363997 -0.02164 0.005699992 -0.02357 -0.02164 0.005699992 0.02357 -0.02164 0.005879998 -0.02338999 -0.02164 0.005879998 0.02338999 -0.02164 0.005949974 0.02313995 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.02213996 0.005949974 -0.004869997 0 0.005949974 0.02313995 0.02164 0.003949999 -0.02363997 0.02164 0.003949999 0.02363997 0.02164 0.00544995 -0.02363997 0.02164 0.00544995 0.02363997 0.02164 0.005699992 -0.02357 0.02164 0.005699992 0.02357 0.02164 0.005879998 -0.02338999 0.02164 0.005879998 0.02338999 0.02164 0.005949974 -0.02313995 0.02164 0.005949974 0.02313995 0.02238994 0.005949974 -0.02293998 0.02238994 0.005949974 0.02293998 0.02250999 0.005879998 -0.02314996 0.02250999 0.005879998 0.02314996 0.02260994 0.005699992 -0.02331 0.02260994 0.005699992 0.02331 0.02263998 0.003959953 -0.02336996 0.02263998 0.003959953 0.02336996 0.02263998 0.00544995 -0.02336996 0.02263998 0.00544995 0.02336996 0.02293998 0.005949974 -0.02238994 0.02293998 0.005949974 0.02238994 0.023 -0.03504997 -0.02499997 0.023 -0.03504997 0.02499997 0.02313995 0.005949974 -0.02164 0.02313995 0.005949974 0 0.02313995 0.005949974 0.02164 0.02314996 0.005879998 -0.02250999 0.02314996 0.005879998 0.02250999 0.02331 0.005699992 -0.02260994 0.02331 0.005699992 0.02260994 0.02336996 0.003959953 -0.02263998 0.02336996 0.003959953 0.02263998 0.02336996 0.00544995 -0.02263998 0.02336996 0.00544995 0.02263998 0.02338999 0.005879998 -0.02164 0.02338999 0.005879998 0.02164 0.02357 0.005699992 -0.02164 0.02357 0.005699992 0.02164 0.02363997 0.003949999 -0.02164 0.02363997 0.003949999 0.02164 0.02363997 0.00544995 -0.02164 0.02363997 0.00544995 0.02164 0.02399998 -0.03504997 -0.02472996 0.02399998 -0.03504997 0.02472996 0.02472996 -0.03504997 -0.02399998 0.02472996 -0.03504997 0.02399998 0.02499997 -0.03504997 -0.023 0.02499997 -0.03504997 0.023 0 0.03504997 0 -0.02213996 0.005949974 0.004869997 -0.02063995 0.005949974 0.006999969 -0.01979994 0.005949974 0.006999969 -0.01838999 0.005949974 0.01014 -0.02113997 0.005949974 0.006869971 -0.02149999 0.005949974 0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.01979994 0.005949974 0.006999969 -0.01838999 0.005949974 0.01014 -0.02113997 0.005949974 0.006869971 -0.02149999 0.005949974 0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.02063995 0.005949974 0.006999969 -0.02213996 0.005949974 0.004869997 -0.02213996 0.005949974 -0.004869997 -0.01979994 0.005949974 0.006999969 -0.01838999 0.005949974 0.01014 -0.02113997 0.005949974 0.006869971 -0.02149999 0.005949974 0.006499946 -0.02164 0.005949974 0.005999982 -0.02164 0.005949974 0.004999995 -0.02263998 0.005949974 -0.003999948 -0.02263998 0.005949974 0.003999948 -0.02249997 0.005949974 -0.004499971 -0.02249997 0.005949974 0.004499971 -0.01838999 0.005949974 -0.01014 -0.01510995 0.005949974 -0.01458996 -0.01510995 0.005949974 0.01458996 -0.01077997 0.005949974 -0.01801997 -0.01077997 0.005949974 0.01801997 -0.005709946 0.005949974 -0.02020996 -0.005709946 0.005949974 0.02020996 -2.5e-4 0.005949974 -0.02099996 -2.5e-4 0.005949974 0.02099996 0 0.005949974 -0.02096998 0 0.005949974 0.02096998 0.005239963 0.005949974 -0.02033996 0.005239963 0.005949974 0.02033996 0.01036 0.005949974 -0.01826995 0.01036 0.005949974 0.01826995 0.01475995 0.005949974 -0.01493996 0.01475995 0.005949974 0.01493996 0.01814997 0.005949974 -0.01056998 0.01814997 0.005949974 0.01056998 0.02026998 0.005949974 -0.005479991 0.02099996 0.005949974 0 0.02026998 0.005949974 0.005479991 -0.005709946 0.005949974 -0.02020996 -0.01077997 0.005949974 -0.01801997 -0.01510995 0.005949974 -0.01458996 -0.01838999 0.005949974 -0.01014 -0.00945729 0.003627836 -0.001710772 0 -0.01554995 0.02431994 0 0.005949974 0 + + + + + + + + + + 0 -1 0 0 -1 0 -0.9492978 0 -0.3143782 -0.9492978 0 0.3143782 -0.7785766 0.001171708 0.6275485 -0.7808654 0.001171231 0.6246982 -0.6209343 -0.001221954 0.7838616 -0.6453004 -0.01542252 0.7637733 -0.3964945 -0.0151537 0.9179121 -0.4893132 0.006718158 0.8720822 -0.4938785 0.006718337 0.8695049 -0.1431924 -0.008457064 0.9896588 -0.316222 0.006060898 0.9486659 -0.3172233 0.0060606 0.9483315 0.2392156 0.007879972 0.9709345 0.119358 -0.01385879 0.9927546 0.0559594 0.00282979 0.9984291 0.1191433 0.00413686 0.9928685 0.05255794 0.004160761 0.9986092 -0.1333157 -0.004680752 0.9910625 0.2450903 0.007883667 0.9694681 0.3747907 -0.01294112 0.9270192 0.4188861 0.001862525 0.9080369 0.4327281 0.001863181 0.9015226 0.6034716 -0.002766609 0.7973797 0.5845207 -0.01302987 0.8112742 0.7900453 -0.01468151 0.6128727 0.7272006 0.007067918 0.6863886 0.7263449 0.007068157 0.6872941 0.9823358 0.002611696 0.1871086 0.9289886 -0.004128932 0.3700852 0.9230861 -0.009820222 0.3844679 0.8414472 0.006709396 0.5402976 0.8421885 0.006709873 0.5391413 0.9819628 0.002612411 0.1890563 0.9911401 -0.01446169 0.1320315 0.9999719 0.007499694 0 0.9999719 0.007499694 0 0.9911401 -0.01446169 -0.1320315 0.9819628 0.002612411 -0.1890563 0.9823358 0.002611696 -0.1871086 0.9231205 -0.004674613 -0.3844823 0.9289375 -0.01127177 -0.3700649 0.7900453 -0.01468151 -0.6128727 0.8421885 0.006709873 -0.5391413 0.8414472 0.006709396 -0.5402976 0.4327281 0.001863181 -0.9015226 0.5845689 -0.002225697 -0.8113411 0.6034404 -0.0105375 -0.7973385 0.7263449 0.007068157 -0.6872941 0.7272006 0.007067918 -0.6863886 0.4188861 0.001862525 -0.9080369 0.3747907 -0.01294112 -0.9270192 0.2450903 0.007883667 -0.9694681 0.2392156 0.007879972 -0.9709345 0.119358 -0.01385879 -0.9927546 0.0559594 0.00282979 -0.9984291 0.1191433 0.00413686 -0.9928685 0.05255794 0.004160761 -0.9986092 -0.1431956 -0.005152344 -0.989681 -0.1333114 -0.009266734 -0.9910309 -0.3964945 -0.0151537 -0.9179121 -0.3172233 0.0060606 -0.9483315 -0.316222 0.006060898 -0.9486659 -0.7808654 0.001171231 -0.6246982 -0.6453768 -8.88654e-4 -0.7638639 -0.6208926 -0.01165908 -0.783809 -0.4938785 0.006718337 -0.8695049 -0.4893132 0.006718158 -0.8720822 -0.7785766 0.001171708 -0.6275485 -0.8049004 -0.01266139 -0.5932749 -0.881016 0.007450222 -0.4730277 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.9471516 0.1680965 0.2732169 -0.407418 0.1514139 -0.9006023 -0.9422059 0.2770094 -0.1884511 -0.9113164 0.1482697 -0.3840813 -0.9315509 0.1626105 -0.3252239 -0.7991091 -0.3146805 -0.5122509 -0.9081746 0.2506373 -0.3352608 -0.9111543 0.1456485 -0.3854666 -0.928664 0.2243673 -0.2953684 -0.925579 0.2859641 -0.2480482 -0.9177774 0.2868903 0.2745518 -0.9178981 0.2856691 0.2754201 -0.9143468 0.1813663 0.3620446 -0.9384292 0.1466302 0.31281 -0.9311535 0.1683419 0.3234411 -0.901751 0.1467134 0.4065961 -0.9103941 0.1680585 0.3780726 -0.912325 0.3225979 0.2521777 -0.9188216 0.2869244 0.2710005 -0.9848498 0.1048805 0.138097 -0.9578264 0.2873474 0 -0.9559739 0.2861407 -0.06509518 -0.9987523 -0.04993659 0 -0.9210147 0.2770513 -0.2738146 -0.4861897 0.1523008 -0.8604789 -0.4091901 0.1777053 -0.8949773 -0.3149025 0.1457877 -0.9378605 -0.1278769 0.1498839 -0.9803991 -0.309273 0.1512408 -0.9388697 -0.141013 0.1688628 -0.9755002 -0.1303948 0.1630746 -0.977959 -0.4837509 0.1474936 -0.8626881 -0.636475 0.163087 -0.7538581 -0.645197 0.1696269 -0.744948 -0.7708169 0.1599442 -0.6166515 -0.6459589 0.159875 -0.746443 -0.7711567 0.1450923 -0.6198915 -0.8274903 0.1763324 -0.533073 -0.8709936 0.1562899 -0.4657721 -0.9149418 0.1555567 -0.3724025 -0.7441748 0.5367867 -0.3975726 -0.9384292 0.1466302 -0.3128098 -0.9364062 0.1603595 0.3121352 -0.8722791 0.1474033 0.4662632 -0.8271023 0.1767897 0.5335236 -0.7664864 0.1686882 0.6197121 -0.8297607 0.1690638 0.5318973 -0.7716946 0.1450895 0.6192225 -0.6450954 0.1696101 0.7450398 -0.6363618 0.1663306 0.7532449 -0.64563 0.1662972 0.7453235 -0.6361207 0.1630551 0.754164 -0.4856274 0.1477051 0.8615968 -0.4099487 0.177209 0.8947284 -0.2937462 0.1736751 0.9399735 -0.4077602 0.1746891 0.8962228 -0.3145152 0.1452308 0.9380769 -0.1418845 0.1683772 0.9754577 -0.1341822 0.1666406 0.976845 -0.1389042 0.1666364 0.9761855 -0.1267469 0.1627997 0.9784843 0.1395421 0.1371724 0.980669 0.05338537 0.1733645 0.9834097 0.07274723 0.1735997 0.9821257 0.1416881 0.1761806 0.9741073 0.2381073 0.1455035 0.9602779 0.4082419 0.1680009 0.8972816 0.4154286 0.1666471 0.8942303 0.4107152 0.1666655 0.8964015 0.4143497 0.165222 0.894995 0.6486326 0.1363442 0.7487897 0.575868 0.1693211 0.7998164 0.575499 0.1693159 0.8000832 0.6442187 0.1772168 0.7440273 0.7189208 0.1454344 0.6797072 0.8300612 0.16697 0.5320899 0.8296917 0.1670521 0.5326398 0.8285282 0.1670665 0.5344434 0.8312138 0.1651425 0.5308593 0.9521918 0.1337416 0.2746708 0.9171259 0.1644181 0.3631072 0.914055 0.1643574 0.3707966 0.9438654 0.1778166 0.2783871 0.9717582 0.1459009 0.1854698 0.9860588 0.1663975 0 0.9860587 0.1663975 0 0.9860587 0.1663975 0 0.9860587 0.1663975 0 0.9500489 0.1327823 -0.2824464 0.9697747 0.1601643 -0.1840775 0.9693279 0.1601721 -0.1864089 0.9447124 0.1775989 -0.2756393 0.830715 0.1654286 -0.5315507 0.8288937 0.166745 -0.533977 0.7189707 0.1454441 -0.6796524 0.6466634 0.1530798 -0.747257 0.717962 0.1537514 -0.6788895 0.6443232 0.1772074 -0.743939 0.5763537 0.1462174 -0.8040131 0.4070715 0.1568021 -0.8998366 0.5925795 0.1567866 -0.7901059 0.4147536 0.1656929 -0.8947209 0.4098841 0.1681987 -0.8964955 0.2399455 0.1457665 -0.9597803 0.1279237 0.1474865 -0.9807565 0.2369926 0.1489789 -0.9600207 0.1408129 0.1768286 -0.9741167 0.05614864 0.1468989 -0.9875566 0.05065917 0.1469421 -0.987847 -0.1424711 -3.79589e-4 -0.9897989 -0.129338 0 -0.9916005 -0.4158082 0 -0.9094523 -0.4154945 -2.07829e-4 -0.9095956 -0.4121701 0 -0.911107 -0.6546844 0 -0.7559022 -0.6546447 -3.01789e-5 -0.7559368 -0.6543759 0 -0.7561693 -0.840663 0 -0.5415588 -0.8408741 2.12565e-4 -0.5412307 -0.8418785 0 -0.5396671 -0.9599732 0 -0.280092 -0.9596633 -5.69179e-4 -0.2811512 -0.9585365 0 -0.2849697 -1 0 0 -1 0 0 -1 0 0 -0.9591508 0 0.2828954 -0.9596631 -8.33003e-4 0.2811512 -0.9608235 0 0.2771607 -0.8403387 0 0.5420618 -0.840874 -4.25545e-4 0.5412307 -0.8418794 0 0.5396656 -0.6545795 0 0.7559933 -0.6546447 -3.46783e-5 0.7559368 -0.6547469 0 0.7558482 -0.4165412 0 0.9091168 -0.4154945 4.29833e-4 0.9095956 -0.4141279 0 0.9102187 -0.1439396 0 0.9895865 -0.142471 5.09058e-4 0.9897989 -0.1408738 0 0.9900277 0.1408738 0 0.9900277 0.142471 5.09058e-4 0.9897989 0.1439396 0 0.9895865 0.4141279 0 0.9102187 0.4154945 4.29833e-4 0.9095956 0.4165412 0 0.9091168 0.6547469 0 0.7558482 0.6546447 -3.46783e-5 0.7559368 0.6545795 0 0.7559933 0.8418794 0 0.5396656 0.840874 -4.25545e-4 0.5412307 0.8403387 0 0.5420618 0.9608235 0 0.2771607 0.9596631 -8.33003e-4 0.2811512 0.9591508 0 0.2828954 1 0 0 1 0 0 1 0 0 0.9585365 0 -0.2849697 0.9596633 -5.69179e-4 -0.2811512 0.9599732 0 -0.280092 0.8418785 0 -0.5396671 0.8408741 2.12565e-4 -0.5412307 0.840663 0 -0.5415588 0.6543759 0 -0.7561693 0.6546447 -3.01789e-5 -0.7559368 0.6546844 0 -0.7559022 0.4121701 0 -0.911107 0.4154945 -2.07829e-4 -0.9095956 0.4158082 0 -0.9094523 0.129338 0 -0.9916005 0.1424711 -3.79589e-4 -0.9897989 0.1430675 0 -0.9897129 -0.1430675 0 -0.9897129 -1 0 0 -1 0 0 -1 0 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0.9838701 0.1788842 -0.007730364 0.9677938 0.2516258 -0.008649408 0.7070808 0.7070799 0.006879746 0.8947982 0.4464178 0.006871819 0.8719238 0.4895934 0.002814829 0.377206 0.9261251 -0.008792817 0.2516237 0.9677851 0 0.1293374 0.9916006 -0.9629649 0 0.269627 -0.9629649 0 0.269627 -0.251012 0.07028329 0.965429 -0.1845505 0.1845505 0.9653404 -0.07028359 0.2510112 0.9654293 -0.1942211 0.6936423 0.6936414 -0.1935422 0.6935194 0.6939541 -0.5112335 0.5112336 0.690855 -0.5112314 0.5112344 0.690856 -0.6932809 0.1941184 0.6940315 -0.7049736 0.1803423 0.6859219 -0.2607492 0.9343423 0.2429284 -0.243086 0.9349478 0.2584215 -0.682882 0.6828859 0.2594976 -0.6944488 0.6756823 0.2473751 -0.9403539 0.2405559 0.2405565 -0.9297617 0.2603295 0.2603303 -0.2516335 0.9678226 0 -0.2516335 0.9678226 0 -0.7167247 0.6973562 0 -0.7167247 0.6973562 0 -0.962965 0.2696263 0 -0.962965 0.2696263 0 -0.7167251 0 -0.6973558 -0.9629649 0 -0.269627 -0.9629649 0 -0.269627 -0.07028359 0.2510112 -0.9654293 -0.1845506 0.1845506 -0.9653404 -0.2440263 0.938564 -0.2440261 -0.2595868 0.9301772 -0.2595866 -0.6923445 0.673635 -0.2586021 -0.6852509 0.685255 -0.2466917 -0.9343009 0.2616005 -0.2421712 -0.9350127 0.2391895 -0.2618005 -0.1936007 0.6937291 -0.6937282 -0.1941202 0.693282 -0.6940299 -0.5112318 0.5112348 -0.6908552 -0.5112326 0.5112326 -0.6908563 -0.6994224 0.1789222 -0.6919503 -0.7027144 0.1967598 -0.6837237 -0.251012 0.07028329 -0.965429 0 0.1543763 -0.9880121 -0.009795784 0.2516214 -0.9677761 -0.007334709 0.722306 -0.6915347 0.00679636 0.4472005 -0.8944079 0.006780803 0.5067117 -0.8620889 -0.009076535 0.9677828 -0.2516229 0 0.9892032 -0.1465499 -0.2263051 0.7072249 0.6697902 0.3015977 0.696307 0.6513029 0.7167264 0.6973544 0 0.5183387 0.69911 -0.4925142 0.03862148 0.6995103 -0.7135781 -0.1644762 0.9669772 0.1946862 -0.1248043 0.9671578 0.2214269 0.01370066 0.9675313 0.2523798 0.06004744 0.9683741 0.2421693 0.1849305 0.9670732 0.1748434 0.1059339 0.9677575 -0.2285243 -0.1261059 0.966584 -0.2231875 -0.199501 0.9668129 -0.1596003 -0.6712515 0.704275 -0.2311238 -0.4654791 0.8727707 -0.1469718 -0.4844728 0.8612704 -0.1532949 -0.8913083 0.3450198 -0.2941612 -0.915989 0.251125 -0.3128905 -0.944593 0.09943693 -0.3128201 -0.9346077 0.1752427 0.3095133 -0.9163941 0.251105 0.3117182 -0.8494461 0.4497035 0.2760577 -0.6587777 0.7186641 0.2225626 -0.11454 0.9926847 0.03817999 -0.2079164 0.9745261 0.08408081 -0.2403051 0.9673867 0.08010166 -0.2239108 0.9672326 0.119688 -0.2239329 0.9672339 -0.1196353 -0.2369856 0.9674997 -0.08821576 -0.168695 0.984063 -0.05623167 -0.220901 0.9681158 -0.1181289 -0.195109 0.9681603 -0.1568376 -0.1642051 0.9670629 -0.194489 -0.1243403 0.9671458 -0.2217399 -0.03328311 0.9677709 -0.249623 0.01294058 0.9675522 -0.2523397 0.01371669 0.9672559 -0.2534325 0.01418453 0.9682756 -0.2494817 0.05979275 0.9683793 -0.2422114 0.1485582 0.966945 -0.2072386 0.1849181 0.9670735 -0.1748546 0.1827625 0.9676989 -0.173657 0.181624 0.9682638 -0.1716915 0.2103747 0.9683087 -0.1346127 0.2516335 0.9678226 0 0.2516335 0.9678226 0 0.2516335 0.9678227 0 0.2469634 0.9678903 -0.04687726 0.2482241 0.9675434 0.04737603 0.2343087 0.9675045 0.09504997 0.2367252 0.9670455 0.09372389 0.21423 0.9671535 0.1368191 0.1064419 0.9675605 0.2291214 0.1058055 0.9677801 0.2284879 0.1058276 0.9677543 0.2285876 0.1467888 0.9679301 0.2038734 -0.0766099 0.966454 0.245148 -0.08143335 0.9670985 0.2410164 -0.07958346 0.9681548 0.2373665 -0.03413015 0.9680389 0.2484672 -0.1947117 0.9681448 0.1574264 -0.2211804 0.9681038 0.1177042 -0.6276769 0.7031698 0.3340269 -0.6718416 0.7042246 0.2295574 -0.8316648 0.4851421 0.270131 -0.224141 0.7071188 -0.6706294 -0.08158397 0.9665949 -0.242978 -0.08118349 0.9673035 -0.240277 -0.230294 0.6945442 -0.6815959 -0.09658795 0.6949279 -0.7125629 -0.1300739 0.2494957 -0.9596003 0.05087167 0.2513077 -0.9665695 0.3019366 0.6962896 -0.6511644 0.06135189 0.9674767 -0.2454073 0.1061488 0.9675673 -0.2292293 0.3024179 0.6942894 -0.653074 0.4201316 0.6941274 -0.584531 0.5638182 0.2583763 -0.7844429 0.7015674 0.2590524 -0.6638488 0.657499 0.7065963 -0.2615662 0.6635753 0.6994388 -0.2654304 0.7016292 0.6998641 -0.133816 0.5189352 0.6991608 0.4918134 0.2101477 0.9683169 0.1349092 0.1812267 0.9682484 0.1721977 0.5147917 0.7040795 0.4891439 0.4141255 0.7046437 0.5761747 0.562515 0.2665852 0.7826297 0.4170309 0.2669032 0.8688198 0.03983807 0.6995611 0.7134614 0.01843094 0.9683725 0.2488275 0.01364505 0.9676027 0.2521088 0.03827118 0.7060714 0.7071057 -0.09523808 0.7051784 0.7026044 -0.1298174 0.2567928 0.9577082 -0.305629 0.2567279 0.916887 -0.4603919 0.7019324 0.5434429 -0.1641054 0.967066 0.194558 -0.164548 0.966937 0.1948249 -0.463243 0.6961148 0.5484799 -0.5580259 0.6967077 0.4507831 -0.8506854 0.2602313 -0.4567428 -0.2229626 0.9675791 -0.1186526 -0.6337479 0.6961472 -0.3372576 -0.6271286 0.7031058 -0.3351895 -0.8501825 0.2659004 -0.454408 -0.8554773 0.2506282 -0.4531491 -0.7517282 0.2706233 -0.6013882 -0.1978173 0.9671215 -0.159826 -0.5579887 0.6967101 -0.4508255 -0.5582559 0.6964031 -0.4509689 -0.7490752 0.2696682 -0.6051161 -0.7513393 0.2621917 -0.6055946 -0.1644515 0.9669765 -0.1947106 -0.4595423 0.7019816 -0.5440982 -0.464033 0.6961706 -0.5477408 -0.6238481 0.261821 -0.7363854 -0.621176 0.2712791 -0.7352197 -0.1242595 0.9673138 -0.221051 -0.3465486 0.7069951 -0.6164916 -0.3499536 0.7017597 -0.6205367 -0.4731478 0.2687683 -0.8389843 -0.4767771 0.2609524 -0.839397 -0.1288474 0.2567702 -0.9578452 -0.305629 0.2567279 -0.9168869 -0.3062461 0.2581284 -0.9162877 -0.3065935 0.2567667 -0.916554 -0.4726419 0.2588881 -0.8423696 0.05413478 0.2532923 -0.965874 0.05391412 0.2545722 -0.9655498 0.03948223 0.7060217 -0.7070888 -0.09333491 0.7052802 -0.7027577 -0.03303414 0.9680098 -0.2487279 -0.07829689 0.9681807 -0.2376884 0.06135183 0.9674766 -0.2454078 0.1740778 0.696309 -0.6963121 0.1739403 0.6986982 -0.6939493 0.2341818 0.2688215 -0.9342879 0.2314112 0.2534631 -0.9392579 0.5634199 0.2665578 -0.7819878 0.4170309 0.2669032 -0.8688198 0.4046592 0.2732187 -0.8726983 0.4044461 0.2603067 -0.8767347 0.2365246 0.2621741 -0.9355859 0.7046238 0.2473371 -0.6650787 0.7013934 0.2572429 -0.6647355 0.5154538 0.7040323 -0.488514 0.415661 0.7047259 -0.5749672 0.1455301 0.9686573 -0.2013059 0.15098 0.9678227 -0.2013067 0.2147524 0.9670686 -0.1366009 0.5968894 0.7068039 -0.3796725 0.6017773 0.7001448 -0.3842673 0.8154708 0.2526968 -0.5207222 0.8161497 0.2468252 -0.5224721 0.9629638 0.2696306 0 0.9449788 0.2718722 -0.1819358 0.9482918 0.2608299 -0.18086 0.9482879 0.261008 -0.1806234 0.8967889 0.2617092 -0.3567605 0.8974719 0.258296 -0.3575298 0.8130294 0.2577725 -0.5220504 0.9629638 0.2696306 0 0.9629638 0.2696306 0 0.7167264 0.6973544 0 0.70467 0.6965711 -0.1350152 0.2486808 0.9674128 -0.04764741 0.2481271 0.9675516 -0.04771661 0.9482879 0.261008 0.1806234 0.2469177 0.9678809 0.04730957 0.7014893 0.6998912 0.1344058 0.7047534 0.6966033 0.1344119 0.9482918 0.2608299 0.18086 0.9449788 0.2718721 0.1819358 0.2322279 0.9682156 0.09289127 0.6569463 0.7066604 0.2627788 0.6640112 0.699507 0.2641569 0.8976452 0.2582861 0.3571012 0.8966205 0.2616998 0.3571906 0.2123748 0.9678058 0.1350885 0.6024934 0.7000937 0.3832371 0.59627 0.7067469 0.3807505 0.8154708 0.2526969 0.5207222 0.8130294 0.2577725 0.5220504 0.5647161 0.2584066 0.7837867 0.7015674 0.2590524 0.6638488 0.7013934 0.2572429 0.6647355 0.7046238 0.2473371 0.6650787 0.8161497 0.2468252 0.5224721 0.4044461 0.2603067 0.8767347 0.4046592 0.2732187 0.8726983 0.3027438 0.6943063 0.6529051 0.4217932 0.6940296 0.5834496 0.1488554 0.9671839 0.2059057 0.1488695 0.9669557 0.2069647 0.06135171 0.9674767 0.2454074 0.1735082 0.6987219 0.6940336 0.1745008 0.6963297 0.6961856 0.2341818 0.2688215 0.9342879 0.2365246 0.2621741 0.9355859 -0.1291 0.2495277 0.9597235 0.05087167 0.2513077 0.9665695 0.05391412 0.2545722 0.9655498 0.05413478 0.2532923 0.965874 0.2314112 0.2534631 0.9392579 -0.3065935 0.2567667 0.916554 -0.3062461 0.2581284 0.9162877 -0.2280214 0.6946619 0.6822397 -0.09468263 0.6948394 0.7129049 -0.03354847 0.9669889 0.2526004 -0.03234249 0.9677872 0.2496837 -0.1255912 0.9665973 0.22342 -0.3491178 0.7017115 0.6210619 -0.3474251 0.7069473 0.616053 -0.4731478 0.2687683 0.8389843 -0.4726419 0.2588881 0.8423696 -0.8506854 0.2602313 0.4567428 -0.7513393 0.2621915 0.6055946 -0.7490752 0.2696682 0.6051161 -0.7517282 0.2706233 0.6013882 -0.6221522 0.2713071 0.7343835 -0.6228598 0.2618451 0.7372128 -0.4767771 0.2609524 0.839397 -0.8554773 0.2506282 0.4531491 -0.8501825 0.2659004 0.454408 -0.6330834 0.6962109 0.3383722 -0.5582225 0.6964006 0.4510145 -0.1946848 0.9681715 0.1572951 -0.1992351 0.9668231 0.15987 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0.2576645 0 -0.9662344 -0.05074721 0 -0.9987115 -0.05074721 0 -0.9987115 -0.3301701 0 -0.9439214 -0.3301701 0 -0.9439214 0.2314149 -0.04166501 -0.9719626 0.2337992 -0.0328713 -0.9717291 -0.2353472 -0.01319801 0.9718218 0.2360719 0 -0.9717356 0.498471 0 -0.8669065 0.5025548 0.05025458 -0.8630836 0.4977039 -0.0518344 -0.8657968 0.5000953 0.04980254 -0.8645372 0.5044022 0 -0.8634689 0.7249996 0 -0.6887493 0.7241208 -0.006897449 -0.6896386 0.8897777 0 -0.4563943 0.8898923 -0.008304476 -0.4560952 0.8900858 0 -0.4557931 0.7240293 0 -0.6897691 -0.9833266 0 0.1818479 -0.9818185 -0.044227 0.1845983 0.9825241 -0.002599418 -0.1861173 0.9825702 0 -0.1858918 0.6556872 0 0.7550326 -0.6556872 0 0.7550326 -0.6515577 -0.04579263 0.7572157 -0.6516571 -0.04426085 0.7572213 -0.6510875 0 0.7590027 -0.8414006 0 0.540412 -0.8395078 -0.03620713 0.54214 -0.8394931 0 0.5433704 -0.9587494 0 0.2842525 -0.9582376 0.04984301 0.2815961 -0.9599216 0 0.2802686 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -0.9598937 0 -0.2803642 -0.9588532 0.03467482 -0.281777 -0.9590831 0 -0.2831244 -0.8426965 0 -0.5383889 0.8393291 -0.04166615 0.5420246 0.8395044 0 0.5433529 0.6522963 0 0.757964 0.6490944 0.0781871 0.7566791 0.6515577 0.04579263 0.7572157 0.8899229 0 0.4561109 0.9838697 0 0.1788867 0.9821668 0.01870387 0.187079 0.9826549 -0.003704786 0.1854068 0.9822553 -0.03267395 0.1846804 0.9842713 0 0.1766641 0.8899229 0 0.4561109 0.7249996 0 0.6887493 0.7241208 -0.006897449 0.6896386 0.7232667 0.02902907 0.6899583 0.7348049 0 0.6782786 0.5044022 0 0.8634689 0.4990119 0.0604375 0.864485 0.5005733 0.02391499 0.8653637 0.498471 0 0.8669065 0.2360719 0 0.9717356 0.2353472 0.01319825 0.9718218 0.2337992 -0.03287124 0.9717291 0.2314148 -0.04166501 0.9719625 0.2576645 0 0.9662344 -0.05074721 0 0.9987115 -0.05074721 0 0.9987115 -0.3301701 0 0.9439214 0.3301701 0 -0.9439214 0 -1 0 0 -1 0 0 -1 0 0 0.03485059 -0.9993925 0.9993925 0.03485059 0 1 0 0 0 0 -1 -0.260666 0 0.9654291 -0.9654291 0 -0.260666 -0.9654291 0 -0.260666 -0.260666 0 -0.9654291 -0.9645484 0.04270619 -0.2604277 -0.9646556 0.04270696 -0.2600302 0.9654291 0 -0.260666 0.2604277 0.04270619 -0.9645484 0.7071068 0 0.7071068 0.2689354 0.9604846 -0.07171601 0.2758985 0.9581687 -0.07611 0.2031767 0.95783 -0.2031767 0.2031767 0.95783 -0.2031767 0.07389479 0.9579936 -0.2771059 0.07417577 0.9603113 -0.2688869 0.6940287 0.6940234 -0.1914564 0.6821739 0.707958 -0.1828504 0.4946855 0.7145437 -0.4946855 0.4946855 0.7145437 -0.4946855 0.1876068 0.7087336 -0.6800739 0.1862189 0.6947355 -0.694741 0.9324051 0.2610736 -0.2499225 0.9336825 0.2543331 -0.2520944 0.6852533 0.2466892 -0.6852533 0.6852534 0.2466892 -0.6852534 0.2503941 0.2542446 -0.9341641 0.2516342 0.2609541 -0.9319781 0 0 0 0 0.7071041 -0.7071095 0 0.7071041 -0.7071095 0 0.2696301 -0.9629639 0 0.2696301 -0.9629639 0.2696297 0.9629641 0 0.2696297 0.9629641 0 0.2696297 0.9629641 0 0.7071095 0.7071041 0 0.7071095 0.7071041 0 0.9629639 0.2696301 0 0.9629639 0.2696301 0 -0.07611 0.9581687 -0.2758985 -0.2031767 0.95783 -0.2031767 -0.2031767 0.95783 -0.2031767 -0.2771059 0.9579936 -0.07389479 -0.2688869 0.9603113 -0.07417577 -0.1914564 0.6940234 -0.6940287 -0.1828504 0.7079579 -0.6821739 -0.4946855 0.7145437 -0.4946855 -0.4946855 0.7145437 -0.4946855 -0.6800737 0.7087335 -0.1876068 -0.694741 0.6947355 -0.1862189 -0.2499225 0.2610736 -0.9324051 -0.2520944 0.2543331 -0.9336825 -0.6852533 0.2466892 -0.6852533 -0.6852534 0.2466892 -0.6852534 -0.9341641 0.2542446 -0.2503941 -0.9319781 0.2609541 -0.2516342 0.07171601 0.9604846 0.2689354 0.07611 0.9581687 0.2758985 0.2031767 0.95783 0.2031767 0.2031767 0.95783 0.2031767 0.2771059 0.9579936 0.07389479 0.2688869 0.9603113 0.07417577 0.1914564 0.6940234 0.6940287 0.1828504 0.7079579 0.6821739 0.4946855 0.7145437 0.4946855 0.4946855 0.7145437 0.4946855 0.6800737 0.7087335 0.1876068 0.694741 0.6947355 0.1862189 0.2499225 0.2610736 0.9324051 0.2520944 0.2543331 0.9336825 0.6852533 0.2466892 0.6852533 0.6852534 0.2466892 0.6852534 0.9341641 0.2542446 0.2503941 0.9319781 0.2609541 0.2516342 -0.2696297 0.9629641 0 -0.2696297 0.9629641 0 -0.2696297 0.9629641 0 -0.7071095 0.7071041 0 -0.7071095 0.7071041 0 -0.9629639 0.2696301 0 -0.9629639 0.2696301 0 0 0.9629641 0.2696297 0 0.9629641 0.2696297 0 0.9629641 0.2696297 0 0.7071041 0.7071095 0 0.7071041 0.7071095 0 0.2696301 0.9629639 0 0.2696301 0.9629639 -0.2689354 0.9604846 0.07171601 -0.2758985 0.9581687 0.07611 -0.2031767 0.95783 0.2031767 -0.2031767 0.95783 0.2031767 -0.07389479 0.9579936 0.2771059 -0.07417577 0.9603113 0.2688869 -0.6940287 0.6940234 0.1914564 -0.6821739 0.707958 0.1828504 -0.4946855 0.7145437 0.4946855 -0.4946855 0.7145437 0.4946855 -0.1876068 0.7087336 0.6800739 -0.1862189 0.6947355 0.694741 -0.9324051 0.2610736 0.2499225 -0.9336825 0.2543331 0.2520944 -0.6852533 0.2466892 0.6852533 -0.6852534 0.2466892 0.6852534 -0.2503941 0.2542446 0.9341641 -0.2516342 0.2609541 0.9319781 -0.9629639 0 -0.2696301 -0.9629639 0 -0.2696301 -0.7167251 0 -0.6973558 -0.7167237 -2.52567e-7 -0.6973572 -0.2516344 0 -0.9678224 -0.2516335 0 -0.9678226 -0.2516344 0 -0.9678224 -0.2516344 0 0.9678224 -0.2516344 0 0.9678224 -0.7167251 0 0.6973558 -0.7167251 0 0.6973558 -0.9629639 0 0.2696301 -0.9629639 0 0.2696301 -1 0 0 -1 0 0 0 0 1 0 0 -1 0 0 -1 -1 0 0 -1 0 0 -0.9122474 0 0.4096398 -0.8835567 -0.01679629 -0.4680228 -0.957763 0.01149314 -0.2873293 -0.957763 0.01149314 0.2873293 -0.8835567 -0.01679629 0.4680228 -0.9086417 0.007449328 0.4175103 -0.881016 0.007450222 0.4730277 -0.8049004 -0.01266139 0.5932749 -0.9122221 0.007446944 -0.4096284 -0.901296 0 -0.4332039 0 0 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 -0.9190827 0.28875 -0.2681613 -0.9068856 0.3034356 -0.292379 -0.9189138 0.2091057 0.3344731 0.9175001 0.1444488 -0.3705781 0.8302804 0.1654177 -0.5322325 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0 1 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 -0.2516344 0 0.9678224 0.002348303 0.6348381 0.7726417 -0.7167251 0 0.6973558 -0.7167251 0 0.6973558 -0.2516344 0 0.9678224 -0.2516344 0 0.9678224 -0.2516344 0 -0.9678224 -0.2516344 0 -0.9678224 -0.7167251 0 -0.6973558 -0.004324674 0.7071006 -0.7070997 0.003669202 0.9127396 -0.4085254 0.2344187 0.967514 -0.09468168 -0.7256583 0.6450302 -0.2394912 -0.3975354 0.9086532 0.1277299 0.2290577 0.9690912 -0.09162318 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 0 0 0 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 0 0 0 0 0 0.03485059 -0.9993925 0.9993925 0.03485059 0 -0.9993925 0.03485059 0 -0.9993925 0.03485059 0 0 0 1 0 0 1 1 0 0 0 0 -1 -1 0 0 -1 0 0 -0.7071068 0 0.7071068 -0.7071068 0 0.7071068 -0.260666 0 0.9654291 -0.9654291 0 0.260666 -0.9654291 0 0.260666 -0.9646556 0.04270696 0.2600302 -0.9645484 0.04270619 0.2604277 -0.706249 0.04924374 0.7062489 -0.7062489 0.04924374 0.7062489 -0.2604277 0.04270619 0.9645484 -0.2600302 0.04270696 0.9646556 -0.7071068 0 -0.7071068 -0.7071068 0 -0.7071068 -0.260666 0 -0.9654291 -0.2600301 0.04270696 -0.9646555 -0.2604277 0.04270619 -0.9645484 -0.7062489 0.04924374 -0.706249 -0.7062489 0.04924374 -0.7062489 0.260666 0 -0.9654291 0.7071068 0 -0.7071068 0.7071068 0 -0.7071068 0.260666 0 -0.9654291 0.9654291 0 -0.260666 0.9646556 0.04270696 -0.2600302 0.9645484 0.04270619 -0.2604277 0.706249 0.04924374 -0.7062489 0.7062489 0.04924374 -0.7062489 0.2600302 0.04270696 -0.9646556 0.2604278 0.04270619 0.9645484 0.2600301 0.04270696 0.9646556 0.9645479 0.04271709 0.2604277 0.964656 0.04269605 0.2600303 0.7062489 0.04924374 0.7062489 0.706249 0.04924374 0.7062489 0.7071068 0 0.7071068 0.9654291 0 0.260666 0.9654291 0 0.260666 0.260666 0 0.9654291 0.260666 0 0.9654291 0 0 0 0 0 0 -1.5404e-7 1 -6.36081e-7 3.24032e-7 1 5.61656e-7 6.54708e-7 1 0 -6.48629e-7 1 0 -5.82531e-7 1 2.98564e-7 4.26935e-7 1 4.96095e-7 -3.27791e-7 1 5.66667e-7 6.28108e-7 1 1.84581e-7 -1.5404e-7 1 6.36081e-7 5.49908e-7 1 -3.55121e-7 5.49908e-7 1 3.55121e-7 -4.74061e-7 1 -4.51486e-7 0 1 6.53629e-7 2.16121e-7 1 -6.17868e-7 3.24032e-7 1 -5.61656e-7 -5.82531e-7 1 -2.98564e-7 -4.74061e-7 1 4.51486e-7 4.26935e-7 1 -4.96095e-7 -3.27791e-7 1 -5.66667e-7 -6.43363e-7 1 1.20963e-7 2.16121e-7 1 6.17868e-7 6.28108e-7 1 -1.84581e-7 -6.43363e-7 1 -1.20963e-7 0 1 -6.53629e-7 0 0.9629641 -0.2696297 0 0.9629641 -0.2696297 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.1872387 -0.9823144 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.1364855 -0.9817008 -0.1327975 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.08765232 -0.9373711 -0.3371237 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.1734797 -0.9848374 0 0 0 0 0 0 0 0 0 0 -0.1734797 -0.9848374 0 0 0 0 0 0 0 0 0.9629641 -0.2696297 0 0 0 -0.07171601 0.9604846 -0.2689354 -0.08764123 -0.9899891 0.1106374 0.07545632 -0.9970984 0.01005166 -0.165484 -0.9851234 -0.04633498 0.06710433 -0.9963871 -0.05205571 -0.01855289 -0.9915714 -0.1282264 -0.1596735 -0.9861569 0.0447086 0.01166564 -0.9952132 0.09702873 0.1506127 0.98234 0.1110134 0.01357448 -0.9934884 -0.1131218 0 0 0 -0.1199531 -0.9858954 0.1167114 0.0334047 -0.9960209 0.08262425 0.07395762 -0.9967855 -0.03080356 -0.1550459 -0.986953 0.04341232 -0.04902708 -0.9923275 0.1135011 -0.05939245 -0.9717465 0.2284322 0.01359909 -0.9934894 -0.1131098 0 0 0 -0.1297165 -0.9869306 0.09561127 0.05006933 -0.9965522 0.06615769 0.07658731 -0.9970107 -0.01020228 -0.1044685 -0.9857454 -0.1318801 0.1044685 0.9857454 0.1318801 0 -0.9662557 0.2575849 -0.01562213 -0.9940313 0.1079711 -0.1443563 -0.9795069 -0.1404551 0.03801149 -0.9948446 -0.09401881 0.06233876 -0.9968829 0.04835891 -0.05876457 -0.9889584 -0.136044 -0.05089199 -0.9793348 0.195738 0.01164436 -0.9952126 0.09703755 -0.1872387 -0.9823144 0 0.05545455 -0.995769 -0.0732733 -0.1144365 -0.9871711 0.1113438 0.07075601 -0.9970582 0.02947002 -0.1506127 -0.98234 -0.1110134 0.05876457 0.9889584 0.136044 -0.1603716 -0.9844262 0.07201397 -0.01855289 -0.9915714 -0.1282265 0 0.03485065 0.9993925 0 0.03485059 0.9993926 0 0.03485053 0.9993925 0 0.03485059 0.9993926 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 -0.1707559 -0.9841526 -0.04781168 0.070692 0.9597275 0.2718927 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.1790391 0.9805517 0.08039653 -0.1790391 -0.9805517 -0.08039653 0 -0.9156407 -0.4019977 + + + + + + + + + + + + + + + 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 4 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 4 4 4 4 4 4 3 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 3 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 4 3 4 4 4 4 4 4 4 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 +

376 0 371 0 375 0 325 1 326 1 329 1 161 2 75 2 120 2 122 3 76 3 284 3 277 4 404 4 278 4 278 5 404 5 280 5 280 6 404 6 406 6 280 7 406 7 282 7 408 8 275 8 406 8 406 9 275 9 281 9 406 10 281 10 282 10 410 11 259 11 408 11 408 12 259 12 266 12 408 13 266 13 275 13 414 14 263 14 264 14 414 15 264 15 412 15 412 16 264 16 262 16 412 17 262 17 410 17 410 18 262 18 260 18 410 19 260 19 259 19 263 20 414 20 257 20 257 21 414 21 416 21 257 22 416 22 248 22 248 23 416 23 241 23 241 24 416 24 418 24 241 25 418 25 242 25 420 26 246 26 418 26 418 27 246 27 244 27 418 28 244 28 242 28 230 29 234 29 423 29 423 30 234 30 239 30 423 31 239 31 420 31 420 32 239 32 245 32 420 33 245 33 246 33 230 34 423 34 229 34 229 35 423 35 422 35 229 36 422 36 221 36 221 37 422 37 214 37 214 38 422 38 421 38 214 39 421 39 215 39 215 40 421 40 217 40 217 41 421 41 419 41 217 42 419 42 219 42 417 43 212 43 419 43 419 44 212 44 218 44 419 45 218 45 219 45 199 46 197 46 415 46 415 47 197 47 196 47 415 48 196 48 417 48 417 49 196 49 203 49 417 50 203 50 212 50 199 51 415 51 201 51 201 52 415 52 413 52 201 53 413 53 200 53 200 54 413 54 194 54 194 55 413 55 411 55 194 56 411 56 185 56 185 57 411 57 409 57 185 58 409 58 178 58 178 59 409 59 407 59 178 60 407 60 179 60 405 61 183 61 407 61 407 62 183 62 181 62 407 63 181 63 179 63 167 64 171 64 403 64 403 65 171 65 176 65 403 66 176 66 405 66 405 67 176 67 182 67 405 68 182 68 183 68 167 69 403 69 166 69 166 70 403 70 402 70 166 71 402 71 162 71 84 72 76 72 122 72 122 73 123 73 84 73 84 74 123 74 150 74 84 75 150 75 151 75 151 76 124 76 84 76 126 77 125 77 84 77 84 78 125 78 27 78 84 79 27 79 82 79 118 80 117 80 107 80 107 81 106 81 28 81 120 82 107 82 121 82 121 83 107 83 119 83 118 84 107 84 130 84 130 85 107 85 28 85 130 86 28 86 129 86 41 87 40 87 127 87 31 88 30 88 173 88 114 89 74 89 36 89 28 90 106 90 34 90 108 91 35 91 110 91 110 92 35 92 34 92 110 93 34 93 109 93 109 94 34 94 106 94 108 95 111 95 35 95 36 96 116 96 114 96 78 97 40 97 80 97 80 98 40 98 79 98 40 99 81 99 83 99 125 100 127 100 27 100 27 101 127 101 40 101 27 102 40 102 82 102 82 103 40 103 83 103 79 104 40 104 77 104 77 105 40 105 39 105 77 106 39 106 73 106 73 107 39 107 38 107 73 108 38 108 74 108 74 109 38 109 37 109 74 110 37 110 36 110 173 111 30 111 152 111 152 112 30 112 29 112 152 113 29 113 188 113 188 114 29 114 71 114 188 115 71 115 190 115 190 116 71 116 72 116 190 117 72 117 134 117 173 118 133 118 31 118 31 119 133 119 169 119 31 120 169 120 32 120 32 121 169 121 164 121 32 122 164 122 33 122 33 123 164 123 132 123 33 124 132 124 34 124 34 125 132 125 159 125 34 126 159 126 28 126 28 127 159 127 131 127 28 128 131 128 129 128 127 129 128 129 41 129 41 130 128 130 148 130 41 131 148 131 42 131 42 132 148 132 291 132 42 133 291 133 43 133 43 134 291 134 289 134 43 135 289 135 44 135 44 136 289 136 158 136 44 137 158 137 45 137 158 138 157 138 45 138 45 139 157 139 272 139 45 140 272 140 46 140 46 141 272 141 146 141 46 142 146 142 47 142 47 143 146 143 147 143 47 144 147 144 48 144 48 145 147 145 271 145 48 146 271 146 49 146 49 147 271 147 269 147 49 148 269 148 50 148 269 149 156 149 50 149 50 150 156 150 155 150 50 151 155 151 51 151 51 152 155 152 254 152 51 153 254 153 52 153 52 154 254 154 144 154 52 155 144 155 53 155 53 156 144 156 145 156 53 157 145 157 54 157 145 158 253 158 54 158 54 159 253 159 251 159 54 160 251 160 55 160 55 161 251 161 154 161 55 162 154 162 56 162 56 163 154 163 236 163 56 164 236 164 57 164 57 165 236 165 143 165 57 166 143 166 58 166 143 167 232 167 58 167 58 168 232 168 142 168 58 169 142 169 59 169 59 170 142 170 227 170 59 171 227 171 60 171 60 172 227 172 140 172 60 173 140 173 61 173 61 174 140 174 141 174 61 175 141 175 62 175 141 176 226 176 62 176 62 177 226 177 224 177 62 178 224 178 63 178 64 179 209 179 139 179 64 180 139 180 65 180 65 181 139 181 138 181 65 182 138 182 66 182 66 183 138 183 137 183 66 184 137 184 67 184 67 185 137 185 208 185 67 186 208 186 68 186 208 187 206 187 68 187 68 188 206 188 153 188 68 189 153 189 69 189 69 190 153 190 191 190 69 191 191 191 70 191 70 192 191 192 136 192 70 193 136 193 72 193 72 194 136 194 135 194 72 195 135 195 134 195 71 196 308 196 309 196 71 197 29 197 308 197 308 198 29 198 30 198 308 199 30 199 292 199 30 200 31 200 292 200 292 201 31 201 32 201 292 202 32 202 307 202 32 203 33 203 307 203 307 204 33 204 34 204 307 205 34 205 306 205 34 206 35 206 306 206 306 207 35 207 36 207 306 208 36 208 305 208 36 209 37 209 305 209 305 210 37 210 38 210 305 211 38 211 304 211 38 212 39 212 304 212 304 213 39 213 40 213 304 214 40 214 303 214 40 215 41 215 303 215 303 216 41 216 42 216 303 217 42 217 302 217 42 218 43 218 302 218 302 219 43 219 44 219 302 220 44 220 301 220 44 221 45 221 301 221 301 222 45 222 46 222 301 223 46 223 293 223 46 224 47 224 293 224 293 225 47 225 48 225 293 226 48 226 294 226 48 227 49 227 294 227 294 228 49 228 50 228 294 229 50 229 295 229 50 230 51 230 295 230 295 231 51 231 52 231 295 232 52 232 296 232 52 233 53 233 296 233 296 234 53 234 54 234 296 235 54 235 297 235 54 236 55 236 297 236 297 237 55 237 56 237 297 238 56 238 298 238 56 239 57 239 298 239 298 240 57 240 58 240 298 241 58 241 299 241 58 242 59 242 299 242 299 243 59 243 60 243 299 244 60 244 300 244 60 245 61 245 300 245 300 246 61 246 62 246 300 247 62 247 313 247 62 248 63 248 313 248 313 249 63 249 64 249 313 250 64 250 312 250 64 251 65 251 312 251 312 252 65 252 66 252 312 253 66 253 311 253 66 254 67 254 311 254 311 255 67 255 68 255 311 256 68 256 310 256 68 257 69 257 310 257 310 258 69 258 70 258 310 259 70 259 309 259 309 260 70 260 72 260 309 261 72 261 71 261 399 262 95 262 398 262 398 263 95 263 99 263 398 264 99 264 397 264 93 265 77 265 73 265 93 266 73 266 115 266 115 267 73 267 74 267 115 268 74 268 114 268 77 269 93 269 79 269 79 270 93 270 89 270 85 271 78 271 89 271 89 272 78 272 80 272 89 273 80 273 79 273 81 274 85 274 83 274 83 275 85 275 84 275 83 276 84 276 82 276 95 277 399 277 401 277 95 278 401 278 92 278 84 279 87 279 88 279 86 280 87 280 84 280 85 281 86 281 84 281 85 282 89 282 86 282 86 283 89 283 90 283 86 284 90 284 87 284 87 285 90 285 91 285 87 286 91 286 88 286 88 287 91 287 92 287 89 288 93 288 90 288 90 289 93 289 94 289 90 290 94 290 91 290 91 291 94 291 96 291 91 292 96 292 92 292 92 293 96 293 95 293 93 294 115 294 97 294 93 295 97 295 94 295 94 296 97 296 98 296 94 297 98 297 96 297 96 298 98 298 99 298 96 299 99 299 95 299 105 300 400 300 103 300 103 301 400 301 397 301 103 302 397 302 99 302 101 303 112 303 107 303 101 304 107 304 104 304 115 305 113 305 97 305 97 306 113 306 100 306 97 307 100 307 98 307 98 308 100 308 102 308 98 309 102 309 99 309 99 310 102 310 103 310 113 311 112 311 100 311 100 312 112 312 101 312 100 313 101 313 102 313 102 314 101 314 104 314 102 315 104 315 103 315 103 316 104 316 105 316 105 317 104 317 107 317 106 318 107 318 109 318 109 319 107 319 112 319 111 320 108 320 112 320 112 321 108 321 110 321 112 322 110 322 109 322 116 323 113 323 115 323 116 324 115 324 114 324 268 325 274 325 276 325 250 326 256 326 258 326 223 327 228 327 231 327 205 328 211 328 213 328 187 329 193 329 195 329 158 330 289 330 290 330 272 331 157 331 273 331 156 332 269 332 270 332 254 333 155 333 255 333 154 334 251 334 252 334 153 335 206 335 207 335 173 336 152 336 174 336 164 337 169 337 170 337 163 338 117 338 160 338 160 339 117 339 118 339 160 340 118 340 130 340 119 341 163 341 121 341 121 342 163 342 161 342 121 343 161 343 120 343 122 344 284 344 123 344 123 345 284 345 286 345 123 346 286 346 150 346 149 347 124 347 151 347 125 348 126 348 127 348 127 349 126 349 149 349 127 350 149 350 128 350 128 351 149 351 148 351 159 352 160 352 131 352 131 353 160 353 130 353 131 354 130 354 129 354 159 355 132 355 165 355 165 356 132 356 164 356 169 357 133 357 175 357 175 358 133 358 173 358 190 359 134 359 187 359 187 360 134 360 135 360 187 361 135 361 193 361 193 362 135 362 136 362 193 363 136 363 191 363 208 364 137 364 205 364 205 365 137 365 138 365 205 366 138 366 211 366 211 367 138 367 139 367 211 368 139 368 209 368 227 369 228 369 140 369 140 370 228 370 223 370 140 371 223 371 141 371 141 372 223 372 226 372 227 373 142 373 233 373 233 374 142 374 232 374 232 375 143 375 238 375 238 376 143 376 236 376 254 377 256 377 144 377 144 378 256 378 250 378 144 379 250 379 145 379 145 380 250 380 253 380 272 381 274 381 146 381 146 382 274 382 268 382 146 383 268 383 147 383 147 384 268 384 271 384 291 385 148 385 288 385 288 386 148 386 149 386 288 387 149 387 286 387 286 388 149 388 151 388 286 389 151 389 150 389 180 390 184 390 174 390 152 391 188 391 174 391 174 392 188 392 189 392 174 393 189 393 180 393 180 394 189 394 186 394 180 395 186 395 178 395 178 396 186 396 185 396 198 397 202 397 192 397 191 398 153 398 192 398 192 399 153 399 207 399 192 400 207 400 198 400 198 401 207 401 204 401 198 402 204 402 196 402 196 403 204 403 203 403 216 404 220 404 210 404 210 405 225 405 216 405 216 406 225 406 222 406 243 407 247 407 237 407 236 408 154 408 237 408 237 409 154 409 252 409 237 410 252 410 243 410 243 411 252 411 249 411 243 412 249 412 241 412 241 413 249 413 248 413 261 414 265 414 255 414 155 415 156 415 255 415 255 416 156 416 270 416 255 417 270 417 261 417 261 418 270 418 267 418 261 419 267 419 259 419 259 420 267 420 266 420 279 421 283 421 273 421 157 422 158 422 273 422 273 423 158 423 290 423 273 424 290 424 279 424 279 425 290 425 287 425 168 426 166 426 162 426 159 427 165 427 160 427 160 428 165 428 168 428 160 429 168 429 163 429 163 430 168 430 162 430 163 431 162 431 161 431 172 432 171 432 167 432 164 433 170 433 165 433 165 434 170 434 172 434 165 435 172 435 168 435 168 436 172 436 167 436 168 437 167 437 166 437 169 438 175 438 170 438 170 439 175 439 177 439 170 440 177 440 172 440 172 441 177 441 176 441 172 442 176 442 171 442 173 443 174 443 175 443 175 444 174 444 184 444 175 445 184 445 177 445 177 446 184 446 182 446 177 447 182 447 176 447 178 448 179 448 180 448 180 449 179 449 181 449 180 450 181 450 184 450 184 451 181 451 183 451 184 452 183 452 182 452 194 453 185 453 195 453 195 454 185 454 186 454 195 455 186 455 187 455 187 456 186 456 189 456 187 457 189 457 190 457 190 458 189 458 188 458 191 459 192 459 193 459 193 460 192 460 202 460 193 461 202 461 195 461 195 462 202 462 200 462 195 463 200 463 194 463 196 464 197 464 198 464 198 465 197 465 199 465 198 466 199 466 202 466 202 467 199 467 201 467 202 468 201 468 200 468 212 469 203 469 213 469 213 470 203 470 204 470 213 471 204 471 205 471 205 472 204 472 207 472 205 473 207 473 208 473 208 474 207 474 206 474 209 475 210 475 211 475 211 476 210 476 220 476 211 477 220 477 213 477 213 478 220 478 218 478 213 479 218 479 212 479 221 480 214 480 222 480 222 481 214 481 215 481 222 482 215 482 216 482 216 483 215 483 217 483 216 484 217 484 220 484 220 485 217 485 219 485 220 486 219 486 218 486 229 487 221 487 231 487 231 488 221 488 222 488 231 489 222 489 223 489 223 490 222 490 225 490 223 491 225 491 226 491 226 492 225 492 224 492 235 493 234 493 230 493 227 494 233 494 228 494 228 495 233 495 235 495 228 496 235 496 231 496 231 497 235 497 230 497 231 498 230 498 229 498 232 499 238 499 233 499 233 500 238 500 240 500 233 501 240 501 235 501 235 502 240 502 239 502 235 503 239 503 234 503 236 504 237 504 238 504 238 505 237 505 247 505 238 506 247 506 240 506 240 507 247 507 245 507 240 508 245 508 239 508 241 509 242 509 243 509 243 510 242 510 244 510 243 511 244 511 247 511 247 512 244 512 246 512 247 513 246 513 245 513 257 514 248 514 258 514 258 515 248 515 249 515 258 516 249 516 250 516 250 517 249 517 252 517 250 518 252 518 253 518 253 519 252 519 251 519 254 520 255 520 256 520 256 521 255 521 265 521 256 522 265 522 258 522 258 523 265 523 263 523 258 524 263 524 257 524 259 525 260 525 261 525 261 526 260 526 262 526 261 527 262 527 265 527 265 528 262 528 264 528 265 529 264 529 263 529 275 530 266 530 276 530 276 531 266 531 267 531 276 532 267 532 268 532 268 533 267 533 270 533 268 534 270 534 271 534 271 535 270 535 269 535 272 536 273 536 274 536 274 537 273 537 283 537 274 538 283 538 276 538 276 539 283 539 281 539 276 540 281 540 275 540 285 541 277 541 287 541 287 542 277 542 278 542 287 543 278 543 279 543 279 544 278 544 280 544 279 545 280 545 283 545 283 546 280 546 282 546 283 547 282 547 281 547 284 548 285 548 286 548 286 549 285 549 287 549 286 550 287 550 288 550 288 551 287 551 290 551 288 552 290 552 291 552 291 553 290 553 289 553 301 554 293 554 389 554 308 555 292 555 363 555 363 556 292 556 307 556 389 557 293 557 385 557 385 558 293 558 294 558 385 559 294 559 380 559 380 560 294 560 295 560 380 561 295 561 377 561 377 562 295 562 296 562 377 563 296 563 373 563 373 564 296 564 297 564 373 565 297 565 370 565 370 566 297 566 298 566 370 567 298 567 390 567 390 568 298 568 299 568 390 569 299 569 393 569 393 570 299 570 300 570 393 571 300 571 313 571 389 572 387 572 301 572 301 573 387 573 365 573 301 574 365 574 302 574 302 575 365 575 342 575 302 576 342 576 303 576 303 577 342 577 345 577 303 578 345 578 304 578 304 579 345 579 351 579 304 580 351 580 305 580 305 581 351 581 354 581 305 582 354 582 306 582 306 583 354 583 357 583 306 584 357 584 307 584 307 585 357 585 359 585 307 586 359 586 363 586 363 587 337 587 308 587 308 588 337 588 315 588 308 589 315 589 309 589 309 590 315 590 319 590 309 591 319 591 310 591 310 592 319 592 324 592 310 593 324 593 311 593 311 594 324 594 327 594 311 595 327 595 312 595 312 596 327 596 330 596 312 597 330 597 313 597 313 598 330 598 334 598 313 599 334 599 393 599 317 600 314 600 319 600 314 601 315 601 319 601 314 602 316 602 315 602 316 603 337 603 315 603 316 604 336 604 337 604 317 605 319 605 318 605 319 606 320 606 318 606 319 607 320 607 324 607 324 608 321 608 320 608 324 609 322 609 321 609 322 610 324 610 323 610 324 611 325 611 323 611 324 612 327 612 325 612 325 613 327 613 326 613 327 614 329 614 326 614 327 615 330 615 329 615 333 616 328 616 334 616 328 617 330 617 334 617 328 618 331 618 330 618 331 619 329 619 330 619 392 620 393 620 332 620 332 621 393 621 334 621 332 622 335 622 334 622 335 623 333 623 334 623 361 624 338 624 363 624 364 625 365 625 339 625 365 626 340 626 339 626 365 627 342 627 340 627 340 628 342 628 341 628 342 629 343 629 341 629 342 630 345 630 343 630 343 631 345 631 344 631 345 632 346 632 344 632 345 633 351 633 346 633 346 634 351 634 347 634 351 635 348 635 347 635 351 636 349 636 348 636 349 637 351 637 350 637 351 638 352 638 350 638 351 639 354 639 352 639 352 640 354 640 353 640 354 641 355 641 353 641 354 642 357 642 355 642 355 643 357 643 356 643 357 644 358 644 356 644 357 645 358 645 359 645 359 646 358 646 360 646 359 647 360 647 363 647 363 648 360 648 362 648 363 649 362 649 361 649 372 650 370 650 373 650 390 651 366 651 391 651 366 652 390 652 367 652 390 653 368 653 367 653 390 654 370 654 368 654 370 655 369 655 368 655 370 656 372 656 369 656 375 657 371 657 377 657 371 658 373 658 377 658 371 659 374 659 373 659 374 660 372 660 373 660 375 661 377 661 376 661 377 662 378 662 376 662 377 663 380 663 378 663 378 664 380 664 379 664 380 665 381 665 379 665 380 666 385 666 381 666 385 667 382 667 381 667 385 668 383 668 382 668 383 669 385 669 384 669 385 670 386 670 384 670 385 671 389 671 386 671 389 672 388 672 386 672 389 673 388 673 387 673 559 708 557 708 593 708 595 708 6 787 26 787 3 787 6 788 22 788 26 788 8 789 22 789 6 789 8 790 20 790 22 790 11 791 20 791 8 791 11 792 18 792 20 792 25 793 0 793 2 793 1 794 24 794 4 794 9 795 19 795 10 795 7 796 19 796 9 796 7 797 21 797 19 797 5 798 21 798 7 798 5 799 23 799 21 799 23 800 4 800 24 800 5 801 4 801 23 801 10 802 15 802 12 802 18 803 13 803 14 803 11 804 13 804 18 804 3 805 25 805 2 805 26 806 25 806 3 806 16 807 15 807 17 807 162 808 13 808 161 808 161 809 13 809 75 809 284 810 76 810 16 810 284 811 16 811 285 811 285 812 16 812 17 812 285 813 17 813 277 813 277 814 17 814 404 814 162 815 402 815 14 815 162 816 14 816 13 816 4 817 76 817 84 817 84 818 124 818 126 818 107 819 117 819 119 819 75 820 2 820 120 820 120 821 2 821 107 821 35 822 111 822 36 822 36 823 111 823 116 823 78 824 81 824 40 824 63 825 224 825 209 825 63 826 209 826 64 826 75 827 13 827 11 827 2 828 75 828 3 828 3 829 75 829 11 829 3 830 11 830 6 830 6 831 11 831 8 831 12 832 15 832 16 832 16 833 76 833 12 833 12 834 76 834 4 834 12 835 4 835 9 835 9 836 4 836 5 836 9 837 5 837 7 837 9 838 10 838 12 838 85 839 81 839 78 839 92 840 401 840 1 840 92 841 1 841 88 841 88 842 1 842 4 842 88 843 4 843 84 843 105 844 107 844 2 844 2 845 0 845 105 845 105 846 0 846 400 846 111 847 112 847 113 847 111 848 113 848 116 848 209 849 224 849 225 849 163 850 119 850 117 850 126 851 124 851 149 851 210 852 209 852 225 852 550 859 548 859 407 859 409 859 544 860 543 860 402 860 403 860 545 868 534 868 570 868 581 868 423 869 420 869 561 869 564 869 590 919 588 919 552 919 554 919 558 920 556 920 415 920 417 920 530 921 385 921 380 921 530 922 337 922 363 922 530 923 354 923 351 923 530 924 390 924 393 924 530 925 334 925 330 925 530 926 363 926 359 926 530 927 327 927 324 927 530 928 357 928 354 928 530 929 324 929 319 929 530 930 345 930 342 930 530 931 359 931 357 931 530 932 377 932 373 932 530 933 319 933 315 933 389 934 530 934 387 934 530 935 365 935 387 935 530 936 373 936 370 936 530 937 330 937 327 937 365 938 530 938 342 938 530 939 380 939 377 939 530 940 393 940 334 940 337 941 530 941 315 941 530 942 351 942 345 942 390 943 530 943 370 943 530 944 389 944 385 944 395 947 574 947 538 947 531 947 414 948 412 948 553 948 555 948 1 949 401 949 542 949 531 949 536 950 535 950 19 950 21 950 560 951 558 951 417 951 419 951 541 952 539 952 397 952 400 952 25 953 26 953 644 953 535 954 532 954 10 954 19 954 476 955 477 955 567 955 566 955 422 956 423 956 564 956 563 956 406 957 404 957 545 957 547 957 552 958 550 958 409 958 411 958 416 959 414 959 555 959 557 959 531 960 538 960 24 960 1 960 537 961 536 961 21 961 23 961 562 962 560 962 419 962 421 962 401 963 399 963 540 963 542 963 408 964 406 964 547 964 549 964 554 965 552 965 411 965 413 965 400 966 0 966 479 966 541 966 533 967 534 967 17 967 15 967 418 968 416 968 557 968 559 968 23 969 24 969 538 969 537 969 475 970 476 970 566 970 565 970 421 971 422 971 563 971 562 971 546 972 544 972 403 972 405 972 404 973 17 973 534 973 545 973 410 974 408 974 549 974 551 974 556 975 554 975 413 975 415 975 532 976 533 976 15 976 10 976 420 977 418 977 559 977 561 977 409 978 475 978 565 978 550 978 477 979 478 979 568 979 567 979 548 980 546 980 405 980 407 980 412 981 410 981 551 981 553 981 587 982 585 982 624 982 626 982 605 983 608 983 569 983 394 983 602 984 603 984 642 984 641 984 621 985 619 985 580 985 582 985 598 986 599 986 638 986 637 986 595 987 593 987 632 987 634 987 581 988 570 988 609 988 620 988 573 989 572 989 536 989 537 989 562 990 563 990 599 990 598 990 582 991 580 991 544 991 546 991 566 992 567 992 603 992 602 992 394 993 569 993 533 993 532 993 551 994 549 994 585 994 587 994 396 995 577 995 541 995 479 995 592 996 590 996 554 996 556 996 561 997 559 997 595 997 597 997 537 998 538 998 574 998 573 998 584 999 582 999 546 999 548 999 571 1000 394 1000 532 1000 535 1000 553 1001 551 1001 587 1001 589 1001 594 1002 592 1002 556 1002 558 1002 564 1003 561 1003 597 1003 600 1003 580 1004 579 1004 543 1004 544 1004 565 1005 566 1005 602 1005 601 1005 569 1006 570 1006 534 1006 533 1006 586 1007 584 1007 548 1007 550 1007 555 1008 553 1008 589 1008 591 1008 550 1009 565 1009 601 1009 586 1009 596 1010 594 1010 558 1010 560 1010 577 1011 575 1011 539 1011 541 1011 644 1012 22 1012 20 1012 563 1013 564 1013 600 1013 599 1013 547 1014 545 1014 581 1014 583 1014 567 1015 568 1015 604 1015 603 1015 572 1016 571 1016 535 1016 536 1016 588 1017 586 1017 550 1017 552 1017 531 1018 542 1018 578 1018 395 1018 557 1019 555 1019 591 1019 593 1019 598 1020 596 1020 560 1020 562 1020 542 1021 540 1021 576 1021 578 1021 549 1022 547 1022 583 1022 585 1022 607 1023 616 1023 577 1023 396 1023 631 1024 629 1024 590 1024 592 1024 597 1025 595 1025 634 1025 636 1025 623 1026 621 1026 582 1026 584 1026 610 1027 605 1027 394 1027 571 1027 589 1028 587 1028 626 1028 628 1028 573 1029 574 1029 613 1029 612 1029 633 1030 631 1030 592 1030 594 1030 611 1031 610 1031 571 1031 572 1031 600 1032 597 1032 636 1032 639 1032 619 1033 618 1033 579 1033 580 1033 601 1034 602 1034 641 1034 640 1034 608 1035 609 1035 570 1035 569 1035 625 1036 623 1036 584 1036 586 1036 591 1037 589 1037 628 1037 630 1037 612 1038 611 1038 572 1038 573 1038 586 1039 601 1039 640 1039 625 1039 635 1040 633 1040 594 1040 596 1040 616 1041 614 1041 575 1041 577 1041 599 1042 600 1042 639 1042 638 1042 583 1043 581 1043 620 1043 622 1043 603 1044 604 1044 643 1044 642 1044 627 1045 625 1045 586 1045 588 1045 607 1046 25 1046 644 1046 395 1047 578 1047 617 1047 606 1047 593 1048 591 1048 630 1048 632 1048 637 1049 635 1049 596 1049 598 1049 578 1050 576 1050 615 1050 617 1050 585 1051 583 1051 622 1051 624 1051 606 1052 613 1052 574 1052 395 1052 629 1053 627 1053 588 1053 590 1053 396 1054 25 1054 607 1054 644 1055 615 1055 398 1055 398 1056 615 1056 576 1056 614 1057 398 1057 575 1057 398 1058 540 1058 399 1058 644 1059 398 1059 614 1059 398 1060 576 1060 540 1060 539 1061 398 1061 397 1061 575 1063 398 1063 539 1063 622 1065 620 1065 644 1065 638 1066 639 1066 644 1066 644 1067 614 1067 616 1067 644 1068 633 1068 635 1068 625 1069 644 1069 640 1069 644 1070 611 1070 612 1070 630 1071 628 1071 644 1071 642 1072 643 1072 644 1072 644 1073 625 1073 627 1073 479 1074 25 1074 396 1074 606 1075 617 1075 644 1075 632 1076 630 1076 644 1076 644 1077 635 1077 637 1077 617 1078 615 1078 644 1078 624 1079 622 1079 644 1079 644 1080 613 1080 606 1080 644 1081 627 1081 629 1081 0 1082 25 1082 479 1082 620 1083 609 1083 644 1083 634 1084 632 1084 644 1084 637 1085 638 1085 644 1085 644 1086 619 1086 621 1086 641 1087 642 1087 644 1087 644 1088 608 1088 605 1088 626 1089 624 1089 644 1089 644 1090 616 1090 607 1090 644 1091 629 1091 631 1091 636 1092 634 1092 644 1092 644 1093 621 1093 623 1093 644 1094 605 1094 610 1094 628 1095 626 1095 644 1095 612 1096 613 1096 644 1096 644 1097 631 1097 633 1097 644 1098 610 1098 611 1098 639 1099 636 1099 644 1099 644 1100 618 1100 619 1100 640 1101 641 1101 644 1101 644 1102 609 1102 608 1102 644 1103 623 1103 625 1103 26 1128 22 1128 644 1128 644 1129 18 1129 20 1129 543 1130 14 1130 402 1130 478 1131 14 1131 568 1131 568 1132 14 1132 604 1132 579 1133 14 1133 543 1133 604 1134 14 1134 643 1134 618 1135 14 1135 579 1135 643 1136 14 1136 644 1136 644 1137 14 1137 618 1137 18 1138 14 1138 644 1138

+ + + + + 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 +

453 674 526 674 528 674 453 675 503 675 524 675 453 676 449 676 503 676 503 677 449 677 466 677 529 678 528 678 520 678 523 679 521 679 522 679 483 680 481 680 468 680 467 681 469 681 457 681 430 682 432 682 439 682 439 683 441 683 430 683 466 684 456 684 468 684 426 685 424 685 441 685 441 686 424 686 430 686 520 687 514 687 522 687 497 688 524 688 503 688 498 689 515 689 500 689 505 690 516 690 501 690 501 691 516 691 508 691 501 692 508 692 491 692 491 693 508 693 493 693 491 694 493 694 489 694 489 695 493 695 487 695 516 696 518 696 508 696 508 697 518 697 510 697 508 698 510 698 493 698 493 699 510 699 495 699 493 700 495 700 487 700 487 701 495 701 485 701 518 702 522 702 510 702 510 703 522 703 514 703 510 704 514 704 495 704 495 705 514 705 499 705 495 706 499 706 485 706 485 707 499 707 483 707 472 709 487 709 485 709 472 710 485 710 470 710 470 711 485 711 483 711 470 712 483 712 468 712 506 713 507 713 517 713 516 714 505 714 506 714 506 715 517 715 516 715 516 716 517 716 519 716 516 717 519 717 518 717 518 718 519 718 523 718 518 719 523 719 522 719 464 720 472 720 460 720 464 721 460 721 451 721 451 722 460 722 444 722 451 723 444 723 446 723 446 724 444 724 436 724 472 725 470 725 460 725 460 726 470 726 458 726 460 727 458 727 444 727 444 728 458 728 442 728 444 729 442 729 436 729 436 730 442 730 434 730 470 731 468 731 458 731 458 732 468 732 456 732 458 733 456 733 442 733 442 734 456 734 439 734 442 735 439 735 434 735 434 736 439 736 432 736 490 737 488 737 492 737 492 738 488 738 494 738 492 739 494 739 502 739 502 740 494 740 509 740 502 741 509 741 507 741 507 742 509 742 517 742 488 743 486 743 494 743 494 744 486 744 496 744 494 745 496 745 509 745 509 746 496 746 511 746 509 747 511 747 517 747 517 748 511 748 519 748 486 749 484 749 496 749 496 750 484 750 500 750 496 751 500 751 511 751 511 752 500 752 515 752 511 753 515 753 519 753 519 754 515 754 523 754 447 755 446 755 436 755 437 756 448 756 447 756 447 757 436 757 437 757 437 758 436 758 434 758 437 759 434 759 435 759 435 760 434 760 432 760 435 761 432 761 433 761 480 762 474 762 473 762 488 763 490 763 480 763 480 764 473 764 488 764 488 765 473 765 471 765 488 766 471 766 486 766 486 767 471 767 469 767 486 768 469 768 484 768 448 769 437 769 452 769 452 770 437 770 445 770 452 771 445 771 465 771 465 772 445 772 461 772 465 773 461 773 474 773 474 774 461 774 473 774 437 775 435 775 445 775 445 776 435 776 443 776 445 777 443 777 461 777 461 778 443 778 459 778 461 779 459 779 473 779 473 780 459 780 471 780 435 781 433 781 443 781 443 782 433 782 440 782 443 783 440 783 459 783 459 784 440 784 457 784 459 785 457 785 471 785 471 786 457 786 469 786 529 853 527 853 453 853 450 854 429 854 453 854 528 855 529 855 453 855 429 856 427 856 453 856 525 857 504 857 453 857 504 858 450 858 453 858 453 861 425 861 424 861 453 862 428 862 449 862 453 863 426 863 428 863 453 864 424 864 426 864 527 865 525 865 453 865 453 866 524 866 526 866 427 867 425 867 453 867 503 870 466 870 481 870 529 871 520 871 521 871 424 872 425 872 431 872 424 873 431 873 430 873 469 874 467 874 484 874 484 875 467 875 482 875 522 876 521 876 520 876 468 877 481 877 466 877 432 878 430 878 433 878 433 879 430 879 431 879 440 880 438 880 457 880 457 881 438 881 454 881 457 882 454 882 467 882 438 883 440 883 431 883 431 884 440 884 433 884 438 885 431 885 425 885 438 886 425 886 427 886 438 887 427 887 454 887 454 888 427 888 429 888 454 889 429 889 450 889 454 890 450 890 467 890 456 891 455 891 439 891 439 892 455 892 441 892 455 893 456 893 466 893 455 894 466 894 449 894 455 895 449 895 428 895 455 896 428 896 441 896 441 897 428 897 426 897 481 898 483 898 499 898 514 899 512 899 499 899 499 900 512 900 497 900 499 901 497 901 481 901 512 902 514 902 520 902 512 903 520 903 528 903 512 904 528 904 526 904 512 905 526 905 497 905 497 906 526 906 524 906 497 907 503 907 481 907 525 908 498 908 504 908 504 909 498 909 482 909 529 910 521 910 527 910 527 911 521 911 513 911 498 912 525 912 527 912 498 913 527 913 513 913 498 914 513 914 515 914 515 915 513 915 523 915 523 916 513 916 521 916 498 917 500 917 482 917 482 918 500 918 484 918 462 945 489 945 487 945 462 946 487 946 472 946 472 1062 463 1062 462 1062 463 1064 472 1064 464 1064 645 1104 450 1104 504 1104 504 1105 482 1105 645 1105 482 1106 467 1106 645 1106 467 1107 450 1107 645 1107 646 1108 452 1108 465 1108 489 1109 462 1109 646 1109 646 1110 506 1110 505 1110 646 1111 465 1111 474 1111 463 1112 464 1112 646 1112 447 1113 448 1113 646 1113 646 1114 492 1114 502 1114 646 1115 490 1115 492 1115 646 1116 502 1116 507 1116 646 1117 448 1117 452 1117 646 1118 507 1118 506 1118 446 1119 447 1119 646 1119 462 1120 463 1120 646 1120 501 1121 491 1121 646 1121 505 1122 501 1122 646 1122 451 1123 446 1123 646 1123 646 1124 480 1124 490 1124 646 1125 474 1125 480 1125 491 1126 489 1126 646 1126 464 1127 451 1127 646 1127

+ + + 1 + + + + + + 7.481132 -6.50764 5.343665 + 0 0 1 46.69194 + 0 1 0 0.619768 + 1 0 0 63.5593 + 1 1 1 + + + + 4.076245 1.005454 5.903862 + 0 0 1 106.9363 + 0 1 0 3.163707 + 1 0 0 37.26105 + 1 1 1 + + + + 0 0 0 + 0 0 1 0 + 0 1 0 0 + 1 0 0 90.00001 + 1 1 1 + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/Coordinates b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/Coordinates new file mode 100755 index 0000000..6fba9d2 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/Coordinates @@ -0,0 +1,20 @@ +========================================= +Location Coordinates (x,y,z) +========================================= + +Chassis : -0.045 0 0.148 +Top : -0.045 0 0.234 + +Swivel : -0.185 0 0.055 +Center Hubcap : -0.211 0 0.037 +Center Wheel : -0.211 0 0.038 + +Right Hubcap : 0 0.158 0.091 +Right Wheel : 0 0.158 0.091 + +Left Hubcap : 0 -0.158 0.091 +Left Wheel : 0 -0.158 0.091 + +Front Sonar : -0.2 0 0.209 +Back Sonar : 0.109 0 0.209 + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/back_rim.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/back_rim.stl new file mode 100755 index 0000000..09587a5 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/back_rim.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/back_sonar.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/back_sonar.stl new file mode 100755 index 0000000..9153bc5 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/back_sonar.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/center_hubcap.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/center_hubcap.stl new file mode 100755 index 0000000..c9400f7 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/center_hubcap.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/center_wheel.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/center_wheel.stl new file mode 100755 index 0000000..c8b857d Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/center_wheel.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/chassis.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/chassis.stl new file mode 100755 index 0000000..13aa2cf Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/chassis.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/front_rim.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/front_rim.stl new file mode 100755 index 0000000..c3074d5 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/front_rim.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/front_sonar.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/front_sonar.stl new file mode 100755 index 0000000..82b9ce0 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/front_sonar.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/left_hubcap.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/left_hubcap.stl new file mode 100755 index 0000000..01bd2f9 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/left_hubcap.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/left_wheel.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/left_wheel.stl new file mode 100755 index 0000000..31137e2 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/left_wheel.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/right_hubcap.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/right_hubcap.stl new file mode 100755 index 0000000..ac3f36f Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/right_hubcap.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/right_wheel.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/right_wheel.stl new file mode 100755 index 0000000..c518c0e Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/right_wheel.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/swivel.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/swivel.stl new file mode 100755 index 0000000..03797a2 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/swivel.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/top.stl b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/top.stl new file mode 100755 index 0000000..302ed5f Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/meshes/p3dx/top.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro new file mode 100755 index 0000000..b736e81 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/camera/camera.xacro @@ -0,0 +1,81 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 30.0 + + 1.3962634 + + 800 + 800 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + true + 0.0 + ${name} + image_raw + camera_info + ${name} + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro new file mode 100755 index 0000000..61e4964 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo.xacro @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 0 0 + + + + + 1 + 2.0944 + -2.0944 + 683 + + + + 0.08 + 5 + 0.01 + + + + + + ${name}/scan + ${name} + + + + 1 + 30 + true + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo_gpu.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo_gpu.xacro new file mode 100755 index 0000000..520ec08 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/laser/hokuyo_gpu.xacro @@ -0,0 +1,76 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 0 0 + 1 + 30 + true + + + + + 1 + 2.0944 + -2.0944 + + + 683 + + + + 0.10 + 140.0 + 0.01 + + + gaussian + + 0.0 + 0.01 + + + + + ${name}/scan + ${name} + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/materials.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/materials.xacro new file mode 100755 index 0000000..311c3cd --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/materials.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/battery_block.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/battery_block.xacro new file mode 100755 index 0000000..c14674e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/battery_block.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/inertia_tensors.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/inertia_tensors.xacro new file mode 100755 index 0000000..d33f1ca --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/inertia_tensors.xacro @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro new file mode 100755 index 0000000..50be732 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx.xacro @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro new file mode 100755 index 0000000..c2a3a16 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_body.xacro @@ -0,0 +1,38 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_chassis.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_chassis.xacro new file mode 100755 index 0000000..4daddb6 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_chassis.xacro @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro new file mode 100755 index 0000000..08efb04 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_plugins.xacro @@ -0,0 +1,39 @@ + + + + + + + + chassis_swivel_joint, swivel_wheel_joint, left_hub_joint, right_hub_joint + 10.0 + true + + + + + + + + Debug + + false + true + true + left_hub_joint + right_hub_joint + 0.3 + 0.18 + 20 + 1.8 + cmd_vel + odom + odom + world + base_link + 10.0 + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_sonar.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_sonar.xacro new file mode 100755 index 0000000..5bdf7a8 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_sonar.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_swivel.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_swivel.xacro new file mode 100755 index 0000000..1cba807 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_swivel.xacro @@ -0,0 +1,114 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_wheel.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_wheel.xacro new file mode 100755 index 0000000..9c0f87e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/multi_robot_scenario/xacro/p3dx/pioneer3dx_wheel.xacro @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_3_double_pendulums.launch b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_3_double_pendulums.launch new file mode 100755 index 0000000..479629b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_3_double_pendulums.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_3_single_pendulums.launch b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_3_single_pendulums.launch new file mode 100755 index 0000000..8f43f58 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_3_single_pendulums.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_double_pendulum.launch b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_double_pendulum.launch new file mode 100755 index 0000000..37ac18b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_double_pendulum.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_link_pose.py b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_link_pose.py new file mode 100755 index 0000000..ced3717 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_link_pose.py @@ -0,0 +1,175 @@ +#!/usr/bin/env python +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# 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 the Willow Garage 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. +# + +## Gazebo test utility +## check a link's pose against some reference pose +## good for unit testing proper behavior + +PKG = 'gazebo_plugins' +NAME = 'test_link_pose' + +import math +import roslib +roslib.load_manifest(PKG) + +import sys, unittest +import os, os.path, threading, time +import rospy, rostest +from nav_msgs.msg import * +from geometry_msgs.msg import Pose +from geometry_msgs.msg import Twist +from tf import transformations + +class LinkPoseTest(unittest.TestCase): + def __init__(self, *args): + super(LinkPoseTest, self).__init__(*args) + self.success = False + + self.link_error_total = 0 + self.link_error_rms = 0 + self.link_sample_count = 0 + self.valid_sample_count = 0 + self.link_error_max = 0 + + self.min_link_samples_topic = "~min_link_samples" + self.min_link_samples = 1000 + + self.min_valid_samples_topic = "~min_valid_samples" + self.min_valid_samples = 0 + + self.tolerance_topic = "~error_tolerance" + self.tolerance = 0.01 + + self.max_error_topic = "~max_error" + self.max_error = 0.02 + + # in seconds + self.test_duration_topic = "~test_duration" + self.test_duration = 10.0 + + # test start time in seconds + self.test_start_time_topic = "~test_start_time" + self.test_start_time = 0.0 + + self.link_pose_topic_name = "~link_pose_topic_name" + self.link_pose_topic = "/model_1/link_2/pose" + + self.valid_pose_topic_name = "~valid_pose_topic_name" + self.valid_pose_topic = "/p3d_valid" + + self.link_pose = Pose() + self.valid_pose = Pose() + + def printLinkPose(self, p3d): + print "P3D pose translan: " + "x: " + str(p3d.pose.pose.position.x) + print " " + "y: " + str(p3d.pose.pose.position.y) + print " " + "z: " + str(p3d.pose.pose.position.z) + print "P3D pose rotation: " + "x: " + str(p3d.pose.pose.orientation.x) + print " " + "y: " + str(p3d.pose.pose.orientation.y) + print " " + "z: " + str(p3d.pose.pose.orientation.z) + print " " + "w: " + str(p3d.pose.pose.orientation.w) + print "P3D rate translan: " + "x: " + str(p3d.twist.twist.linear.x) + print " " + "y: " + str(p3d.twist.twist.linear.y) + print " " + "z: " + str(p3d.twist.twist.linear.z) + print "P3D rate rotation: " + "x: " + str(p3d.twist.twist.angular.x) + print " " + "y: " + str(p3d.twist.twist.angular.y) + print " " + "z: " + str(p3d.twist.twist.angular.z) + + + def linkP3dInput(self, p3d): + #self.printLinkPose(p3d) + self.link_pose = p3d.pose.pose + + # start logging after we get minimum valid pose specified + # todo: synchronize the two input streams somehow + if self.valid_sample_count >= self.min_valid_samples: + self.link_sample_count += 1 + tmpx = self.link_pose.position.x - self.valid_pose.position.x + tmpy = self.link_pose.position.y - self.valid_pose.position.y + tmpz = self.link_pose.position.z - self.valid_pose.position.z + error = math.sqrt(tmpx*tmpx+tmpy*tmpy+tmpz*tmpz) + self.link_error_total += error + self.link_error_rms = self.link_error_total / self.link_sample_count + if error > self.link_error_max: + self.link_error_max = math.sqrt(tmpx*tmpx+tmpy*tmpy+tmpz*tmpz) + + def validP3dInput(self, p3d): + self.valid_pose = p3d.pose.pose + self.valid_sample_count += 1 + + def checkPose(self): + # difference in pose + print "error: " + str(self.link_sample_count) \ + + " error:" + str(self.link_error_total) \ + + " avg err:" + str(self.link_error_rms) \ + + " max err:" + str(self.link_error_max) \ + + " count: " + str(self.link_sample_count) + if self.link_sample_count >= self.min_link_samples: + if self.link_error_rms < self.tolerance: + if self.link_error_max < self.max_error: + self.success = True + + + def test_link_pose(self): + print "LNK\n" + rospy.init_node(NAME, anonymous=True) + self.link_pose_topic = rospy.get_param(self.link_pose_topic_name,self.link_pose_topic); + self.valid_pose_topic = rospy.get_param(self.valid_pose_topic_name,self.valid_pose_topic); + self.min_link_samples = rospy.get_param(self.min_link_samples_topic,self.min_link_samples); + self.min_valid_samples = rospy.get_param(self.min_valid_samples_topic,self.min_valid_samples); + self.tolerance = rospy.get_param(self.tolerance_topic,self.tolerance); + self.max_error = rospy.get_param(self.max_error_topic,self.max_error); + self.test_duration = rospy.get_param(self.test_duration_topic,self.test_duration); + self.test_start_time = rospy.get_param(self.test_start_time_topic,self.test_start_time); + + while not rospy.is_shutdown() and time.time() < self.test_start_time: + rospy.stdinfo("Waiting for test to start at time [%s]"% self.test_start_time) + time.sleep(0.1) + + print "subscribe" + rospy.Subscriber(self.link_pose_topic, Odometry, self.linkP3dInput) + rospy.Subscriber(self.valid_pose_topic, Odometry, self.validP3dInput) + + start_t = time.time() + timeout_t = start_t + self.test_duration + while not rospy.is_shutdown() and not self.success and time.time() < timeout_t: + self.checkPose() + time.sleep(0.1) + self.assert_(self.success) + +if __name__ == '__main__': + print "Waiting for test to start at time " + rostest.run(PKG, sys.argv[0], LinkPoseTest, sys.argv) #, text_mode=True) + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_single_pendulum.launch b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_single_pendulum.launch new file mode 100755 index 0000000..20d205e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/test_single_pendulum.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/3_double_pendulums.world b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/3_double_pendulums.world new file mode 100755 index 0000000..f0c83b0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/3_double_pendulums.world @@ -0,0 +1,459 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + true + 1000.0 + link_1 + /model_1/link_1/pose + world + 0 0 -2.1 + 0 0 0.0 + + + true + 1000.0 + link_2 + /model_1/link_2/pose + link_1 + 0 0 1.0 + 0 0 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + true + 1000.0 + link_1 + /model_2/link_1/pose + world + 0 -1.5 -2.1 + 0 0 0.0 + + + true + 1000.0 + link_2 + /model_2/link_2/pose + link_1 + 0 0 1.0 + 0 0 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + true + 1000.0 + link_1 + /model_3/link_1/pose + world + 0 2 -2.1 + 0 0 0.0 + + + true + 1000.0 + link_2 + /model_3/link_2/pose + link_1 + 0 0 1.0 + 0 0 0.0 + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/3_single_pendulums.world b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/3_single_pendulums.world new file mode 100755 index 0000000..86e8a72 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/3_single_pendulums.world @@ -0,0 +1,249 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + true + 1000.0 + link_1 + /model_1/link_1/pose + world + 0 0 -2.1 + 0 0 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + true + 1000.0 + link_1 + /model_2/link_1/pose + world + 0 -1.5 -2.1 + 0 0 0.0 + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + true + 1000.0 + link_1 + /model_3/link_1/pose + world + 0 2.0 -2.1 + 0 0 0.0 + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/double_pendulum.world b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/double_pendulum.world new file mode 100755 index 0000000..03b1abd --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/double_pendulum.world @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + true + 1000.0 + link_1 + /model_1/link_1/pose + world + 0 0 -2.1 + 0 0 0.0 + + + true + 1000.0 + link_2 + /model_1/link_2/pose + link_1 + 0 0 1.0 + 0 0 0.0 + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/single_pendulum.world b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/single_pendulum.world new file mode 100755 index 0000000..e05754d --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/p3d_test/worlds/single_pendulum.world @@ -0,0 +1,96 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 250 + + + + + + + + + + + + + + + + + + + + + + + + + + + 0.2 + 0.0 + 1000.0 + 0.0 + 0.0 + + + + + + + + + true + 1000.0 + link_1 + /model_1/link_1/pose + world + 0 0 -2.1 + 0 0 0.0 + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/pub_joint_trajectory_test.cpp b/gazebo_ros_pkgs/gazebo_plugins/test/pub_joint_trajectory_test.cpp new file mode 100755 index 0000000..3c8b0e3 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/pub_joint_trajectory_test.cpp @@ -0,0 +1,80 @@ +#include +#include +#include +#include +#include + +int main(int argc, char** argv) +{ + + ros::init(argc, argv, "pub_joint_trajectory_test"); + ros::NodeHandle rosnode; + ros::Publisher pub_ = rosnode.advertise("set_joint_trajectory",100); + ros::ServiceClient srv_ = rosnode.serviceClient("set_joint_trajectory"); + + trajectory_msgs::JointTrajectory jt; + + jt.header.stamp = ros::Time::now(); + jt.header.frame_id = "pr2::torso_lift_link"; + + jt.joint_names.push_back("r_shoulder_pan_joint"); + jt.joint_names.push_back("r_shoulder_lift_joint"); + jt.joint_names.push_back("r_upper_arm_roll_joint"); + jt.joint_names.push_back("r_elbow_flex_joint"); + jt.joint_names.push_back("r_forearm_roll_joint"); + jt.joint_names.push_back("r_wrist_flex_joint"); + jt.joint_names.push_back("l_shoulder_pan_joint"); + jt.joint_names.push_back("l_shoulder_lift_joint"); + jt.joint_names.push_back("l_upper_arm_roll_joint"); + jt.joint_names.push_back("l_elbow_flex_joint"); + jt.joint_names.push_back("l_forearm_roll_joint"); + jt.joint_names.push_back("l_wrist_flex_joint"); + + int n = 500; + double dt = 0.1; + double rps = 0.05; + jt.points.resize(n); + for (int i = 0; i < n; i++) + { + double theta = rps*2.0*M_PI*i*dt; + double x1 = -0.5*sin(2*theta); + double x2 = 0.5*sin(1*theta); + jt.points[i].positions.push_back(x1); + jt.points[i].positions.push_back(x2); + jt.points[i].positions.push_back(3.14); + jt.points[i].positions.push_back(-10.0); + jt.points[i].positions.push_back(-0.2); + jt.points[i].positions.push_back(-0.2); + jt.points[i].positions.push_back(x1); + jt.points[i].positions.push_back(x2); + jt.points[i].positions.push_back(3.14); + jt.points[i].positions.push_back(10.0); + jt.points[i].positions.push_back(-0.2); + jt.points[i].positions.push_back(-0.2); + // set duration + jt.points[i].time_from_start = ros::Duration(dt); + ROS_INFO_NAMED("joint_trajectory_test", "test: angles[%d][%f, %f]",n,x1,x2); + } + + // pub_.publish(jt); // use publisher + + gazebo_msgs::SetJointTrajectory sjt; + sjt.request.joint_trajectory = jt; + sjt.request.disable_physics_updates = false; + + ignition::math::Quaterniond r(0, 0, M_PI); + geometry_msgs::Pose p; + p.position.x = 0; + p.position.y = 0; + p.position.z = 0; + p.orientation.x = r.X(); + p.orientation.y = r.Y(); + p.orientation.z = r.Z(); + p.orientation.w = r.W(); + sjt.request.model_pose = p; + sjt.request.set_model_pose = true; + + srv_.call(sjt); // use service + + return 0; +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/range/range_plugin.test b/gazebo_ros_pkgs/gazebo_plugins/test/range/range_plugin.test new file mode 100755 index 0000000..3e1ee96 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/range/range_plugin.test @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/set_model_state_test/set_model_state_test.cpp b/gazebo_ros_pkgs/gazebo_plugins/test/set_model_state_test/set_model_state_test.cpp new file mode 100755 index 0000000..754d799 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/set_model_state_test/set_model_state_test.cpp @@ -0,0 +1,110 @@ +#include +#include +#include + +#include +#include + +#include + +double curX_; +double curY_; +double curHeading_; +bool vehicleSpawned_; + +void modelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg) +{ + // Find the robot's state, update pose variables + size_t modelCount = msg->name.size(); + tf::Quaternion quat; + double dummy; + + for(size_t modelInd = 0; modelInd < modelCount; ++modelInd) + { + if(msg->name[modelInd] == "pioneer2dx") + { + vehicleSpawned_ = true; + + tf::quaternionMsgToTF(msg->pose[modelInd].orientation, quat); + tf::Matrix3x3 mat(quat); + mat.getRPY(dummy, dummy, curHeading_); + curX_ = msg->pose[modelInd].position.x; + curY_ = msg->pose[modelInd].position.y; + + break; + } + } +} + +TEST (ModelStateTest, FrameTest) +{ + vehicleSpawned_ = false; + curHeading_ = 0; + + ros::NodeHandle nh; + ros::Publisher modelStatePub = nh.advertise("/gazebo/set_model_state", 5); + ros::Subscriber modelStatesSub = nh.subscribe("/gazebo/model_states", 1, &modelStatesCallback); + + gazebo_msgs::ModelState state; + + // Issue commands in the chassis frame + state.model_name = "pioneer2dx"; + state.pose.orientation.w = 1.0; + state.reference_frame = "pioneer2dx::chassis"; + + // Wait for model to spawn + while(!vehicleSpawned_) + { + ros::spinOnce(); + ros::Duration(0.1).sleep(); + } + + // First, turn the robot so it's going about 45 degrees + state.pose.orientation.z = 0.04018; + do + { + modelStatePub.publish(state); + ros::Duration(0.2).sleep(); + ros::spinOnce(); + } while(::fabs(curHeading_ - M_PI/4) > 0.05); + + state.pose.orientation.z = 0; + state.pose.position.x = 0.1; + + // Now, stop the robot and drive forwards about 5 meters + do + { + modelStatePub.publish(state); + ros::Duration(0.2).sleep(); + ros::spinOnce(); + } while(::sqrt(curX_*curX_ + curY_*curY_) < 5.0); + + // The X and Y values should be approximately the same, + // and should be roughly sqrt(25/2) + EXPECT_LT(::fabs(curX_ - 3.535533906), 0.2); + EXPECT_LT(::fabs(curY_ - 3.535533906), 0.2); + + state.pose.position.x = 0; + state.reference_frame = "world"; + + // Now, send the robot to (0, 0) in the world frame + do + { + modelStatePub.publish(state); + ros::Duration(0.2).sleep(); + ros::spinOnce(); + } while(::sqrt(curX_*curX_ + curY_*curY_) > 0.1); + + EXPECT_LT(::fabs(curX_), 0.01); + EXPECT_LT(::fabs(curY_), 0.01); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + + ros::init(argc, argv, "set_model_state-test"); + ros::Time::init(); + + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/set_model_state_test/set_model_state_test.test b/gazebo_ros_pkgs/gazebo_plugins/test/set_model_state_test/set_model_state_test.test new file mode 100755 index 0000000..1ced8a2 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/set_model_state_test/set_model_state_test.test @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/set_model_state_test/set_model_state_test_p2dx.world b/gazebo_ros_pkgs/gazebo_plugins/test/set_model_state_test/set_model_state_test_p2dx.world new file mode 100755 index 0000000..80f9df0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/set_model_state_test/set_model_state_test_p2dx.world @@ -0,0 +1,356 @@ + + + + 1 + 0 0 10 0 -0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + + + 0.001 + 1 + 1000 + 0 0 -9.8 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + + 0 0 0.16 0 -0 0 + + 5.67 + + 0.07 + 0.08 + 0.1 + 0 + 0 + 0 + + + + + + 0.445 0.277 0.17 + + + 10 + + + + + + + + + + + + -0.2 0 -0.12 0 -0 0 + + + 0.04 + + + + + + 0 + 0 + 1 + 1 + + + + + + + + 10 + + + 0 0 0.04 0 -0 0 + + + model://pioneer2dx/meshes/chassis.dae + + + + + -0.2 0 -0.12 0 -0 0 + + + 0.04 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + + 0.1 -0.17 0.11 0 1.5707 1.5707 + + 1.5 + + 0.0051 + 0.0051 + 0.009 + 0 + 0 + 0 + + + + + + 0.11 + 0.05 + + + + + + 100000 + 100000 + 0 + 0 + + + + + + + + 10 + + + + + 0.11 + 0.05 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + + 0.1 0.17 0.11 0 1.5707 1.5707 + + 1.5 + + 0.0051 + 0.0051 + 0.009 + 0 + 0 + 0 + + + + + + 0.11 + 0.05 + + + + + + 100000 + 100000 + 0 + 0 + + + + + + + + 10 + + + + + 0.11 + 0.05 + + + + + + + + 0 + 0 + + 0 + 0 + 1 + + + 0 0 -0.03 0 -0 0 + left_wheel + chassis + + 0 1 0 + + -1e+16 + 1e+16 + + + + + 0 0 0.03 0 -0 0 + right_wheel + chassis + + 0 1 0 + + -1e+16 + 1e+16 + + + + + left_wheel_hinge + right_wheel_hinge + 5 + + 0 0 0 0 -0 0 + 0 + + + 350 317000000 + 101 310248604 + 1411160586 171302518 + + 0 0 0 0 -0 0 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 0 0 0 0 + + 0 0 0.160002 0 0 0 + 0 0 0 0 0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.1 0.17 0.11 0 1.5707 1.5707 + 0 0 0 0 0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + 0.1 -0.17 0.11 0 1.5707 1.5707 + 0 0 0 0 0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + + + 4.41021 0.618362 4.61186 4.45496e-16 1.53964 3.0162 + orbit + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/spawn_test/parameter_server_test.launch b/gazebo_ros_pkgs/gazebo_plugins/test/spawn_test/parameter_server_test.launch new file mode 100755 index 0000000..f4c697e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/spawn_test/parameter_server_test.launch @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/spawn_test/spawn_robots.sh b/gazebo_ros_pkgs/gazebo_plugins/test/spawn_test/spawn_robots.sh new file mode 100755 index 0000000..4e8526f --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/spawn_test/spawn_robots.sh @@ -0,0 +1,18 @@ +# Required: rrtbot_description package + +read -p "URDF from file:" +rosrun gazebo_ros spawn_model -file `rospack find rrbot_description`/urdf/rrbot.xml -urdf -y 1 -model rrbot1 -robot_namespace robot2 + +read -p "URDF from parameter server using roslaunch and xacro:" +roslaunch parameter_server_test.launch + +read -p "SDF from local model database:" +rosrun gazebo_ros spawn_model -file `echo $GAZEBO_MODEL_PATH`/coke_can/model.sdf -sdf -model coke_can1 -y 0.2 -x -0.3 + +read -p "SDF from local model database with Deprecated tag:" +rosrun gazebo_ros spawn_model -file `echo $GAZEBO_MODEL_PATH`/coke_can/model.sdf -gazebo -model coke_can2 -y 1.2 -x -0.3 + +read -p "SDF from the online model database:" +rosrun gazebo_ros spawn_model -database coke_can -sdf -model coke_can3 -y 2.2 -x -0.3 + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/bumper_test.world b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/bumper_test.world new file mode 100755 index 0000000..109a5bf --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/bumper_test.world @@ -0,0 +1,257 @@ + + + + 0.500000 0.500000 0.500000 0.500000 + 0.700000 0.700000 0.700000 1.000000 + 0 + + + 0.000000 -1.000000 -10.000000 + + + quick + 50 + 0 + 1.300000 + + + 0.000000 + 0.200000 + 100.000000 + 0.001000 + + + 0.000000 + 0.001000 + + + 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 + + 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 + + 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 + + + 0.000000 0.000000 1.000000 + + + + + + 50.000000 + 50.000000 + 0.000000 0.000000 0.000000 + 0.000000 + 0.000000 + + + + 0.000000 + 100000.000000 + + + + 0.000000 + 0.200000 + 1000000000.000000 + 1.000000 + 100.000000 + 0.001000 + + + + 0.000000 + + 1 + 0 + 0 + + 1 + + + false + 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 + + 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 + + 1.000000 0.000000 0.250000 0.000000 -0.000000 0.000000 + + 0.100000 + 0.000000 + 0.000000 + 0.100000 + 0.000000 + 0.100000 + + 1.000000 + + + 1.000000 0.000000 0.250000 0.000000 -0.000000 0.000000 + + + 0.500000 0.500000 0.500000 + + + + + + -1.000000 + -1.000000 + 0.000000 0.000000 0.000000 + 0.000000 + 0.000000 + + + + 0.000000 + 100000.000000 + + + + 0.000000 + 0.200000 + 1000000000000.000000 + 1.000000 + 100.000000 + 0.001000 + + + + 0.000000 + + + 1.000000 0.000000 0.250000 0.000000 -0.000000 0.000000 + + + 0.500000 0.500000 0.500000 + + + + + + 1 + 0.000000 + 0.000000 + + + 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 + + + + + 1 + 10.0 + box_bumper + world + + 0 + 10.000000 + 0 + + + 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 + + + + + 1 + 10.0 + box1_bumper + box1 + + 0 + 10.000000 + 0 + + 1 + 0 + 0 + + 0 + + + false + 0.000000 0.000000 0.000000 1.570800 -0.000000 0.000000 + + 0.000000 0.000000 0.000000 0.000000 -0.000000 0.000000 + + 0.000000 0.500000 0.000000 0.000000 -0.000000 0.000000 + + 10.000000 + 0.000000 + 0.000000 + 10.000000 + 0.000000 + 10.000000 + + 100.000000 + + + 0.000000 0.500000 0.000000 0.000000 -0.000000 0.000000 + + + 1.000000 1.000000 1.000000 + + + + + + -1.000000 + -1.000000 + 0.000000 0.000000 0.000000 + 0.000000 + 0.000000 + + + + 0.000000 + 100000.000000 + + + + 0.000000 + 0.200000 + 1000000000000.000000 + 1.000000 + 100.000000 + 0.001000 + + + + 0.000000 + + + 0.000000 0.500000 0.000000 0.000000 -0.000000 0.000000 + + + 1.000000 1.000000 1.000000 + + + + + + 1 + 0.000000 + 0.000000 + + 1 + 0 + 0 + + 0 + + + 0.000000 0.000000 8.000000 0.000000 -0.000000 0.000000 + 1.000000 1.000000 1.000000 1.000000 + 0.100000 0.100000 0.100000 1.000000 + + 10.000000 + 0.100000 + 0.200000 + 0.000000 + + 0.000000 0.000000 -1.000000 + 0 + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/elevator.world b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/elevator.world new file mode 100755 index 0000000..d30f20f --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/elevator.world @@ -0,0 +1,334 @@ + + + + + + + model://sun + + + + + model://ground_plane + + + + + + + region1 + + 1.5 -1 0 + 2.5 1 1 + + + + + + region2 + + 1.5 -1 3 + 2.5 1 4 + + + + + + region1_event + occupied + region1 + ~/elevator + 0 + + + + + region2_event + occupied + region2 + ~/elevator + 1 + + + + + + 0 0 0.075 0 0 0 + + + 800 + + + + + + 2.25 2.25 0.15 + + + + + + + 2.25 2.25 0.15 + + + + + + + 1.0745 0.5725 1.125 0 0 0 + + + 0.1 1.15 2.25 + + + + + 1.0745 0.5725 1.125 0 0 0 + + + 0.1 1.15 2.25 + + + + + + + + + + 1.0745 -1.0625 1.125 0 0 0 + + + 0.1 0.125 2.25 + + + + + 1.0745 -1.0625 1.125 0 0 0 + + + 0.1 0.125 2.25 + + + + + + + + + + 1.0745 -0.5 1.125 0 0 0 + + + + 0.08 1.0 2.25 + + + + + + + 0.08 1.0 2.25 + + + + + + + link + door + + 0 1 0 + + 0 + 1 + 10 + + + + 2 + + + + + + world + link + + 0 0 1 + + 0 + 10 + 100000 + + + + 50 + + + + + 1 + + + + + + + elevator::lift + elevator::door + 3.075 + + + 10 + + + elevator + + + + + + true + + + 0 1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + 0 1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + + 0 -1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + 0 -1.25 3 0 0 0 + + + 2.5 0.15 6 + + + + + + -1.25 0 3 0 0 0 + + + 0.15 2.7 6 + + + + + -1.25 0 3 0 0 0 + + + 0.15 2.7 6 + + + + + + 1.19 0 0.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + 1.19 0 0.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + + 1.19 0 3.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + 1.19 0 3.075 0 0 0 + + + 0.12 2.5 0.15 + + + + + + + + 2.25 0 0.075 0 0 0 + true + + + + + 2.0 5.0 0.15 + + + + + + + 2.0 5.0 0.15 + + + + + + + + 2.25 0 3.075 0 0 0 + true + + + + + 2.0 5.0 0.15 + + + + + + + 2.0 5.0 0.15 + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_block_laser.world b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_block_laser.world new file mode 100755 index 0000000..905b3fc --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_block_laser.world @@ -0,0 +1,660 @@ + + + + 0.5 0.5 0.5 1.0 + 0.5 0.5 0.5 1.0 + true + + 0.0 0.0 0.5 0.5 + linear + 2.0 + 10.0 + 0.5 + + + + 250 + + + quick +
0.001
+ 20 + 1.3 + + + 0 + 0.2 + 100.0 + 0.0 + + + 0.0000 0 0 + + + -1.5 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 0.05 0.05 + + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 0.05 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + 300 + 1.0 + -0.5236 + 0.5236 + + + 100 + 1.0 + -0.5236 + 0.5236 + + + + 0.05 + 50.0 + + + + + 0.00 + true + 20 + test_block_laser + base_link + + true + 10.0 + + false + true + false + + false + + + + + -2.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 0.05 0.05 + + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 0.05 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 2.57 + + 640 + 480 + R8G8B8 + + + + + + 1 + 10.0 + /test + image_raw + points + camera_info + base_link + 0.5 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + true + 10.0 + + false + true + false + + false + + + -0.5 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.1 0.0 0.0 0.0 0.0 + + + pr2/gripper_v0/l_finger_tip.stl + 3 3 3 + + + + true + 100.0 + + + 0.0 0.1 0.0 0.0 0.0 0.0 + 250 + + + pr2/gripper_v0/l_finger_tip.stl + 3 3 3 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.0 -0.1 0.0 0.0 0.0 0.0 + + + pr2/gripper_v0/l_finger_tip.dae + 3 3 3 + + + true + 100.0 + + + 0.0 -0.1 0.0 0.0 0.0 0.0 + 250 + + + pr2/gripper_v0/l_finger_tip.dae + 3 3 3 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + false + true + false + + false + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + -0.1 -0.5 -0.5 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.1 0.0 0.5 0.0 0.0 0.0 + + + 0.1 0.25 0.5 + + + + true + 100.0 + + + 0.1 0.0 0.5 0.0 0.0 0.0 + 250 + + + 0.1 0.25 0.5 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + true + true + false + + + 0.0 0.5 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 + + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + true + true + false + + + 0.5 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 + 0.5 + + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 + 0.5 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + true + true + false + + false + + + + 1.0 -0.1 -0.1 -0.1 -0.1 -0.1 + + 0 0 0.0 0 0 0 + + 0.012 + 0 + 0 + 0.015 + 0 + 0.011 + + 2.0 + + + + + chair/models/Chair.stl + 0.0254 0.0254 0.0254 + + + + + + 0.0 + 0.0 + 0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e10 + 1 + 100.0 + 0.0001 + + + + + + + + chair/models/Chair.dae + 0.0254 0.0254 0.0254 + + + false + + + false + + + 0.0 0.0 10.0 0.0 0.0 0.0 + 0.5 0.5 0.5 0.5 + + 1000.0 + 0.01 + 0.0001 + + 0 0 -1 + + 1.57 + 1.57 + 1.0 + + true + + + 1 + 1000.0 + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_camera.world b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_camera.world new file mode 100755 index 0000000..f4fe440 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_camera.world @@ -0,0 +1,146 @@ + + + + + model://sun + + + 0.0 0.0 -9.81 + + + quick + 20 + 1.3 + + + 0 + 0.2 + 100.0 + 0.0 + + + 250 + 0.001 + + + 0.0 0.0 1.0 0.0 0.0 0.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.5 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.5 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.0 0.0 0.0 1.0 0.0 0.0 + + 1.57 + + 256 + 160 + BAYER_BGGR8 + + + + + + 1 + 1.0 + image_raw + points + camera_info + world + 0.5 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + true + 2.0 + + false + true + false + + 2.0 0.0 0.0 0.0 0.0 0.0 + + + 1.0 1.0 1.0 + + + + 0.5 0.3 0.5 1.0 + + + true + 100.0 + + + false + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_depth_camera.world b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_depth_camera.world new file mode 100755 index 0000000..522a742 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_depth_camera.world @@ -0,0 +1,223 @@ + + + + 0.5 0.5 0.5 1.0 + 0.5 0.5 0.5 1.0 + true + + 0.0 0.0 0.5 0.5 + linear + 2.0 + 10.0 + 0.5 + + + + 0.0 0.0 0.0 + + + quick +
0.001
+ 20 + 1.3 + + + 0 + 0.2 + 100.0 + 0.0 + + + 250 + + + 0.0 0.0 5.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.5 + + + + 0.05 0.3 0.3 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.5 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.578 + + 640 + 480 + R8G8B8 + + + + + + + 1 + 10.0 + image_raw + points + camera_info + depth_cam + /base_link + 0.001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + true + 10.0 + + false + true + false + + false + + + 5.0 0.0 5.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1 2 2 + + + + 0.00 0.9 0.1 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 1 2 2 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + false + true + false + + false + + + 0.0 0.0 10.0 0.0 0.0 0.0 + 0.5 0.5 0.5 0.5 + + 1000.0 + 0.01 + 0.0001 + + 0 0 -1 + + 1.57 + 1.57 + 1.0 + + true + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_gpu_laser.world b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_gpu_laser.world new file mode 100755 index 0000000..4de0d73 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_gpu_laser.world @@ -0,0 +1,646 @@ + + + + 0.5 0.5 0.5 1.0 + 0.5 0.5 0.5 1.0 + + + 0.0 0.0 0.0 + + + quick +
0.001
+ 20 + 1.3 + + + 0 + 0.2 + 100.0 + 0.0 + + + 250 + + + 0.0 0.0 5.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.707 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 0.05 0.05 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 0.05 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + 640 + 1 + -1.396263 + 1.396263 + + + 1 + 0.5 + -0.7854 + 0.7854 + + + + 0.080000000000000002 + 10.0 + 0.01 + + + + 1 + 20.0 + 0.0 + image + laser_scan + /map + 0.001 + + true + 20.0 + + false + true + false + + true + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 3.0 0.0 5.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1 2 2 + + + + 0.5 0.03 0.03 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 1 2 2 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + false + true + false + + + 3.0 -0.5 6.8 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 3 0.5 0.5 + + + + 0.03 0.5 0.03 1.0 + + + true + 50.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 3 0.5 0.5 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 50.0 + + false + true + false + + + 0.0 3.0 5.0 0.0 0.0 0.7 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 2 2 2 + + + + 0.5 0.5 0.03 1.0 + + + true + 1.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 2 2 2 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + false + true + false + + + 2.0 -2.0 5.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1 + 2 + + + + 0.5 0.03 0.5 1.0 + + + true + 70.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 1 + 2 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + false + true + false + + + 2.0 2.0 5.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1 + + + + 0.03 0.5 0.5 1.0 + + + true + 35.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 1 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + false + true + false + + + -2.0 -2.0 5.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1 + + + + 0.03 0.03 0.5 1.0 + + + true + 35.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 1 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + false + true + false + + + -1.5 0.0 5.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1 2 2 + + + + 0.5 0.03 0.03 1.0 + + + true + 150.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 1 2 2 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + false + true + false + + false + + + 0.0 0.0 10.0 0.0 0.0 0.0 + 0.5 0.5 0.5 0.5 + + 1000.0 + 0.01 + 0.0001 + + 0 0 -1 + + 1.57 + 1.57 + 1.0 + + true + + + 1 + 1000.0 + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_laser.world b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_laser.world new file mode 100755 index 0000000..6ab1d46 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_laser.world @@ -0,0 +1,445 @@ + + + + 0.5 0.5 0.5 1.0 + 0.5 0.5 0.5 1.0 + true + + 0.0 0.0 0.5 0.5 + linear + 2.0 + 10.0 + 0.5 + + + + 250 + + + quick +
0.001
+ 20 + 1.3 + + + 0 + 0.2 + 100.0 + 0.0 + + + 0.0000 0 0 + + + -1.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 0.05 0.05 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 0.05 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + 641 + 1.0 + -0.5 + 0.5 + + + + + 0.15 + 30.0 + + + + + 0.005 + true + 20 + base_scan + base_link + + true + 2.0 + + false + true + false + + + + false + + + -0.2 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.1 0.0 0.0 0.0 0.0 + + + pr2/gripper_v0/l_finger_tip.stl + 1 1 1 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.1 0.0 0.0 0.0 0.0 + 250 + + + pr2/gripper_v0/l_finger_tip.stl + 1 1 1 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.0 -0.1 0.0 0.0 0.0 0.0 + + + pr2/gripper_v0/l_finger_tip.dae + 1 1 1 + + + true + 100.0 + + + 0.0 -0.1 0.0 0.0 0.0 0.0 + 250 + + + pr2/gripper_v0/l_finger_tip.dae + 1 1 1 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + false + true + false + + false + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.1 30.0 30.0 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.1 30.0 30.0 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + true + true + false + + false + + + -5.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 0.05 0.05 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 0.05 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + 1021 + 1.0 + -0.5 + 0.5 + + + + + 0.15 + 30.0 + + + + + 0.005 + true + 20 + tilt_scan + base_link + + true + 2.0 + + false + true + false + + false + + + 0.0 0.0 10.0 0.0 0.0 0.0 + 0.5 0.5 0.5 0.5 + + 1000.0 + 0.01 + 0.0001 + + 0 0 -1 + + 1.57 + 1.57 + 1.0 + + true + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_range.world b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_range.world new file mode 100755 index 0000000..8083ed7 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_range.world @@ -0,0 +1,231 @@ + + + + 0.5 0.5 0.5 1.0 + 0.5 0.5 0.5 1.0 + true + + 0.0 0.0 0.5 0.5 + linear + 2.0 + 10.0 + 0.5 + + + + 250 + + + quick + 20 + 1.3 + + + 0 + 0.2 + 100.0 + 0.0 + + + 0.0000 0 0 + 0.001 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 0.05 0.05 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 0.05 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.25 0 0 0 0 0 + 5 + + + + 5 + 1.0 + -0.25 + 0.25 + + + 5 + 1 + -0.25 + 0.25 + + + + 0.01 + 0.75 + 0.01 + + + + 0.005 + true + 5 + sonar + sonar_link + 0.5 + ultrasound + + + false + true + false + + false + + + + 0.5 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 0.05 0.05 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 0.05 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.25 0 0 0 0 0 + 5 + + + + 5 + 1.0 + -0.25 + 0.25 + + + 5 + 1 + -0.25 + 0.25 + + + + 0.01 + 0.75 + 0.01 + + + + 0.005 + true + 5 + sonar2 + sonar_link2 + 0.5 + ultrasound + + + false + true + false + + false + + + + 0.0 0.0 10.0 0.0 0.0 0.0 + 0.5 0.5 0.5 0.5 + + 1000.0 + 0.01 + 0.0001 + + 0 0 -1 + + 1.57 + 1.57 + 1.0 + + true + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_trimesh_collision.world b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_trimesh_collision.world new file mode 100755 index 0000000..c17d93f --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/gazebo_ros_trimesh_collision.world @@ -0,0 +1,552 @@ + + + + + 0.2 0.2 0.2 1 + 0.5 0.5 0.5 1 + false + + + 250 + + + quick +
0.001
+ 20 + 1.3 + + + 0 + 0.2 + 100.0 + 0.0 + + + 0.0000 0 0 + + + -1.5 1.8 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 0.05 0.05 + + + + 0.03 0.5 0.5 1.0 + + + false + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 0.05 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + + 300 + 1.0 + -0.5236 + 0.5236 + + + 100 + 1.0 + -0.5236 + 0.5236 + + + + 0.05 + 50.0 + + + + + 0.00 + true + 20 + test_block_laser + /base_link + + true + 10.0 + + false + true + false + + false + + + -1.5 1.8 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 0.05 0.05 + + + + 0.03 0.5 0.5 1.0 + + + false + 100.0 + + + -1.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 0.05 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.9 + + 640 + 480 + R8G8B8 + + + + + + 1 + 10.0 + /test_depth + image_raw + points + camera_info + /base_link + + true + 10.0 + + false + true + false + + false + + + 1.4 1.8 0 0 0 0 + + + 0 0 0.0 0 0 0 + + 0.012 + 0 + 0 + 0.015 + 0 + 0.011 + + 2.0 + + + + + chair3/model/chair.stl + 0.008 0.008 0.01 + + + + + + 0.0 + 0.0 + 0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e10 + 1 + 100.0 + 0.0001 + + + + + + + + chair3/model/chair.dae + 0.008 0.008 0.01 + + + false + + + true + + + 1.9 1.8 0 0 0 0 + + + 0 0 0.0 0 0 0 + + 0.012 + 0 + 0 + 0.015 + 0 + 0.011 + + 2.0 + + + + + chair3/model/chair.stl + 0.008 0.008 0.01 + + + + + + 0.0 + 0.0 + 0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e10 + 1 + 100.0 + 0.0001 + + + + + + + + chair3/model/chair.dae + 0.008 0.008 0.01 + + + false + + + true + + + 1.6 1.5 0 0 0 3.1416 + + + 0 0 0.0 0 0 0 + + 0.012 + 0 + 0 + 0.015 + 0 + 0.011 + + 2.0 + + + + + chair3/model/chair.stl + 0.008 0.008 0.01 + + + + + + 0.0 + 0.0 + 0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e10 + 1 + 100.0 + 0.0001 + + + + + + + + chair3/model/chair.dae + 0.008 0.008 0.01 + + + false + + + true + + + 2.1 1.5 0 0 0 3.1416 + + + 0 0 0 0 0 0 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.01 + + 1.0 + + + + + chair3/model/chair.stl + 0.008 0.008 0.01 + + + + + + 0.0 + 0.0 + 0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e10 + 1 + 100.0 + 0.0001 + + + + + + + + chair3/model/chair.dae + 0.008 0.008 0.01 + + + false + + + false + + + 2.3 1.8 0 0 0 -1.57 + + + 0 0 0 0 0 0 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.01 + + 1.0 + + + + + chair3/model/chair.stl + 0.008 0.008 0.01 + + + + + + 0.0 + 0.0 + 0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e10 + 1 + 100.0 + 0.0001 + + + + + + + + chair3/model/chair.dae + 0.008 0.008 0.01 + + + false + + + + false + + + 0.0 0.0 10.0 0.0 0.0 0.0 + 0.5 0.5 0.5 0.5 + + 1000.0 + 0.01 + 0.0001 + + 0 0 -1 + + 1.57 + 1.57 + 1.0 + + false + + + 1 + 1000.0 + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/test_lasers.world b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/test_lasers.world new file mode 100755 index 0000000..0397531 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/test_worlds/test_lasers.world @@ -0,0 +1,443 @@ + + + + 0.5 0.5 0.5 1.0 + 0.5 0.5 0.5 1.0 + true + + 0.0 0.0 0.5 0.5 + linear + 2.0 + 10.0 + 0.5 + + + + 250 + + + quick +
0.001
+ 20 + 1.3 + + + 0 + 0.2 + 100.0 + 0.0 + + + 0.0000 0 0 + + + -1.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 0.05 0.05 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 0.05 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0 0 0 0 -0 0 + + + + 640 + 1 + -1.3962634000002327 + 1.3962634000002327 + + + + 0.080000000000000002 + 10 + 0.01 + + + + 0.00 + 1 + 40 + tilt_scan + base_link + + 0 + 40 + + false + true + false + + + + false + + + -0.2 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.1 0.0 0.0 0.0 0.0 + + + pr2/gripper_v0/l_finger_tip.stl + 1 1 1 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.1 0.0 0.0 0.0 0.0 + 250 + + + pr2/gripper_v0/l_finger_tip.stl + 1 1 1 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0.0 -0.1 0.0 0.0 0.0 0.0 + + + pr2/gripper_v0/l_finger_tip.dae + 1 1 1 + + + true + 100.0 + + + 0.0 -0.1 0.0 0.0 0.0 0.0 + 250 + + + pr2/gripper_v0/l_finger_tip.dae + 1 1 1 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + false + true + false + + false + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.1 30.0 30.0 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.1 30.0 30.0 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + true + true + false + + false + + + -5.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 1.0 + 0.0 + 0.0 + 1.0 + 0.0 + 1.0 + + 10.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + + + 0.05 0.05 0.05 + + + + 0.03 0.5 0.5 1.0 + + + true + 100.0 + + + 0.0 0.0 0.0 0.0 0.0 0.0 + 250 + + + 0.05 0.05 0.05 + + + + + + 0.5 + 0.2 + 1.0 0 0 + 0 + 0 + + + + 0 + 1000000.0 + + + + 0 + 0.2 + 1e15 + 1e13 + 100.0 + 0.0001 + + + + 100.0 + + + 0 0 0 0 -0 0 + + + + 640 + 1 + -2.2688999999959423 + 2.2688999999959423 + + + + 0.080000000000000002 + 10 + 0.01 + + + + 0.00 + 1 + 20 + base_scan + base_link + + 0 + 20 + + false + true + false + + false + + + 0.0 0.0 10.0 0.0 0.0 0.0 + 0.5 0.5 0.5 0.5 + + 1000.0 + 0.01 + 0.0001 + + 0 0 -1 + + 1.57 + 1.57 + 1.0 + + true + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/.directory b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/.directory new file mode 100755 index 0000000..b0371cd --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/.directory @@ -0,0 +1,4 @@ +[Dolphin] +Timestamp=2013,11,4,9,14,4 +Version=3 +ViewMode=2 diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle.gazebo.launch b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle.gazebo.launch new file mode 100755 index 0000000..4e79e58 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle.gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle.rviz b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle.rviz new file mode 100755 index 0000000..49c0fc8 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle.rviz @@ -0,0 +1,237 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Marker1 + - /Marker1/Namespaces1 + - /LaserScan1 + Splitter Ratio: 0.590385 + Tree Height: 690 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.03 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + r1/base_link: + Value: true + r1/chassis: + Value: true + r1/front_steering: + Value: true + r1/front_wheel: + Value: true + r1/left_wheel: + Value: true + r1/odom: + Value: true + r1/right_wheel: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base_link: + r1/odom: + r1/base_link: + r1/chassis: + {} + r1/front_steering: + r1/front_wheel: + {} + r1/left_wheel: + {} + r1/right_wheel: + {} + Update Interval: 0 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: visualization_marker + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + chassis: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_steering: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: r1/robot_description + TF Prefix: r1 + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/LaserScan + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: LaserScan + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.01 + Style: Flat Squares + Topic: /r1/front_laser/scan + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: r1/base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Topic: /initialpose + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 7.43455 + Enable Stereo Rendering: + Stereo Eye Separation: 0.06 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.351517 + Y: 0.0334811 + Z: 0.722431 + Name: Current View + Near Clip Distance: 0.01 + Pitch: 0.234797 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.99819 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 1054 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000010f0000036bfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000450000034c000000ee00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610100000397000000190000001900000019fb0000000c00430061006d00650072006103000003360000023100000250000001b3fb0000000c00430061006d00650072006101000002c5000000c80000000000000000000000010000010f0000036bfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000450000036b000000c100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000068e00000046fc0100000002fb0000000800540069006d006501000000000000068e0000038b00fffffffb0000000800540069006d00650100000000000004500000000000000000000004640000036b00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1678 + X: 1920 + Y: 0 diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle.urdf.launch b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle.urdf.launch new file mode 100755 index 0000000..6325c8b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle.urdf.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch new file mode 100755 index 0000000..cd5691b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle_drive_scenario.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle_rviz.launch b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle_rviz.launch new file mode 100755 index 0000000..d2d5b2d --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/launch/tricycle_rviz.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/.directory b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/.directory new file mode 100755 index 0000000..b41f2a1 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/.directory @@ -0,0 +1,3 @@ +[Dolphin] +Timestamp=2013,10,18,16,23,17 +ViewMode=2 diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/materials.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/materials.xacro new file mode 100755 index 0000000..311c3cd --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/materials.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/inertia_tensors.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/inertia_tensors.xacro new file mode 100755 index 0000000..ed07e4e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/inertia_tensors.xacro @@ -0,0 +1,72 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle.xacro new file mode 100755 index 0000000..0434552 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle.xacro @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_body.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_body.xacro new file mode 100755 index 0000000..a6ea3db --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_body.xacro @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_chassis.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_chassis.xacro new file mode 100755 index 0000000..bced5fa --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_chassis.xacro @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_plugins.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_plugins.xacro new file mode 100755 index 0000000..cadd1f4 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/tricycle_plugins.xacro @@ -0,0 +1,36 @@ + + + + + + + + Debug + + false + true + true + front_steering_joint + front_wheel_joint + left_wheel_joint + right_wheel_joint + 0.135 + 0.135 + 0.548 + cmd_vel + odom + odom + encoder + base_link + 10.0 + 1.8 + 5.0 + 0.05 + 20 + 0.4 + 0.02 + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel.xacro new file mode 100755 index 0000000..3738ee3 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel.xacro @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel_actuated.xacro b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel_actuated.xacro new file mode 100755 index 0000000..1ae0f39 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test/tricycle_drive/xacro/tricycle/wheel_actuated.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/CMakeLists_tests_pkg.txt b/gazebo_ros_pkgs/gazebo_plugins/test2/CMakeLists_tests_pkg.txt new file mode 100755 index 0000000..95c9869 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/CMakeLists_tests_pkg.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) + +# Set the build type. Options are: +# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage +# Debug : w/ debug symbols, w/o optimization +# Release : w/o debug symbols, w/ optimization +# RelWithDebInfo : w/ debug symbols, w/ optimization +# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries +set(ROS_BUILD_TYPE RelWithDebInfo) + +rosbuild_init() + +#set the default path for built executables to the "bin" directory +set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) +#set the default path for built libraries to the "lib" directory +set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) + +#uncomment if you have defined messages +#rosbuild_genmsg() +#uncomment if you have defined services +#rosbuild_gensrv() + +rosbuild_add_boost_directories() + +rosbuild_add_executable(spawn_box test/spawn_model/spawn_box.cpp) +rosbuild_add_gtest_build_flags(spawn_box) +rosbuild_add_rostest_labeled(gazebo test/spawn_model/spawn_box.launch) + +rosbuild_add_executable(check_model test/spawn_model/check_model.cpp) +rosbuild_add_gtest_build_flags(check_model) +rosbuild_add_rostest_labeled(gazebo test/spawn_model/spawn_box_file.launch) +rosbuild_add_rostest_labeled(gazebo test/spawn_model/spawn_box_param.launch) + +rosbuild_add_executable(contact_tolerance test/contact_tolerance/contact_tolerance.cpp) +rosbuild_add_gtest_build_flags(contact_tolerance) +rosbuild_add_rostest_labeled(gazebo test/contact_tolerance/contact_tolerance.launch) + +#common commands for building c++ executables and libraries +#rosbuild_add_library(${PROJECT_NAME} src/example.cpp) +#target_link_libraries(${PROJECT_NAME} another_library) +#rosbuild_link_boost(${PROJECT_NAME} thread) +#rosbuild_add_executable(example examples/example.cpp) +#target_link_libraries(example ${PROJECT_NAME}) diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/contact_tolerance/contact_tolerance.cpp b/gazebo_ros_pkgs/gazebo_plugins/test2/contact_tolerance/contact_tolerance.cpp new file mode 100755 index 0000000..f9d6f40 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/contact_tolerance/contact_tolerance.cpp @@ -0,0 +1,135 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* 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 the Willow Garage 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: John Hsu */ + +#include +#include +#include "gazebo/SpawnModel.h" +#include "geometry_msgs/Pose.h" +#include "geometry_msgs/Quaternion.h" +#include "std_srvs/Empty.h" +#include "gazebo/GetModelState.h" + +#include +#include + +// Including ros, just to be able to call ros::init(), to remove unwanted +// args from command-line. +#include + +int g_argc; +char** g_argv; + +TEST(SpawnTest, checkBoxStackDrift) +{ + ros::NodeHandle nh(""); + // make the service call to spawn model + ros::service::waitForService("gazebo/spawn_urdf_model"); + ros::ServiceClient spawn_model_client = nh.serviceClient("gazebo/spawn_urdf_model"); + gazebo::SpawnModel spawn_model; + + // load urdf file + std::string urdf_filename = std::string(g_argv[1]); + ROS_DEBUG_NAMED("contact_tolerance", "loading file: %s",urdf_filename.c_str()); + // read urdf / gazebo model xml from file + TiXmlDocument xml_in(urdf_filename); + xml_in.LoadFile(); + std::ostringstream stream; + stream << xml_in; + spawn_model.request.model_xml = stream.str(); // load xml file + ROS_DEBUG_NAMED("contact_tolerance", "XML string: %s",stream.str().c_str()); + + spawn_model.request.robot_namespace = ""; + spawn_model.request.reference_frame = ""; + + // spawn 10 boxes + for (int i=0;i<10;i++) + { + std::ostringstream mn_stream; + mn_stream << "box_" << i; + spawn_model.request.model_name = mn_stream.str(); + geometry_msgs::Pose pose; + pose.position.x = pose.position.y = 0; pose.position.z = 1 + 2.0*(i); + pose.orientation.w = 1.0; pose.orientation.x = pose.orientation.y = pose.orientation.z = 0; + spawn_model.request.initial_pose = pose; + ASSERT_TRUE(spawn_model_client.call(spawn_model)); + } + + // get pose of top box, check for drift + // setup the service call to get model state + ros::service::waitForService("gazebo/get_model_state"); + ros::ServiceClient get_model_state_client = nh.serviceClient("gazebo/get_model_state"); + gazebo::GetModelState model_state; + model_state.request.model_name = "box_9"; + double test_duration = 10.0; // check drift over this period of sim time + double LINEAR_DRIFT_TOLERANCE = 0.0105; // linear drift tolerance of top block + // given contact layer is 1mm, 10 blocks leads to 10*1mm and .5mm of numerical error + ros::Time timeout = ros::Time::now() + ros::Duration(test_duration); + // get intial pose + ASSERT_TRUE(get_model_state_client.call(model_state)); + geometry_msgs::Pose initial_pose = model_state.response.pose; + while (ros::Time::now() < timeout) + { + ASSERT_TRUE(get_model_state_client.call(model_state)); + geometry_msgs::Pose current_pose = model_state.response.pose; + double error_x = fabs(current_pose.position.x - initial_pose.position.x); + double error_y = fabs(current_pose.position.y - initial_pose.position.y); + double error_z = fabs(current_pose.position.z - initial_pose.position.z); + double error_linear = sqrt(error_x*error_x+error_y*error_y+error_z*error_z); + ROS_INFO_NAMED("contact_tolerance", "error: %f",error_linear); + if (error_linear > LINEAR_DRIFT_TOLERANCE); + ROS_INFO_NAMED("contact_tolerance", "box stack teset failed with this linear error (%f): this means the top box in the box stack is moving too much. Check to see if surface contact layer has changed from 1mm, bounce parameter has changed, world time step, solver has changed. You can reproduce this test by:\n\nroslaunch gazebo_tests empty.launch\nrosrun gazebo_tests contact_tolerance `rospack find gazebo_tests`/test/urdf/box.urdf\n",error_linear); + ASSERT_TRUE(error_linear <= LINEAR_DRIFT_TOLERANCE); + + // FIXME: do something about orientation drift too + geometry_msgs::Quaternion current_q = current_pose.orientation; + geometry_msgs::Quaternion initial_q = initial_pose.orientation; + } + + + SUCCEED(); +} + + + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "test", ros::init_options::AnonymousName); + testing::InitGoogleTest(&argc, argv); + g_argc = argc; + g_argv = argv; + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/contact_tolerance/contact_tolerance.launch b/gazebo_ros_pkgs/gazebo_plugins/test2/contact_tolerance/contact_tolerance.launch new file mode 100755 index 0000000..97ca241 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/contact_tolerance/contact_tolerance.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/large_model.launch b/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/large_model.launch new file mode 100755 index 0000000..e33de64 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/large_model.launch @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/large_model.urdf.xacro b/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/large_model.urdf.xacro new file mode 100755 index 0000000..1a48120 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/large_model.urdf.xacro @@ -0,0 +1,270 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ${material} + false + + + + + 1 + 1 + + + + true + 100.0 + ${link_name} + ${link_name}_pose + map + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 1000.0 + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/large_models.world b/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/large_models.world new file mode 100755 index 0000000..e88a47c --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/large_models.world @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/smaller_large_model.launch b/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/smaller_large_model.launch new file mode 100755 index 0000000..cde878b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/smaller_large_model.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/smaller_large_model.urdf.xacro b/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/smaller_large_model.urdf.xacro new file mode 100755 index 0000000..a4caa13 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/large_models/smaller_large_model.urdf.xacro @@ -0,0 +1,242 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ${material} + false + + + + true + 100.0 + ${link_name} + ${link_name}_pose + map + + + + + + 1 + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 1000.0 + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/balance.launch b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/balance.launch new file mode 100755 index 0000000..010eb4a --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/balance.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/balance.world b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/balance.world new file mode 100755 index 0000000..e1e08ed --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/balance.world @@ -0,0 +1,186 @@ + + + + + + + + + + + + 15 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stack.launch b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stack.launch new file mode 100755 index 0000000..964e2cb --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stack.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stack.world b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stack.world new file mode 100755 index 0000000..802ff23 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stack.world @@ -0,0 +1,111 @@ + + + + + + + + + + + + 15 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stacks.launch b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stacks.launch new file mode 100755 index 0000000..57363ab --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stacks.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stacks.world b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stacks.world new file mode 100755 index 0000000..1cc37f8 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/lcp_tests/stacks.world @@ -0,0 +1,243 @@ + + + + + + + + + + + + 15 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/meshes/cube.wings b/gazebo_ros_pkgs/gazebo_plugins/test2/meshes/cube.wings new file mode 100755 index 0000000..fced736 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test2/meshes/cube.wings differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/meshes/cube_20k.stl b/gazebo_ros_pkgs/gazebo_plugins/test2/meshes/cube_20k.stl new file mode 100755 index 0000000..ae4e765 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test2/meshes/cube_20k.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/meshes/cube_30k.stl b/gazebo_ros_pkgs/gazebo_plugins/test2/meshes/cube_30k.stl new file mode 100755 index 0000000..3b3fd10 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_plugins/test2/meshes/cube_30k.stl differ diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/check_model.cpp b/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/check_model.cpp new file mode 100755 index 0000000..60d9008 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/check_model.cpp @@ -0,0 +1,112 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* 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 the Willow Garage 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: John Hsu */ + +#include +#include +#include "gazebo/GetWorldProperties.h" +#include "geometry_msgs/Pose.h" +#include "std_srvs/Empty.h" + +#include +#include +#include + +// Including ros, just to be able to call ros::init(), to remove unwanted +// args from command-line. +#include + +int g_argc; +char** g_argv; + +TEST(SpawnTest, spawnSingleBox) +{ + ros::NodeHandle nh(""); + // make the service call to spawn model + ros::service::waitForService("gazebo/get_world_properties"); + ros::ServiceClient check_model_client = nh.serviceClient("gazebo/get_world_properties"); + gazebo::GetWorldProperties world_properties; + + + bool found = false; + + // test duration + double test_duration = 10.0; // default to 10 seconds sim time + try + { + test_duration = boost::lexical_cast(g_argv[1]); + } + catch (boost::bad_lexical_cast &e) + { + ROS_ERROR_NAMED("check_model", "first argument of check_model should be timeout"); + return; + } + ros::Time timeout = ros::Time::now() + ros::Duration(test_duration); + + // model name to look for + std::string model_name = std::string(g_argv[2]); + ROS_INFO_NAMED("check_model", "looking for model: %s",model_name.c_str()); + + while (!found && ros::Time::now() < timeout) + { + check_model_client.call(world_properties); + for (std::vector::iterator mit = world_properties.response.model_names.begin(); + mit != world_properties.response.model_names.end(); + mit++) + { + if (*mit == model_name) + { + found = true; + break; + } + } + } + + ASSERT_TRUE(found); + + SUCCEED(); +} + + + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "test", ros::init_options::AnonymousName); + testing::InitGoogleTest(&argc, argv); + g_argc = argc; + g_argv = argv; + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box.cpp b/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box.cpp new file mode 100755 index 0000000..bf32a56 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box.cpp @@ -0,0 +1,163 @@ +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2008, Willow Garage, Inc. +* All rights reserved. +* +* 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 the Willow Garage 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: John Hsu */ + +#include +#include +#include "gazebo/SpawnModel.h" +#include "gazebo/GetWorldProperties.h" +#include "gazebo/DeleteModel.h" +#include "geometry_msgs/Pose.h" +#include "std_srvs/Empty.h" + +#include +#include + +// Including ros, just to be able to call ros::init(), to remove unwanted +// args from command-line. +#include + +int g_argc; +char** g_argv; + +TEST(SpawnTest, spawnSingleBox) +{ + ros::NodeHandle nh(""); + // make the service call to spawn model + ros::service::waitForService("gazebo/spawn_urdf_model"); + ros::ServiceClient spawn_model_client = nh.serviceClient("gazebo/spawn_urdf_model"); + gazebo::SpawnModel spawn_model; + spawn_model.request.model_name = "box1"; + + // load urdf file + std::string urdf_filename = std::string(g_argv[1]); + ROS_DEBUG_NAMED("spawn_box", "loading file: %s",urdf_filename.c_str()); + // read urdf / gazebo model xml from file + TiXmlDocument xml_in(urdf_filename); + xml_in.LoadFile(); + std::ostringstream stream; + stream << xml_in; + spawn_model.request.model_xml = stream.str(); // load xml file + ROS_DEBUG_NAMED("spawn_box", "XML string: %s",stream.str().c_str()); + + spawn_model.request.robot_namespace = ""; + geometry_msgs::Pose pose; + pose.position.x = pose.position.y = 0; pose.position.z = 1; + pose.orientation.w = 1.0; pose.orientation.x = pose.orientation.y = pose.orientation.z = 0; + spawn_model.request.initial_pose = pose; + spawn_model.request.reference_frame = ""; + + ASSERT_TRUE(spawn_model_client.call(spawn_model)); + + SUCCEED(); +} + + +TEST(SpawnTest, spawnBoxStack) +{ + ros::NodeHandle nh(""); + // make the service call to spawn model + ros::service::waitForService("gazebo/spawn_urdf_model"); + ros::ServiceClient spawn_model_client = nh.serviceClient("gazebo/spawn_urdf_model"); + gazebo::SpawnModel spawn_model; + + // load urdf file + std::string urdf_filename = std::string(g_argv[1]); + ROS_DEBUG_NAMED("spawn_box", "loading file: %s",urdf_filename.c_str()); + // read urdf / gazebo model xml from file + TiXmlDocument xml_in(urdf_filename); + xml_in.LoadFile(); + std::ostringstream stream; + stream << xml_in; + spawn_model.request.model_xml = stream.str(); // load xml file + ROS_DEBUG_NAMED("spawn_box", "XML string: %s",stream.str().c_str()); + + spawn_model.request.robot_namespace = ""; + spawn_model.request.reference_frame = ""; + + // spawn 10 boxes + for (int i=0;i<10;i++) + { + std::ostringstream mn_stream; + mn_stream << "box_" << i; + spawn_model.request.model_name = mn_stream.str(); + geometry_msgs::Pose pose; + pose.position.x = pose.position.y = 0; pose.position.z = 1 + 2.0*(i+1); + pose.orientation.w = 1.0; pose.orientation.x = pose.orientation.y = pose.orientation.z = 0; + spawn_model.request.initial_pose = pose; + ASSERT_TRUE(spawn_model_client.call(spawn_model)); + } + + SUCCEED(); +} + +TEST(DeleteTest, deleteAllModels) +{ + ros::NodeHandle nh(""); + // make the service call to delete model + ros::service::waitForService("gazebo/delete_model"); + ros::ServiceClient delete_model_client = nh.serviceClient("gazebo/delete_model"); + gazebo::DeleteModel delete_model; + + // model names to delete + ros::service::waitForService("gazebo/get_world_properties"); + ros::ServiceClient check_model_client = nh.serviceClient("gazebo/get_world_properties"); + gazebo::GetWorldProperties world_properties; + // get all models in the world and loop through deleting each one + check_model_client.call(world_properties); + for (std::vector::iterator mit = world_properties.response.model_names.begin(); + mit != world_properties.response.model_names.end(); + mit++) + { + delete_model.request.model_name = *mit; + ASSERT_TRUE(delete_model_client.call(delete_model)); + } + + // should have no more models in the world + check_model_client.call(world_properties); + ASSERT_TRUE(world_properties.response.model_names.empty()); + + SUCCEED(); +} + + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "test", ros::init_options::AnonymousName); + testing::InitGoogleTest(&argc, argv); + g_argc = argc; + g_argv = argv; + return RUN_ALL_TESTS(); +} diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box.launch b/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box.launch new file mode 100755 index 0000000..6123c56 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box_file.launch b/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box_file.launch new file mode 100755 index 0000000..de46c64 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box_file.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box_param.launch b/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box_param.launch new file mode 100755 index 0000000..6713191 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/spawn_model/spawn_box_param.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/trimesh_tests/test_trimesh.launch b/gazebo_ros_pkgs/gazebo_plugins/test2/trimesh_tests/test_trimesh.launch new file mode 100755 index 0000000..50b73c0 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/trimesh_tests/test_trimesh.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/urdf/box.urdf b/gazebo_ros_pkgs/gazebo_plugins/test2/urdf/box.urdf new file mode 100755 index 0000000..0a03771 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/urdf/box.urdf @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Blue + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/urdf/cube.urdf b/gazebo_ros_pkgs/gazebo_plugins/test2/urdf/cube.urdf new file mode 100755 index 0000000..527ae50 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/urdf/cube.urdf @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/GrayGrid + false + + diff --git a/gazebo_ros_pkgs/gazebo_plugins/test2/worlds/empty.world b/gazebo_ros_pkgs/gazebo_plugins/test2/worlds/empty.world new file mode 100755 index 0000000..b290d8c --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_plugins/test2/worlds/empty.world @@ -0,0 +1,130 @@ + + + + + 4 + 5 + + + + + + 0.001 + 0 0 -9.8 + 0.0000000001 + 0.2 + true + 10 + 1.3 + 100.0 + 0.001 + + + + 37.4270909558-122.077919338 + + + + fltk + 480 320 + 0 0 + + + + 0.3 0 3 + 0 90 90 + + + + + + + + 0.5 0.5 0.5 0.5 + + Gazebo/CloudySky + + false + 10. + none + false + + + + + 0 0 0 + 0 0 0 + true + + + + 2000.0 + 50.0 + 50.0 + 1000000000.0 + 1.0 + 0 0 1 + 51.3 51.3 + 10 10 + 100 100 + Gazebo/GrayGrid + + + + + + + + + + 0.0 0.0 8 + false + + point + 0.5 0.5 0.5 + .1 .1 .1 + 0.2 0.1 0 + 10 + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/CHANGELOG.rst b/gazebo_ros_pkgs/gazebo_ros/CHANGELOG.rst new file mode 100755 index 0000000..cbe3963 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/CHANGELOG.rst @@ -0,0 +1,409 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gazebo_ros +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.5.15 (2018-02-12) +------------------- +* Fix last gazebo8 warnings! (`#658 `_) +* Fix for relative frame errors (`#605 `_) +* Fix gazebo8 warnings part 10: ifdefs for GetModel, GetEntity, Light (`#656 `_) +* gazebo8 warnings: ifdefs for Get.*Vel() (`#653 `_) +* Prevents GAZEBO_MODEL_DATABASE_URI from being overwritten (`#644 `_) +* Fix gazebo8 warnings part 7: ifdef's for Joint::GetAngle and some cleanup (`#642 `_) +* Contributors: Hamza Merzić, R, Steven Peters + +2.5.14 (2017-12-11) +------------------- +* for gazebo8+, call functions without Get (`#639 `_) +* Fix gazebo8 warnings part 5: ignition math in gazebo_ros (`#635 `_) +* Fix gazebo8 warnings part 4: convert remaining local variables in plugins to ign-math (`#633 `_) +* gazebo_ros: fix support for python3 (`#622 `_) +* gazebo_ros_api_plugin: improve plugin xml parsing (`#625 `_) +* Replace Events::Disconnect* with pointer reset (`#623 `_) +* Install spawn_model using catkin_install_python (`#621 `_) +* [gazebo_ros] don't overwrite parameter "use_sim_time" (`#606 `_) + * Parameter /use_sim_time is only set if not present on Parameter Server +* Contributors: Jose Luis Rivero, Manuel Ilg, Mike Purvis, Nils Rokita, Steven Peters + +2.5.13 (2017-06-24) +------------------- +* Quote arguments to echo in libcommon.sh (`#590 `_) +* Add catkin package(s) to provide the default version of Gazebo (`#571 `_) + * Added catkin package gazebo_dev which provides the cmake config of the installed Gazebo version +* Contributors: Jose Luis Rivero, daewok + +2.5.12 (2017-04-25) +------------------- + +2.5.11 (2017-04-18) +------------------- +* Changed the spawn model methods to spawn also lights. (`#511 `_) +* Change build system to set DEPEND on Gazebo/SDFormat (fix catkin warning) + Added missing DEPEND clauses to catkin_package to fix gazebo catkin warning. + Note that after the change problems could appear related to -lpthreads + errors. This is an known issue related to catkin: + https://github.com/ros/catkin/issues/856. +* Use correct logerr method (`#557 `_) +* Contributors: Alessandro Ambrosano, Dave Coleman, Gary Servin + +2.5.10 (2017-03-03) +------------------- +* Revert catkin warnings to fix regressions (problems with catkin -lpthreads errors) + For reference and reasons, please check: + https://discourse.ros.org/t/need-to-sync-new-release-of-rqt-topic-indigo-jade-kinetic/1410/4 + * Revert "Fix gazebo catkin warning, cleanup CMakeLists (`#537 `_)" + This reverts commit 5a0305fcb97864b66bc2e587fc0564435b4f2034. + * Revert "Fix gazebo and sdformat catkin warnings" + This reverts commit 11f95d25dcd32faccd2401d45c722f7794c7542c. +* Contributors: Jose Luis Rivero + +2.5.9 (2017-02-20) +------------------ +* Fix gazebo catkin warning, cleanup CMakeLists (`#537 `_) +* Namespace console output (`#543 `_) +* Removed all trailing whitespace +* Contributors: Dave Coleman + +2.5.8 (2016-12-06) +------------------ +* Workaround to support gazebo and ROS arguments in the command line +* Fix ROS remapping by reverting "Remove ROS remapping arguments from gazebo_ros launch scripts. +* Fixed getlinkstate service's angular velocity return +* Honor GAZEBO_MASTER_URI in gzserver and gzclient +* Contributors: Jared, Jon Binney, Jordan Liviero, Jose Luis Rivero, Martin Pecka + +2.5.7 (2016-06-10) +------------------ + +2.5.6 (2016-04-28) +------------------ +* Remove deprecated spawn_gazebo_model service +* Contributors: Steven Peters + +2.5.5 (2016-04-27) +------------------ +* merge indigo, jade to kinetic-devel +* Upgrade to gazebo 7 and remove deprecated driver_base dependency + * Upgrade to gazebo 7 and remove deprecated driver_base dependency + * disable gazebo_ros_control until dependencies are met + * Remove stray backslash +* spawn_model: adding -b option to bond to the model and delete it on sigint +* Update maintainer for Kinetic release +* Allow respawning gazebo node. +* Contributors: Hugo Boyer, Isaac IY Saito, Jackie Kay, Jonathan Bohren, Jose Luis Rivero, Steven Peters + +2.5.3 (2016-04-11) +------------------ +* Include binary in runtime +* Remove ROS remapping arguments from gazebo_ros launch scripts. +* Contributors: Jose Luis Rivero, Martin Pecka + +2.5.2 (2016-02-25) +------------------ +* merging from indigo-devel +* Merge pull request `#302 `_ from maxbader/jade-devel-GetModelState + Header for GetModelState service request for jade-devel +* Fix invalid signal name on OS X + scripts/gazebo: line 30: kill: SIGINT: invalid signal specification +* Fix invalid signal name on OS X + scripts/gazebo: line 30: kill: SIGINT: invalid signal specification +* Restart package resolving from last position, do not start all over. +* 2.4.9 +* Generate changelog +* Import changes from jade-branch +* Add range world and launch file +* fix crash +* Set GAZEBO_CXX_FLAGS to fix c++11 compilation errors +* GetModelState modification for jade +* Contributors: Bence Magyar, Boris Gromov, Guillaume Walck, Ian Chen, John Hsu, Jose Luis Rivero, Markus Bader, Steven Peters, hsu + +2.5.1 (2015-08-16) +------------------ +* Port of Pal Robotics range sensor plugin to Jade +* Added a comment about the need of libgazebo5-dev in runtime +* Added missing files +* Added elevator plugin +* Use c++11 +* run_depend on libgazebo5-dev (`#323 `_) + Declare the dependency. + It can be fixed later if we don't want it. +* Contributors: Jose Luis Rivero, Nate Koenig, Steven Peters + +* Port of Pal Robotics range sensor plugin to Jade +* Added a comment about the need of libgazebo5-dev in runtime +* Added missing files +* Added elevator plugin +* Use c++11 +* run_depend on libgazebo5-dev +* Contributors: Jose Luis Rivero, Nate Koenig, Steven Peters + +2.5.0 (2015-04-30) +------------------ +* run_depend on libgazebo5-dev instead of gazebo5 +* Changed the rosdep key for gazebo to gazebo5, for Jade Gazebo5 will be used. +* Contributors: Steven Peters, William Woodall + +2.4.10 (2016-02-25) +------------------- +* Fix invalid signal name on OS X + scripts/gazebo: line 30: kill: SIGINT: invalid signal specification +* Restart package resolving from last position, do not start all over. +* Contributors: Boris Gromov, Guillaume Walck + +2.4.9 (2015-08-16) +------------------ +* Import changes from jade-branch +* Add range world and launch file +* fix crash +* Set GAZEBO_CXX_FLAGS to fix c++11 compilation errors +* Contributors: Bence Magyar, Ian Chen, Jose Luis Rivero, Steven Peters + +2.4.8 (2015-03-17) +------------------ +* Specify physics engine in args to empty_world.launch +* Contributors: Steven Peters + +2.4.7 (2014-12-15) +------------------ +* temporary hack to **fix** the -J joint position option (issue `#93 `_), sleeping for 1 second to avoid race condition. this branch should only be used for debugging, merge only as a last resort. +* Fixing set model state method and test +* Extended the fix for `#246 `_ also to debug, gazebo, gzclient and perf scripts. +* Update Gazebo/ROS tutorial URL +* [gazebo_ros] Fix for `#246 `_ + Fixing issue `#246 `_ in gzserver. +* Fixing handling of non-world frame velocities in setModelState. +* update headers to apache 2.0 license +* update headers to apache 2.0 license +* Contributors: John Hsu, Jose Luis Rivero, Martin Pecka, Tom Moore, ayrton04 + +2.4.6 (2014-09-01) +------------------ +* Merge pull request `#232 `_ from ros-simulation/fix_get_physics_properties_non_ode + Fix get physics properties non ode +* Merge pull request `#183 `_ from ros-simulation/issue_182 + Fix STL iterator errors, misc. cppcheck (`#182 `_) +* check physics engine type before calling set_physics_properties and get_physics_properteis +* check physics engine type before calling set_physics_properties and get_physics_properteis +* Fixes for calling GetParam() with different physic engines. +* 2.3.6 +* Update changelogs for the upcoming release +* Fixed boost any cast +* Removed a few warnings +* Update for hydro + gazebo 1.9 +* Fix build with gazebo4 and indigo +* Fix STL iterator errors, misc. cppcheck (`#182 `_) + There were some errors in STL iterators. + Initialized values of member variables in constructor. + Removed an unused variable (model_name). +* Contributors: Carlos Aguero, John Hsu, Jose Luis Rivero, Nate Koenig, Steven Peters, hsu, osrf + +2.4.5 (2014-08-18) +------------------ +* Port fix_build branch for indigo-devel + See pull request `#221 `_ +* Contributors: Jose Luis Rivero + +2.4.4 (2014-07-18) +------------------ +* Fix repo names in package.xml's +* fix issue `#198 `_ + Operator ``==`` is not recognized by sh scripts. +* Add verbose parameter + Add verbose parameter for --verbose gazebo flag +* added osx support for gazebo start scripts +* Contributors: Arn-O, Jon Binney, Markus Achtelik, Vincenzo Comito + +2.4.3 (2014-05-12) +------------------ +* added osx support for gazebo start scripts +* Remove gazebo_ros dependency on gazebo_plugins +* Contributors: Markus Achtelik, Steven Peters + +2.4.2 (2014-03-27) +------------------ +* merging from hydro-devel +* bump patch version for indigo-devel to 2.4.1 +* merging from indigo-devel after 2.3.4 release +* "2.4.0" +* catkin_generate_changelog +* Contributors: John Hsu + +2.4.1 (2013-11-13) +------------------ + +2.3.5 (2014-03-26) +------------------ +* gazebo_ros: [less-than-minor] fix newlines +* gazebo_ros: remove assignment to self + If this is needed for any twisted reason, it should be made clear + anyway. Assuming this line is harmless and removing it because it + generates cppcheck warnings. +* Contributors: Paul Mathieu + +2.3.4 (2013-11-13) +------------------ +* rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. +* remove debug statement +* fix sdf spawn with initial pose +* fix sdf spawn with initial pose +* Merge branch 'hydro-devel' into ``spawn_model_pose_fix`` +* fix indentation +* Merge pull request `#142 `_ from hsu/hydro-devel + fix issue `#38 `_, gui segfault on model deletion +* Merge pull request `#140 `_ from ``v4hn/spawn_model_sleep`` + replace time.sleep by rospy.Rate.sleep +* fix spawn initial pose. When model has a non-zero initial pose and user specified initial model spawn pose, add the two. +* fix issue `#38 `_, gui segfault on model deletion by removing an obsolete call to set selected object state to "normal". +* replace time.sleep by rospy.Rate.sleep + time was not even imported, so I don't know + why this could ever have worked... +* Add time import + When using the -wait option the script fails because is missing the time import +* Use pre-increment for iterators +* Fix iterator erase() problems + +2.4.0 (2013-10-14) +------------------ + +2.3.3 (2013-10-10) +------------------ +* Cleaned up unnecessary debug output that was recently added +* Fixed issue where ``catkin_find`` returns more than one library if it is installed from both source and debian + +2.3.2 (2013-09-19) +------------------ +* Make gazebo includes use full path + In the next release of gazebo, it will be required to use the + full path for include files. For example, + `include ` will not be valid + `include ` must be done instead. +* update gazebo includes +* Fixed a minor typo in spawn_model error message when `-model` not specified + +2.3.1 (2013-08-27) +------------------ +* Cleaned up template, fixes for header files + +2.3.0 (2013-08-12) +------------------ +* gazebo_ros: fixed missing dependency on TinyXML +* gazebo_plugins: replace deprecated boost function + This is related to `this gazebo issue `_ + +2.2.1 (2013-07-29) +------------------ + +2.2.0 (2013-07-29) +------------------ +* Switched to pcl_conversions +* Remove find_package(SDF) from CMakeLists.txt + It is sufficient to find gazebo, which will export the information + about the SDFormat package. + +2.1.5 (2013-07-18) +------------------ +* gazebo_ros: fixed variable names in gazebo_ros_paths_plugin + +2.1.4 (2013-07-14) +------------------ + +2.1.3 (2013-07-13) +------------------ + +2.1.2 (2013-07-12) +------------------ +* Added author +* Tweak to make SDFConfig.cmake +* Cleaned up CMakeLists.txt for all gazebo_ros_pkgs +* Cleaned up gazebo_ros_paths_plugin +* 2.1.1 + +2.1.1 (2013-07-10 19:11) +------------------------ +* Merge branch 'hydro-devel' of github.com:ros-simulation/gazebo_ros_pkgs into hydro-devel +* Reduced number of debug msgs +* Fixed physics dynamic reconfigure namespace +* gazebo_ros_api_plugin: set `plugin_loaded_` flag to true in + GazeboRosApiPlugin::Load() function +* Actually we need `__init__.py` +* Cleaning up code +* Moved gazebo_interface.py from gazebo/ folder to gazebo_ros/ folder +* Removed searching for plugins under 'gazebo' pkg because of rospack warnings +* Minor print modification +* Added dependency to prevent missing msg header, cleaned up CMakeLists + +2.1.0 (2013-06-27) +------------------ +* gazebo_ros: added deprecated warning for packages that use gazebo as + package name for exported paths +* Hiding some debug info +* gazebo_ros: use rosrun in debug script, as rospack find gazebo_ros returns the wrong path in install space +* Hide Model XML debut output to console +* gazebo_ros_api_plugin.h is no longer exposed in the include folder +* Added args to launch files, documentation +* Merge pull request `#28 `_ from osrf/no_roscore_handling + Better handling of gazebo_ros run when no roscore started +* gazebo_ros: also support gazebo instead of gazebo_ros as package name for plugin_path, gazebo_model_path or gazebo_media_path exports +* gazebo_plugins/gazebo_ros: fixed install directories for include files and gazebo scripts +* changed comment location +* added block comments for walkChildAddRobotNamespace +* SDF and URDF now set robotNamespace for plugins +* Better handling of gazebo_ros run when no roscore started + +2.0.2 (2013-06-20) +------------------ +* Added Gazebo dependency +* changed the final kill to send a SIGINT and ensure only the last background process is killed. +* modified script to work in bash correctly (tested on ubuntu 12.04 LTS) + +2.0.1 (2013-06-19) +------------------ +* Incremented version to 2.0.1 +* Fixed circular dependency, removed deprecated pkgs since its a stand alone pkg +* Shortened line lengths of function headers + +2.0.0 (2013-06-18) +------------------ +* Changed version to 2.0.0 based on gazebo_simulator being 1.0.0 +* Updated package.xml files for ros.org documentation purposes +* Combined updateSDFModelPose and updateSDFName, added ability to spawn SDFs from model database, updates SDF version to lastest in parts of code, updated the tests +* Renamed Gazebo model to SDF model, added ability to spawn from online database +* Fixed really obvious error checking bug +* Deprecated -gazebo arg in favor of -sdf tag +* Reordered services and messages to be organized and reflect documentation. No code change +* Cleaned up file, addded debug info +* Merged changes from Atlas ROS plugins, cleaned up headers +* Small fixes per ffurrer's code review +* Deprecated warnings fixes +* Cleaned up comment blocks - removed from .cpp and added to .h +* Merged branches and more small cleanups +* Small compile error fix +* Standardized function and variable naming convention, cleaned up function comments +* Reduced debug output and refresh frequency of robot spawner +* Converted all non-Gazebo pointers to boost shared_ptrs +* Removed old Gazebo XML handling functions - has been replaced by SDF, various code cleanup +* Removed the physics reconfigure node handle, switched to async ROS spinner, reduced required while loops +* Fixed shutdown segfault, renamed `rosnode_` to `nh_`, made all member variables have `_` at end, formatted functions +* Added small comment +* adding install for gazebo_ros launchfiles +* Formatted files to be double space indent per ROS standards +* Started fixing thread issues +* Fixing install script names and adding gzserver and gdbrun to install command +* Fixed deprecated warnings, auto formatted file +* Cleaned up status messages +* Added -h -help --help arguemnts to spawn_model +* Removed broken worlds +* Removed deprecated namespace argument +* Using pkg-config to find the script installation path. + Corrected a bash typo with client_final variable in gazebo script. +* Cleaning up world files +* Deprecated fix +* Moved from gazebo_worlds +* Cleaning up launch files +* Moved from gazebo_worlds +* Fixing renaming errors +* Updated launch and world files and moved to gazebo_ros +* Combined gzclient and gzserver +* Added finished loading msg +* All packages building in Groovy/Catkin +* Imported from bitbucket.org diff --git a/gazebo_ros_pkgs/gazebo_ros/CMakeLists.txt b/gazebo_ros_pkgs/gazebo_ros/CMakeLists.txt new file mode 100755 index 0000000..4859288 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/CMakeLists.txt @@ -0,0 +1,109 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gazebo_ros) + +find_package(catkin REQUIRED COMPONENTS + gazebo_dev + cmake_modules + roslib + roscpp + geometry_msgs + std_srvs + tf + rosgraph_msgs + dynamic_reconfigure + std_msgs + gazebo_msgs +) + +include (FindPkgConfig) +if (PKG_CONFIG_FOUND) + pkg_check_modules(XML libxml-2.0) +else() + message(FATAL_ERROR "pkg-config is required; please install it") +endif() + +find_package(Boost REQUIRED COMPONENTS thread) + +find_package(TinyXML REQUIRED) + +catkin_python_setup() + +generate_dynamic_reconfigure_options(cfg/Physics.cfg) + +catkin_package( + LIBRARIES + gazebo_ros_api_plugin + gazebo_ros_paths_plugin + + CATKIN_DEPENDS + roslib + roscpp + geometry_msgs + std_srvs + tf + rosgraph_msgs + dynamic_reconfigure + std_msgs + gazebo_msgs + + DEPENDS + TinyXML +) + +include_directories( + include + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${TinyXML_INCLUDE_DIRS}) + +link_directories(${catkin_LIBRARY_DIRS}) + +set(cxx_flags) +foreach (item ${GAZEBO_CFLAGS}) + set(cxx_flags "${cxx_flags} ${item}") +endforeach () + +set(ld_flags) +foreach (item ${GAZEBO_LDFLAGS}) + set(ld_flags "${ld_flags} ${item}") +endforeach () + +## Plugins +add_library(gazebo_ros_api_plugin src/gazebo_ros_api_plugin.cpp) +add_dependencies(gazebo_ros_api_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +set_target_properties(gazebo_ros_api_plugin PROPERTIES LINK_FLAGS "${ld_flags}") +set_target_properties(gazebo_ros_api_plugin PROPERTIES COMPILE_FLAGS "${cxx_flags}") +target_link_libraries(gazebo_ros_api_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${TinyXML_LIBRARIES}) + +add_library(gazebo_ros_paths_plugin src/gazebo_ros_paths_plugin.cpp) +add_dependencies(gazebo_ros_paths_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +set_target_properties(gazebo_ros_paths_plugin PROPERTIES COMPILE_FLAGS "${cxx_flags}") +set_target_properties(gazebo_ros_paths_plugin PROPERTIES LINK_FLAGS "${ld_flags}") +target_link_libraries(gazebo_ros_paths_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +# Install Gazebo System Plugins +install(TARGETS gazebo_ros_api_plugin gazebo_ros_paths_plugin + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ) + +# Install Gazebo Scripts +install(PROGRAMS scripts/gazebo + scripts/debug + scripts/gzclient + scripts/gzserver + scripts/gdbrun + scripts/perf + scripts/libcommon.sh + DESTINATION + ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# This one is a Python program, not a shell script, so install it separately +catkin_install_python(PROGRAMS scripts/spawn_model + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Install Gazebo launch files +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +) diff --git a/gazebo_ros_pkgs/gazebo_ros/cfg/Physics.cfg b/gazebo_ros_pkgs/gazebo_ros/cfg/Physics.cfg new file mode 100755 index 0000000..ca4ae0e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/cfg/Physics.cfg @@ -0,0 +1,59 @@ +#! /usr/bin/env python +#* All rights reserved. +#* +#* 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 the Willow Garage 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: John Hsu +# Gazebo ode physics configuration + +PACKAGE='gazebo_ros' + +from math import pi + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() +# Name Type Reconfiguration level Description Default Min Max +gen.add( "time_step" , double_t, 1 , "Simulation world time step size in seconds, smaller time steps producesslower, more stable simulation." , 0.001 , 0 , 10) +gen.add( "max_update_rate" , double_t, 1 , "Simulator max update rate, -1 unlimited, 1 restricts to real-time if possible." , 60 , 0 , 1000) +gen.add( "gravity_x" , double_t, 1 , "Simulated gravity in the x direction." , 0 , -100, 100) +gen.add( "gravity_y" , double_t, 1 , "Simulated gravity in the y direction." , 0 , -100, 100) +gen.add( "gravity_z" , double_t, 1 , "Simulated gravity in the z direction." , -9.8 , -100, 100) +gen.add( "auto_disable_bodies" , bool_t , 1 , "Auto disable of bodies in simulation if body it's not moving." , False) +gen.add( "sor_pgs_precon_iters" , int_t , 1 , "Number of preconditioning iterations for SOR PGS LCP as implemented in quickstep." , 0 , 0 , 10000) +gen.add( "sor_pgs_iters" , int_t , 1 , "Number of iterations for SOR PGS LCP as implemented in quickstep." , 20 , 0 , 10000) +gen.add( "sor_pgs_w" , double_t, 1 , "Relaxation parameter for SOR PGS LCP, usually set to 1.3, but reduce to stabilize simulation." , 1.3 , 0 , 5) +gen.add( "sor_pgs_rms_error_tol" , double_t, 1 , "The number of scans to skip between each measured scan" , -1 , -1 , 10000) +gen.add( "cfm" , double_t, 1 , "Constraint Force Mixing per ODE's users manual." , 0 , 0 , 10) +gen.add( "erp" , double_t, 1 , "Error Reduction Parameter per ODE's users manual." , 0.2 , 0 , 10) +gen.add( "contact_surface_layer" , double_t, 1 , "Margin for penetration for which restorative forces are not applied." , 0.001 , 0 , 10) +gen.add( "contact_max_correcting_vel", double_t, 1 , "Maximum contact penetration correction velocity." , 100 , 0 , 10000000) +gen.add( "max_contacts" , int_t , 1 , "Maximum number of contacts between any 2 bodies." , 100 , 0 , 10000000) + +exit(gen.generate(PACKAGE, "physics_node", "Physics")) diff --git a/gazebo_ros_pkgs/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h b/gazebo_ros_pkgs/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h new file mode 100755 index 0000000..b35b008 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/include/gazebo_ros/gazebo_ros_api_plugin.h @@ -0,0 +1,418 @@ +/* + * 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: External interfaces for Gazebo + * Author: Nate Koenig, John Hsu, Dave Coleman + * Date: 25 Apr 2010 + */ + +#ifndef __GAZEBO_ROS_API_PLUGIN_HH__ +#define __GAZEBO_ROS_API_PLUGIN_HH__ + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include + +// ROS +#include +#include +#include +#include +#include + +// Services +#include "std_srvs/Empty.h" + +#include "gazebo_msgs/JointRequest.h" +#include "gazebo_msgs/BodyRequest.h" + +#include "gazebo_msgs/SpawnModel.h" +#include "gazebo_msgs/DeleteModel.h" +#include "gazebo_msgs/DeleteLight.h" + +#include "gazebo_msgs/ApplyBodyWrench.h" + +#include "gazebo_msgs/SetPhysicsProperties.h" +#include "gazebo_msgs/GetPhysicsProperties.h" + +#include "gazebo_msgs/SetJointProperties.h" + +#include "gazebo_msgs/GetWorldProperties.h" + +#include "gazebo_msgs/GetModelProperties.h" +#include "gazebo_msgs/GetModelState.h" +#include "gazebo_msgs/SetModelState.h" + +#include "gazebo_msgs/GetJointProperties.h" +#include "gazebo_msgs/ApplyJointEffort.h" + +#include "gazebo_msgs/GetLinkProperties.h" +#include "gazebo_msgs/SetLinkProperties.h" +#include "gazebo_msgs/SetLinkState.h" +#include "gazebo_msgs/GetLinkState.h" + +#include "gazebo_msgs/GetLightProperties.h" +#include "gazebo_msgs/SetLightProperties.h" + +// Topics +#include "gazebo_msgs/ModelState.h" +#include "gazebo_msgs/LinkState.h" +#include "gazebo_msgs/ModelStates.h" +#include "gazebo_msgs/LinkStates.h" + +#include "geometry_msgs/Vector3.h" +#include "geometry_msgs/Wrench.h" +#include "geometry_msgs/Pose.h" +#include "geometry_msgs/Twist.h" + +// For model pose transform to set custom joint angles +#include +#include +#include + +// For physics dynamics reconfigure +#include +#include +#include "gazebo_msgs/SetPhysicsProperties.h" +#include "gazebo_msgs/GetPhysicsProperties.h" + +#include + +namespace gazebo +{ + +/// \brief A plugin loaded within the gzserver on startup. +class GazeboRosApiPlugin : public SystemPlugin +{ +public: + /// \brief Constructor + GazeboRosApiPlugin(); + + /// \brief Destructor + ~GazeboRosApiPlugin(); + + /// \bried Detect if sig-int shutdown signal is recieved + void shutdownSignal(); + + /// \brief Gazebo-inherited load function + /// + /// Called before Gazebo is loaded. Must not block. + /// Capitalized per Gazebo cpp style guidelines + /// \param _argc Number of command line arguments. + /// \param _argv Array of command line arguments. + void Load(int argc, char** argv); + + /// \brief ros queue thread for this node + void gazeboQueueThread(); + + /// \brief advertise services + void advertiseServices(); + + /// \brief + void onLinkStatesConnect(); + + /// \brief + void onModelStatesConnect(); + + /// \brief + void onLinkStatesDisconnect(); + + /// \brief + void onModelStatesDisconnect(); + + /// \brief Function for inserting a URDF into Gazebo from ROS Service Call + bool spawnURDFModel(gazebo_msgs::SpawnModel::Request &req, + gazebo_msgs::SpawnModel::Response &res); + + /// \brief Both SDFs and converted URDFs get sent to this function for further manipulation from a ROS Service call + bool spawnSDFModel(gazebo_msgs::SpawnModel::Request &req, + gazebo_msgs::SpawnModel::Response &res); + + /// \brief delete model given name + bool deleteModel(gazebo_msgs::DeleteModel::Request &req,gazebo_msgs::DeleteModel::Response &res); + + /// \brief delete a given light by name + bool deleteLight(gazebo_msgs::DeleteLight::Request &req,gazebo_msgs::DeleteLight::Response &res); + + /// \brief + bool getModelState(gazebo_msgs::GetModelState::Request &req,gazebo_msgs::GetModelState::Response &res); + + /// \brief + bool getModelProperties(gazebo_msgs::GetModelProperties::Request &req,gazebo_msgs::GetModelProperties::Response &res); + + /// \brief + bool getWorldProperties(gazebo_msgs::GetWorldProperties::Request &req,gazebo_msgs::GetWorldProperties::Response &res); + + /// \brief + bool getJointProperties(gazebo_msgs::GetJointProperties::Request &req,gazebo_msgs::GetJointProperties::Response &res); + + /// \brief + bool getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req,gazebo_msgs::GetLinkProperties::Response &res); + + /// \brief + bool getLinkState(gazebo_msgs::GetLinkState::Request &req,gazebo_msgs::GetLinkState::Response &res); + + /// \brief + bool getLightProperties(gazebo_msgs::GetLightProperties::Request &req,gazebo_msgs::GetLightProperties::Response &res); + + /// \brief + bool setLightProperties(gazebo_msgs::SetLightProperties::Request &req,gazebo_msgs::SetLightProperties::Response &res); + + /// \brief + bool setLinkProperties(gazebo_msgs::SetLinkProperties::Request &req,gazebo_msgs::SetLinkProperties::Response &res); + + /// \brief + bool setPhysicsProperties(gazebo_msgs::SetPhysicsProperties::Request &req,gazebo_msgs::SetPhysicsProperties::Response &res); + + /// \brief + bool getPhysicsProperties(gazebo_msgs::GetPhysicsProperties::Request &req,gazebo_msgs::GetPhysicsProperties::Response &res); + + /// \brief + bool setJointProperties(gazebo_msgs::SetJointProperties::Request &req,gazebo_msgs::SetJointProperties::Response &res); + + /// \brief + bool setModelState(gazebo_msgs::SetModelState::Request &req,gazebo_msgs::SetModelState::Response &res); + + /// \brief + void updateModelState(const gazebo_msgs::ModelState::ConstPtr& model_state); + + /// \brief + bool applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req,gazebo_msgs::ApplyJointEffort::Response &res); + + /// \brief + bool resetSimulation(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res); + + /// \brief + bool resetWorld(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res); + + /// \brief + bool pausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res); + + /// \brief + bool unpausePhysics(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res); + + /// \brief + bool clearJointForces(gazebo_msgs::JointRequest::Request &req,gazebo_msgs::JointRequest::Response &res); + bool clearJointForces(std::string joint_name); + + /// \brief + bool clearBodyWrenches(gazebo_msgs::BodyRequest::Request &req,gazebo_msgs::BodyRequest::Response &res); + bool clearBodyWrenches(std::string body_name); + + /// \brief + bool setModelConfiguration(gazebo_msgs::SetModelConfiguration::Request &req,gazebo_msgs::SetModelConfiguration::Response &res); + + /// \brief + bool setLinkState(gazebo_msgs::SetLinkState::Request &req,gazebo_msgs::SetLinkState::Response &res); + + /// \brief + void updateLinkState(const gazebo_msgs::LinkState::ConstPtr& link_state); + + /// \brief + bool applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &req,gazebo_msgs::ApplyBodyWrench::Response &res); + +private: + + /// \brief + void wrenchBodySchedulerSlot(); + + /// \brief + void forceJointSchedulerSlot(); + + /// \brief + void publishSimTime(const boost::shared_ptr &msg); + void publishSimTime(); + + /// \brief + void publishLinkStates(); + + /// \brief + void publishModelStates(); + + /// \brief + void stripXmlDeclaration(std::string &model_xml); + + /// \brief Update the model name and pose of the SDF file before sending to Gazebo + void updateSDFAttributes(TiXmlDocument &gazebo_model_xml, + const std::string &model_name, + const ignition::math::Vector3d &initial_xyz, + const ignition::math::Quaterniond &initial_q); + + /// \brief Update the model pose of the URDF file before sending to Gazebo + void updateURDFModelPose(TiXmlDocument &gazebo_model_xml, + const ignition::math::Vector3d &initial_xyz, + const ignition::math::Quaterniond &initial_q); + + /// \brief Update the model name of the URDF file before sending to Gazebo + void updateURDFName(TiXmlDocument &gazebo_model_xml, const std::string &model_name); + + /// \brief + void walkChildAddRobotNamespace(TiXmlNode* model_xml); + + /// \brief + bool spawnAndConform(TiXmlDocument &gazebo_model_xml, const std::string &model_name, + gazebo_msgs::SpawnModel::Response &res); + + /// \brief helper function for applyBodyWrench + /// shift wrench from reference frame to target frame + void transformWrench(ignition::math::Vector3d &target_force, ignition::math::Vector3d &target_torque, + const ignition::math::Vector3d &reference_force, + const ignition::math::Vector3d &reference_torque, + const ignition::math::Pose3d &target_to_reference ); + + /// \brief Used for the dynamic reconfigure callback function template + void physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level); + + /// \brief waits for the rest of Gazebo to be ready before initializing the dynamic reconfigure services + void physicsReconfigureThread(); + + /// \brief Unused + void onResponse(ConstResponsePtr &response); + + /// \brief utility for checking if string is in URDF format + bool isURDF(std::string model_xml); + + /// \brief utility for checking if string is in SDF format + bool isSDF(std::string model_xml); + + /// \brief Connect to Gazebo via its plugin interface, get a pointer to the world, start events + void loadGazeboRosApiPlugin(std::string world_name); + + /// \brief convert xml to Pose + ignition::math::Pose3d parsePose(const std::string &str); + + /// \brief convert xml to Pose + ignition::math::Vector3d parseVector3(const std::string &str); + + // track if the desconstructor event needs to occur + bool plugin_loaded_; + + // detect if sigint event occurs + bool stop_; + gazebo::event::ConnectionPtr sigint_event_; + + std::string robot_namespace_; + + gazebo::transport::NodePtr gazebonode_; + gazebo::transport::SubscriberPtr stat_sub_; + gazebo::transport::PublisherPtr factory_pub_; + gazebo::transport::PublisherPtr factory_light_pub_; + gazebo::transport::PublisherPtr light_modify_pub_; + gazebo::transport::PublisherPtr request_pub_; + gazebo::transport::SubscriberPtr response_sub_; + + boost::shared_ptr nh_; + ros::CallbackQueue gazebo_queue_; + boost::shared_ptr gazebo_callback_queue_thread_; + + gazebo::physics::WorldPtr world_; + gazebo::event::ConnectionPtr wrench_update_event_; + gazebo::event::ConnectionPtr force_update_event_; + gazebo::event::ConnectionPtr time_update_event_; + gazebo::event::ConnectionPtr pub_link_states_event_; + gazebo::event::ConnectionPtr pub_model_states_event_; + gazebo::event::ConnectionPtr load_gazebo_ros_api_plugin_event_; + + ros::ServiceServer spawn_sdf_model_service_; + ros::ServiceServer spawn_urdf_model_service_; + ros::ServiceServer delete_model_service_; + ros::ServiceServer delete_light_service_; + ros::ServiceServer get_model_state_service_; + ros::ServiceServer get_model_properties_service_; + ros::ServiceServer get_world_properties_service_; + ros::ServiceServer get_joint_properties_service_; + ros::ServiceServer get_link_properties_service_; + ros::ServiceServer get_link_state_service_; + ros::ServiceServer get_light_properties_service_; + ros::ServiceServer set_light_properties_service_; + ros::ServiceServer set_link_properties_service_; + ros::ServiceServer set_physics_properties_service_; + ros::ServiceServer get_physics_properties_service_; + ros::ServiceServer apply_body_wrench_service_; + ros::ServiceServer set_joint_properties_service_; + ros::ServiceServer set_model_state_service_; + ros::ServiceServer apply_joint_effort_service_; + ros::ServiceServer set_model_configuration_service_; + ros::ServiceServer set_link_state_service_; + ros::ServiceServer reset_simulation_service_; + ros::ServiceServer reset_world_service_; + ros::ServiceServer pause_physics_service_; + ros::ServiceServer unpause_physics_service_; + ros::ServiceServer clear_joint_forces_service_; + ros::ServiceServer clear_body_wrenches_service_; + ros::Subscriber set_link_state_topic_; + ros::Subscriber set_model_state_topic_; + ros::Publisher pub_link_states_; + ros::Publisher pub_model_states_; + int pub_link_states_connection_count_; + int pub_model_states_connection_count_; + + // ROS comm + boost::shared_ptr async_ros_spin_; + + // physics dynamic reconfigure + boost::shared_ptr physics_reconfigure_thread_; + bool physics_reconfigure_initialized_; + ros::ServiceClient physics_reconfigure_set_client_; + ros::ServiceClient physics_reconfigure_get_client_; + boost::shared_ptr< dynamic_reconfigure::Server > physics_reconfigure_srv_; + dynamic_reconfigure::Server::CallbackType physics_reconfigure_callback_; + + ros::Publisher pub_clock_; + int pub_clock_frequency_; + gazebo::common::Time last_pub_clock_time_; + + /// \brief A mutex to lock access to fields that are used in ROS message callbacks + boost::mutex lock_; + + bool world_created_; + + class WrenchBodyJob + { + public: + gazebo::physics::LinkPtr body; + ignition::math::Vector3d force; + ignition::math::Vector3d torque; + ros::Time start_time; + ros::Duration duration; + }; + + class ForceJointJob + { + public: + gazebo::physics::JointPtr joint; + double force; // should this be a array? + ros::Time start_time; + ros::Duration duration; + }; + + std::vector wrench_body_jobs_; + std::vector force_joint_jobs_; + + /// \brief index counters to count the accesses on models via GetModelState + std::map access_count_get_model_state_; +}; +} +#endif diff --git a/gazebo_ros_pkgs/gazebo_ros/launch/elevator_world.launch b/gazebo_ros_pkgs/gazebo_ros/launch/elevator_world.launch new file mode 100755 index 0000000..0985392 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/launch/elevator_world.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/launch/empty_world (另一个复件).launch b/gazebo_ros_pkgs/gazebo_ros/launch/empty_world (另一个复件).launch new file mode 100755 index 0000000..7fba552 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/launch/empty_world (另一个复件).launch @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/launch/empty_world (复件).launch b/gazebo_ros_pkgs/gazebo_ros/launch/empty_world (复件).launch new file mode 100755 index 0000000..2abae93 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/launch/empty_world (复件).launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/launch/empty_world.launch b/gazebo_ros_pkgs/gazebo_ros/launch/empty_world.launch new file mode 100755 index 0000000..887c71b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/launch/empty_world.launch @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/launch/empty_world_o.launch b/gazebo_ros_pkgs/gazebo_ros/launch/empty_world_o.launch new file mode 100755 index 0000000..e478d26 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/launch/empty_world_o.launch @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/launch/empty_world_try.launch b/gazebo_ros_pkgs/gazebo_ros/launch/empty_world_try.launch new file mode 100755 index 0000000..13d118d --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/launch/empty_world_try.launch @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/launch/mud_world.launch b/gazebo_ros_pkgs/gazebo_ros/launch/mud_world.launch new file mode 100755 index 0000000..a1bdd9a --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/launch/mud_world.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/launch/range_world.launch b/gazebo_ros_pkgs/gazebo_ros/launch/range_world.launch new file mode 100755 index 0000000..1bff7b3 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/launch/range_world.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/launch/rubble_world.launch b/gazebo_ros_pkgs/gazebo_ros/launch/rubble_world.launch new file mode 100755 index 0000000..a09e264 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/launch/rubble_world.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/launch/shapes_world.launch b/gazebo_ros_pkgs/gazebo_ros/launch/shapes_world.launch new file mode 100755 index 0000000..56eeab4 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/launch/shapes_world.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/launch/willowgarage_world.launch b/gazebo_ros_pkgs/gazebo_ros/launch/willowgarage_world.launch new file mode 100755 index 0000000..ceb0b3c --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/launch/willowgarage_world.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros/package.xml b/gazebo_ros_pkgs/gazebo_ros/package.xml new file mode 100755 index 0000000..d4b9f66 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/package.xml @@ -0,0 +1,42 @@ + + + gazebo_ros + 2.5.15 + + Provides ROS plugins that offer message and service publishers for interfacing with Gazebo through ROS. + Formally simulator_gazebo/gazebo + + + Jose Luis Rivero + + Apache 2.0 + + http://gazebosim.org/tutorials?cat=connect_ros + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs + + John Hsu + Nate Koenig + Dave Coleman + + catkin + + cmake_modules + gazebo_dev + + gazebo_dev + gazebo_msgs + roslib + roscpp + tf + std_srvs + rosgraph_msgs + dynamic_reconfigure + std_msgs + geometry_msgs + tinyxml + + diff --git a/gazebo_ros_pkgs/gazebo_ros/scripts/debug b/gazebo_ros_pkgs/gazebo_ros/scripts/debug new file mode 100755 index 0000000..37cd902 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/scripts/debug @@ -0,0 +1,22 @@ +#!/bin/sh +final="$@" + +EXT=so +if [ $(uname) == "Darwin" ]; then + EXT=dylib +fi + +# add ros path plugin if it does not already exist in the passed in arguments +if [ `expr "$final" : ".*libgazebo_ros_paths_plugin\.$EXT.*"` -eq 0 ] +then + final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" +fi + +# add ros api plugin if it does not already exist in the passed in arguments +if [ `expr "$final" : ".*libgazebo_ros_api_plugin\.$EXT.*"` -eq 0 ] +then + final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" +fi + +setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ +. $setup_path/setup.sh && rosrun gazebo_ros gdbrun gzserver $final diff --git a/gazebo_ros_pkgs/gazebo_ros/scripts/gazebo b/gazebo_ros_pkgs/gazebo_ros/scripts/gazebo new file mode 100755 index 0000000..0e96229 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/scripts/gazebo @@ -0,0 +1,50 @@ +#!/bin/sh +[ -L ${0} ] && SCRIPT_DIR=$(readlink ${0}) || SCRIPT_DIR=${0} +SCRIPT_DIR=$(dirname ${SCRIPT_DIR}) + +. ${SCRIPT_DIR}/libcommon.sh + +final="$@" + +EXT=so +SIGNAL=SIGINT +if [ $(uname) = "Darwin" ]; then + EXT=dylib + SIGNAL=INT +fi + +# add ros path plugin if it does not already exist in the passed in arguments +if [ `expr "$final" : ".*libgazebo_ros_paths_plugin\.$EXT.*"` -eq 0 ] +then + final="$final -s `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" +fi + +# add ros api plugin if it does not already exist in the passed in arguments +if [ `expr "$final" : ".*libgazebo_ros_api_plugin\.$EXT.*"` -eq 0 ] +then + final="$final -s `catkin_find --first-only libgazebo_ros_api_plugin.$EXT`" +fi + +client_final="-g `catkin_find --first-only libgazebo_ros_paths_plugin.$EXT`" + +setup_path=$(pkg-config --variable=prefix gazebo)/share/gazebo/ + +# source setup.sh, but keep local modifications to GAZEBO_MASTER_URI and GAZEBO_MODEL_DATABASE_URI +desired_master_uri="$GAZEBO_MASTER_URI" +desired_model_database_uri="$GAZEBO_MODEL_DATABASE_URI" +. $setup_path/setup.sh +if [ "$desired_master_uri" = "" ]; then + desired_master_uri="$GAZEBO_MASTER_URI" +fi +if [ "$desired_model_database_uri" = "" ]; then + desired_model_database_uri="$GAZEBO_MODEL_DATABASE_URI" +fi + +final=$(relocate_remappings "${final}") + +# Combine the commands +GAZEBO_MASTER_URI="$desired_master_uri" GAZEBO_MODEL_DATABASE_URI="$desired_model_database_uri" gzserver $final & +GAZEBO_MASTER_URI="$desired_master_uri" GAZEBO_MODEL_DATABASE_URI="$desired_model_database_uri" gzclient $client_final + +# Kill the server +kill -s $SIGNAL $! diff --git a/gazebo_ros_pkgs/gazebo_ros/scripts/gdbrun b/gazebo_ros_pkgs/gazebo_ros/scripts/gdbrun new file mode 100755 index 0000000..9a50384 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/scripts/gdbrun @@ -0,0 +1,25 @@ +#!/bin/bash +extra_text="" +if [ "$1" = "--break-main" ]; then + extra_text="break main" + shift +fi + +EXEC="$1" + +shift + +run_text="run" +for a in "$@"; do + run_text="${run_text} \"$a\"" +done + +TMPFILE=/tmp/gdbrun.$$.$#.tmp +cat > ${TMPFILE} <||] - source of the model xml or the trimesh file + -model - name of the model to be spawned. + -reference_frame - optinal: name of the model/body where initial pose is defined. + If left empty or specified as "world", gazebo world frame is used. + -gazebo_namespace - optional: ROS namespace of gazebo offered ROS interfaces. Defaults to /gazebo/ (e.g. /gazebo/spawn_model). + -robot_namespace - optional: change ROS namespace of gazebo-plugins. + -unpause - optional: !!!Experimental!!! unpause physics after spawning model + -wait - optional: !!!Experimental!!! wait for model to exist + -trimesh_mass - required if -trimesh is used: linear mass + -trimesh_ixx - required if -trimesh is used: moment of inertia about x-axis + -trimesh_iyy - required if -trimesh is used: moment of inertia about y-axis + -trimesh_izz - required if -trimesh is used: moment of inertia about z-axis + -trimesh_gravity - required if -trimesh is used: gravity turned on for this trimesh model + -trimesh_material - required if -trimesh is used: E.g. Gazebo/Blue + -trimesh_name - required if -trimesh is used: name of the link containing the trimesh + -x - optional: initial pose, use 0 if left out + -y - optional: initial pose, use 0 if left out + -z - optional: initial pose, use 0 if left out + -R - optional: initial pose, use 0 if left out + -P - optional: initial pose, use 0 if left out + -Y - optional: initial pose, use 0 if left out + -J - optional: initialize the specified joint at the specified value + -package_to_model - optional: convert urdf i+2: + self.joint_names.append(sys.argv[i+1]) + self.joint_positions.append(float(sys.argv[i+2])) + else: + rospy.logerr("Error: must specify a joint name and joint value pair") + sys.exit(0) + if sys.argv[i] == '-param': + if len(sys.argv) > i+1: + if self.file_name != "" or self.database_name != "": + rospy.logerr("Error: you cannot specify file name if parameter or database name is given, must pick one source of model xml") + sys.exit(0) + else: + self.param_name = sys.argv[i+1] + if sys.argv[i] == '-file': + if len(sys.argv) > i+1: + if self.param_name != "" or self.database_name != "": + rospy.logerr("Error: you cannot specify parameter if file or database name is given, must pick one source of model xml") + sys.exit(0) + else: + self.file_name = sys.argv[i+1] + if sys.argv[i] == '-database': + if len(sys.argv) > i+1: + if self.param_name != "" or self.file_name != "": + rospy.logerr("Error: you cannot specify parameter if file or parameter name is given, must pick one source of model xml") + sys.exit(0) + else: + self.database_name = sys.argv[i+1] + if sys.argv[i] == '-model': + if len(sys.argv) > i+1: + self.model_name = sys.argv[i+1] + if sys.argv[i] == '-wait': + if len(sys.argv) > i+1: + self.wait_for_model = sys.argv[i+1] + if sys.argv[i] == '-reference_frame': + if len(sys.argv) > i+1: + self.reference_frame = sys.argv[i+1] + if sys.argv[i] == '-robot_namespace': + if len(sys.argv) > i+1: + self.robot_namespace = sys.argv[i+1] + if sys.argv[i] == '-namespace': + if len(sys.argv) > i+1: + self.robot_namespace = sys.argv[i+1] + if sys.argv[i] == '-gazebo_namespace': + if len(sys.argv) > i+1: + self.gazebo_namespace = sys.argv[i+1] + if sys.argv[i] == '-x': + if len(sys.argv) > i+1: + self.initial_xyz[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-y': + if len(sys.argv) > i+1: + self.initial_xyz[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-z': + if len(sys.argv) > i+1: + self.initial_xyz[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-R': + if len(sys.argv) > i+1: + self.initial_rpy[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-P': + if len(sys.argv) > i+1: + self.initial_rpy[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-Y': + if len(sys.argv) > i+1: + self.initial_rpy[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-package_to_model': + self.package_to_model = True; + if sys.argv[i] == '-b': + self.bond = True + + if not self.sdf_format and not self.urdf_format: + rospy.logerr("Error: you must specify incoming format as either urdf or sdf format xml") + sys.exit(0) + if self.model_name == "": + rospy.logerr("Error: you must specify model name") + sys.exit(0) + + def checkForModel(self,model): + for n in model.name: + if n == self.wait_for_model: + self.wait_for_model_exists = True + + + # Generate a blank SDF file with an include for the model from the model database + def createDatabaseCode(self, database_name): + return model_database_template.replace("MODEL_NAME", database_name); + + def callSpawnService(self): + + # wait for model to exist + rospy.init_node('spawn_model') + + if not self.wait_for_model == "": + rospy.Subscriber("%s/model_states"%(self.gazebo_namespace), ModelStates, self.checkForModel) + r= rospy.Rate(10) + while not rospy.is_shutdown() and not self.wait_for_model_exists: + r.sleep() + + if rospy.is_shutdown(): + sys.exit(0) + + if self.file_name != "": + rospy.loginfo("Loading model XML from file") + if os.path.exists(self.file_name): + if os.path.isdir(self.file_name): + rospy.logerr("Error: file name is a path? %s", self.file_name) + sys.exit(0) + if not os.path.isfile(self.file_name): + rospy.logerr("Error: unable to open file %s", self.file_name) + sys.exit(0) + else: + rospy.logerr("Error: file does not exist %s", self.file_name) + sys.exit(0) + # load file + f = open(self.file_name,'r') + model_xml = f.read() + if model_xml == "": + rospy.logerr("Error: file is empty %s", self.file_name) + sys.exit(0) + + # ROS Parameter + elif self.param_name != "": + rospy.loginfo( "Loading model XML from ros parameter") + model_xml = rospy.get_param(self.param_name) + if model_xml == "": + rospy.logerr("Error: param does not exist or is empty") + sys.exit(0) + + # Gazebo Model Database + elif self.database_name != "": + rospy.loginfo( "Loading model XML from Gazebo Model Database") + model_xml = self.createDatabaseCode(self.database_name) + if model_xml == "": + rospy.logerr("Error: an error occured generating the SDF file") + sys.exit(0) + else: + rospy.logerr("Error: user specified param or filename is an empty string") + sys.exit(0) + + if self.package_to_model: + model_xml = re.sub("<\s*mesh\s+filename\s*=\s*([\"|'])package://","model://", model_xml) + + # setting initial pose + initial_pose = Pose() + initial_pose.position.x = self.initial_xyz[0] + initial_pose.position.y = self.initial_xyz[1] + initial_pose.position.z = self.initial_xyz[2] + # convert rpy to quaternion for Pose message + tmpq = tft.quaternion_from_euler(self.initial_rpy[0],self.initial_rpy[1],self.initial_rpy[2]) + q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3]) + initial_pose.orientation = q; + + # spawn model + if self.urdf_format: + success = gazebo_interface.spawn_urdf_model_client(self.model_name, model_xml, self.robot_namespace, + initial_pose, self.reference_frame, self.gazebo_namespace) + elif self.sdf_format: + success = gazebo_interface.spawn_sdf_model_client(self.model_name, model_xml, self.robot_namespace, + initial_pose, self.reference_frame, self.gazebo_namespace) + else: + rospy.logerr("Error: should not be here in spawner helper script, there is a bug") + sys.exit(0) + + # set model configuration before unpause if user requested + if len(self.joint_names) != 0: + try: + success = gazebo_interface.set_model_configuration_client(self.model_name, self.param_name, + self.joint_names, self.joint_positions, self.gazebo_namespace) + except rospy.ServiceException as e: + rospy.logerr("Set model configuration service call failed: %s", e) + + # unpause physics if user requested + if self.unpause_physics: + rospy.wait_for_service('%s/unpause_physics'%(self.gazebo_namespace)) + try: + unpause_physics = rospy.ServiceProxy('%s/unpause_physics'%(self.gazebo_namespace), Empty) + unpause_physics() + except rospy.ServiceException as e: + rospy.logerr("Unpause physics service call failed: %s", e) + + return + + + def callDeleteService(self): + try: + delete_model = rospy.ServiceProxy('%s/delete_model'%(self.gazebo_namespace), DeleteModel) + delete_model(model_name=self.model_name) + except rospy.ServiceException as e: + rospy.logerr("Delete model service call failed: %s", e) + +if __name__ == "__main__": + if len(sys.argv) < 2: + print(usage()) + else: + print("SpawnModel script started") # make this a print incase roscore has not been started + sm = SpawnModel() + sm.parseUserInputs() + sm.callSpawnService() + + if sm.bond: + rospy.on_shutdown(sm.callDeleteService) + rospy.spin() diff --git a/gazebo_ros_pkgs/gazebo_ros/scripts/spawn_model1 b/gazebo_ros_pkgs/gazebo_ros/scripts/spawn_model1 new file mode 100755 index 0000000..f3f4c9b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/scripts/spawn_model1 @@ -0,0 +1,317 @@ +#!/usr/bin/env python +# +# 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: helper script for spawning models in gazebo +# Author: John Hsu, Dave Coleman +# + +import rospy, sys, os, time +import string +import warnings +import re + +from gazebo_ros import gazebo_interface + +from gazebo_msgs.msg import * +from gazebo_msgs.srv import * +from std_srvs.srv import Empty +from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench +import tf.transformations as tft + +model_database_template = """ + + + model://MODEL_NAME + + +""" + +def usage(): + print('''Commands: + -[urdf|sdf|trimesh|gazebo] - specify incoming xml is urdf, sdf or trimesh format. gazebo arg is deprecated in ROS Hydro + -[file|param|database] [||] - source of the model xml or the trimesh file + -model - name of the model to be spawned. + -reference_frame - optinal: name of the model/body where initial pose is defined. + If left empty or specified as "world", gazebo world frame is used. + -gazebo_namespace - optional: ROS namespace of gazebo offered ROS interfaces. Defaults to /gazebo/ (e.g. /gazebo/spawn_model). + -robot_namespace - optional: change ROS namespace of gazebo-plugins. + -unpause - optional: !!!Experimental!!! unpause physics after spawning model + -wait - optional: !!!Experimental!!! wait for model to exist + -trimesh_mass - required if -trimesh is used: linear mass + -trimesh_ixx - required if -trimesh is used: moment of inertia about x-axis + -trimesh_iyy - required if -trimesh is used: moment of inertia about y-axis + -trimesh_izz - required if -trimesh is used: moment of inertia about z-axis + -trimesh_gravity - required if -trimesh is used: gravity turned on for this trimesh model + -trimesh_material - required if -trimesh is used: E.g. Gazebo/Blue + -trimesh_name - required if -trimesh is used: name of the link containing the trimesh + -x - optional: initial pose, use 0 if left out + -y - optional: initial pose, use 0 if left out + -z - optional: initial pose, use 0 if left out + -R - optional: initial pose, use 0 if left out + -P - optional: initial pose, use 0 if left out + -Y - optional: initial pose, use 0 if left out + -J - optional: initialize the specified joint at the specified value + -package_to_model - optional: convert urdf i+2: + self.joint_names.append(sys.argv[i+1]) + self.joint_positions.append(float(sys.argv[i+2])) + else: + rospy.logerr("Error: must specify a joint name and joint value pair") + sys.exit(0) + if sys.argv[i] == '-param': + if len(sys.argv) > i+1: + if self.file_name != "" or self.database_name != "": + rospy.logerr("Error: you cannot specify file name if parameter or database name is given, must pick one source of model xml") + sys.exit(0) + else: + self.param_name = sys.argv[i+1] + if sys.argv[i] == '-file': + if len(sys.argv) > i+1: + if self.param_name != "" or self.database_name != "": + rospy.logerr("Error: you cannot specify parameter if file or database name is given, must pick one source of model xml") + sys.exit(0) + else: + self.file_name = sys.argv[i+1] + if sys.argv[i] == '-database': + if len(sys.argv) > i+1: + if self.param_name != "" or self.file_name != "": + rospy.logerr("Error: you cannot specify parameter if file or parameter name is given, must pick one source of model xml") + sys.exit(0) + else: + self.database_name = sys.argv[i+1] + if sys.argv[i] == '-model': + if len(sys.argv) > i+1: + self.model_name = sys.argv[i+1] + if sys.argv[i] == '-wait': + if len(sys.argv) > i+1: + self.wait_for_model = sys.argv[i+1] + if sys.argv[i] == '-reference_frame': + if len(sys.argv) > i+1: + self.reference_frame = sys.argv[i+1] + if sys.argv[i] == '-robot_namespace': + if len(sys.argv) > i+1: + self.robot_namespace = sys.argv[i+1] + if sys.argv[i] == '-namespace': + if len(sys.argv) > i+1: + self.robot_namespace = sys.argv[i+1] + if sys.argv[i] == '-gazebo_namespace': + if len(sys.argv) > i+1: + self.gazebo_namespace = sys.argv[i+1] + if sys.argv[i] == '-x': + if len(sys.argv) > i+1: + self.initial_xyz[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-y': + if len(sys.argv) > i+1: + self.initial_xyz[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-z': + if len(sys.argv) > i+1: + self.initial_xyz[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-R': + if len(sys.argv) > i+1: + self.initial_rpy[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-P': + if len(sys.argv) > i+1: + self.initial_rpy[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-Y': + if len(sys.argv) > i+1: + self.initial_rpy[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-package_to_model': + self.package_to_model = True; + if sys.argv[i] == '-b': + self.bond = True + + if not self.sdf_format and not self.urdf_format: + rospy.logerr("Error: you must specify incoming format as either urdf or sdf format xml") + sys.exit(0) + if self.model_name == "": + rospy.logerr("Error: you must specify model name") + sys.exit(0) + + def checkForModel(self,model): + for n in model.name: + if n == self.wait_for_model: + self.wait_for_model_exists = True + + + # Generate a blank SDF file with an include for the model from the model database + def createDatabaseCode(self, database_name): + return model_database_template.replace("MODEL_NAME", database_name); + + def callSpawnService(self): + + # wait for model to exist + rospy.init_node('spawn_model') + + if not self.wait_for_model == "": + rospy.Subscriber("%s/model_states"%(self.gazebo_namespace), ModelStates, self.checkForModel) + r= rospy.Rate(10) + while not rospy.is_shutdown() and not self.wait_for_model_exists: + r.sleep() + + if rospy.is_shutdown(): + sys.exit(0) + + if self.file_name != "": + rospy.loginfo("Loading model XML from file") + if os.path.exists(self.file_name): + if os.path.isdir(self.file_name): + rospy.logerr("Error: file name is a path? %s", self.file_name) + sys.exit(0) + if not os.path.isfile(self.file_name): + rospy.logerr("Error: unable to open file %s", self.file_name) + sys.exit(0) + else: + rospy.logerr("Error: file does not exist %s", self.file_name) + sys.exit(0) + # load file + f = open(self.file_name,'r') + model_xml = f.read() + if model_xml == "": + rospy.logerr("Error: file is empty %s", self.file_name) + sys.exit(0) + + # ROS Parameter + elif self.param_name != "": + rospy.loginfo( "Loading model XML from ros parameter") + model_xml = rospy.get_param(self.param_name) + if model_xml == "": + rospy.logerr("Error: param does not exist or is empty") + sys.exit(0) + + # Gazebo Model Database + elif self.database_name != "": + rospy.loginfo( "Loading model XML from Gazebo Model Database") + model_xml = self.createDatabaseCode(self.database_name) + if model_xml == "": + rospy.logerr("Error: an error occured generating the SDF file") + sys.exit(0) + else: + rospy.logerr("Error: user specified param or filename is an empty string") + sys.exit(0) + + if self.package_to_model: + model_xml = re.sub("<\s*mesh\s+filename\s*=\s*([\"|'])package://","model://", model_xml) + + # setting initial pose + initial_pose = Pose() + initial_pose.position.x = self.initial_xyz[0] + initial_pose.position.y = self.initial_xyz[1] + initial_pose.position.z = self.initial_xyz[2] + # convert rpy to quaternion for Pose message + tmpq = tft.quaternion_from_euler(self.initial_rpy[0],self.initial_rpy[1],self.initial_rpy[2]) + q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3]) + initial_pose.orientation = q; + + # spawn model + if self.urdf_format: + success = gazebo_interface.spawn_urdf_model_client(self.model_name, model_xml, self.robot_namespace, + initial_pose, self.reference_frame, self.gazebo_namespace) + elif self.sdf_format: + success = gazebo_interface.spawn_sdf_model_client(self.model_name, model_xml, self.robot_namespace, + initial_pose, self.reference_frame, self.gazebo_namespace) + else: + rospy.logerr("Error: should not be here in spawner helper script, there is a bug") + sys.exit(0) + + # set model configuration before unpause if user requested + if len(self.joint_names) != 0: + try: + success = gazebo_interface.set_model_configuration_client(self.model_name, self.param_name, + self.joint_names, self.joint_positions, self.gazebo_namespace) + except rospy.ServiceException as e: + rospy.logerr("Set model configuration service call failed: %s", e) + + # unpause physics if user requested + if self.unpause_physics: + rospy.wait_for_service('%s/unpause_physics'%(self.gazebo_namespace)) + try: + unpause_physics = rospy.ServiceProxy('%s/unpause_physics'%(self.gazebo_namespace), Empty) + unpause_physics() + except rospy.ServiceException as e: + rospy.logerr("Unpause physics service call failed: %s", e) + + return + + + def callDeleteService(self): + try: + delete_model = rospy.ServiceProxy('%s/delete_model'%(self.gazebo_namespace), DeleteModel) + delete_model(model_name=self.model_name) + except rospy.ServiceException as e: + rospy.logerr("Delete model service call failed: %s", e) + +if __name__ == "__main__": + if len(sys.argv) < 2: + print(usage()) + else: + print("SpawnModel script started") # make this a print incase roscore has not been started + sm = SpawnModel() + sm.parseUserInputs() + sm.callSpawnService() + + if sm.bond: + rospy.on_shutdown(sm.callDeleteService) + rospy.spin() diff --git a/gazebo_ros_pkgs/gazebo_ros/scripts/spawn_model2 b/gazebo_ros_pkgs/gazebo_ros/scripts/spawn_model2 new file mode 100755 index 0000000..d64f553 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/scripts/spawn_model2 @@ -0,0 +1,317 @@ +#!/usr/bin/env python +# +# 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: helper script for spawning models in gazebo +# Author: John Hsu, Dave Coleman +# + +import rospy, sys, os, time +import string +import warnings +import re + +from gazebo_ros import gazebo_interface + +from gazebo_msgs.msg import * +from gazebo_msgs.srv import * +from std_srvs.srv import Empty +from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench +import tf.transformations as tft + +model_database_template = """ + + + model://MODEL_NAME + + +""" + +def usage(): + print('''Commands: + -[urdf|sdf|trimesh|gazebo] - specify incoming xml is urdf, sdf or trimesh format. gazebo arg is deprecated in ROS Hydro + -[file|param|database] [||] - source of the model xml or the trimesh file + -model - name of the model to be spawned. + -reference_frame - optinal: name of the model/body where initial pose is defined. + If left empty or specified as "world", gazebo world frame is used. + -gazebo_namespace - optional: ROS namespace of gazebo offered ROS interfaces. Defaults to /gazebo/ (e.g. /gazebo/spawn_model). + -robot_namespace - optional: change ROS namespace of gazebo-plugins. + -unpause - optional: !!!Experimental!!! unpause physics after spawning model + -wait - optional: !!!Experimental!!! wait for model to exist + -trimesh_mass - required if -trimesh is used: linear mass + -trimesh_ixx - required if -trimesh is used: moment of inertia about x-axis + -trimesh_iyy - required if -trimesh is used: moment of inertia about y-axis + -trimesh_izz - required if -trimesh is used: moment of inertia about z-axis + -trimesh_gravity - required if -trimesh is used: gravity turned on for this trimesh model + -trimesh_material - required if -trimesh is used: E.g. Gazebo/Blue + -trimesh_name - required if -trimesh is used: name of the link containing the trimesh + -x - optional: initial pose, use 0 if left out + -y - optional: initial pose, use 0 if left out + -z - optional: initial pose, use 0 if left out + -R - optional: initial pose, use 0 if left out + -P - optional: initial pose, use 0 if left out + -Y - optional: initial pose, use 0 if left out + -J - optional: initialize the specified joint at the specified value + -package_to_model - optional: convert urdf i+2: + self.joint_names.append(sys.argv[i+1]) + self.joint_positions.append(float(sys.argv[i+2])) + else: + rospy.logerr("Error: must specify a joint name and joint value pair") + sys.exit(0) + if sys.argv[i] == '-param': + if len(sys.argv) > i+1: + if self.file_name != "" or self.database_name != "": + rospy.logerr("Error: you cannot specify file name if parameter or database name is given, must pick one source of model xml") + sys.exit(0) + else: + self.param_name = sys.argv[i+1] + if sys.argv[i] == '-file': + if len(sys.argv) > i+1: + if self.param_name != "" or self.database_name != "": + rospy.logerr("Error: you cannot specify parameter if file or database name is given, must pick one source of model xml") + sys.exit(0) + else: + self.file_name = sys.argv[i+1] + if sys.argv[i] == '-database': + if len(sys.argv) > i+1: + if self.param_name != "" or self.file_name != "": + rospy.logerr("Error: you cannot specify parameter if file or parameter name is given, must pick one source of model xml") + sys.exit(0) + else: + self.database_name = sys.argv[i+1] + if sys.argv[i] == '-model': + if len(sys.argv) > i+1: + self.model_name = sys.argv[i+1] + if sys.argv[i] == '-wait': + if len(sys.argv) > i+1: + self.wait_for_model = sys.argv[i+1] + if sys.argv[i] == '-reference_frame': + if len(sys.argv) > i+1: + self.reference_frame = sys.argv[i+1] + if sys.argv[i] == '-robot_namespace': + if len(sys.argv) > i+1: + self.robot_namespace = sys.argv[i+1] + if sys.argv[i] == '-namespace': + if len(sys.argv) > i+1: + self.robot_namespace = sys.argv[i+1] + if sys.argv[i] == '-gazebo_namespace': + if len(sys.argv) > i+1: + self.gazebo_namespace = sys.argv[i+1] + if sys.argv[i] == '-x': + if len(sys.argv) > i+1: + self.initial_xyz[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-y': + if len(sys.argv) > i+1: + self.initial_xyz[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-z': + if len(sys.argv) > i+1: + self.initial_xyz[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-R': + if len(sys.argv) > i+1: + self.initial_rpy[0] = float(sys.argv[i+1]) + if sys.argv[i] == '-P': + if len(sys.argv) > i+1: + self.initial_rpy[1] = float(sys.argv[i+1]) + if sys.argv[i] == '-Y': + if len(sys.argv) > i+1: + self.initial_rpy[2] = float(sys.argv[i+1]) + if sys.argv[i] == '-package_to_model': + self.package_to_model = True; + if sys.argv[i] == '-b': + self.bond = True + + if not self.sdf_format and not self.urdf_format: + rospy.logerr("Error: you must specify incoming format as either urdf or sdf format xml") + sys.exit(0) + if self.model_name == "": + rospy.logerr("Error: you must specify model name") + sys.exit(0) + + def checkForModel(self,model): + for n in model.name: + if n == self.wait_for_model: + self.wait_for_model_exists = True + + + # Generate a blank SDF file with an include for the model from the model database + def createDatabaseCode(self, database_name): + return model_database_template.replace("MODEL_NAME", database_name); + + def callSpawnService(self): + + # wait for model to exist + rospy.init_node('spawn_model') + + if not self.wait_for_model == "": + rospy.Subscriber("%s/model_states"%(self.gazebo_namespace), ModelStates, self.checkForModel) + r= rospy.Rate(10) + while not rospy.is_shutdown() and not self.wait_for_model_exists: + r.sleep() + + if rospy.is_shutdown(): + sys.exit(0) + + if self.file_name != "": + rospy.loginfo("Loading model XML from file") + if os.path.exists(self.file_name): + if os.path.isdir(self.file_name): + rospy.logerr("Error: file name is a path? %s", self.file_name) + sys.exit(0) + if not os.path.isfile(self.file_name): + rospy.logerr("Error: unable to open file %s", self.file_name) + sys.exit(0) + else: + rospy.logerr("Error: file does not exist %s", self.file_name) + sys.exit(0) + # load file + f = open(self.file_name,'r') + model_xml = f.read() + if model_xml == "": + rospy.logerr("Error: file is empty %s", self.file_name) + sys.exit(0) + + # ROS Parameter + elif self.param_name != "": + rospy.loginfo( "Loading model XML from ros parameter") + model_xml = rospy.get_param(self.param_name) + if model_xml == "": + rospy.logerr("Error: param does not exist or is empty") + sys.exit(0) + + # Gazebo Model Database + elif self.database_name != "": + rospy.loginfo( "Loading model XML from Gazebo Model Database") + model_xml = self.createDatabaseCode(self.database_name) + if model_xml == "": + rospy.logerr("Error: an error occured generating the SDF file") + sys.exit(0) + else: + rospy.logerr("Error: user specified param or filename is an empty string") + sys.exit(0) + + if self.package_to_model: + model_xml = re.sub("<\s*mesh\s+filename\s*=\s*([\"|'])package://","model://", model_xml) + + # setting initial pose + initial_pose = Pose() + initial_pose.position.x = self.initial_xyz[0] + initial_pose.position.y = self.initial_xyz[1] + initial_pose.position.z = self.initial_xyz[2] + # convert rpy to quaternion for Pose message + tmpq = tft.quaternion_from_euler(self.initial_rpy[0],self.initial_rpy[1],self.initial_rpy[2]) + q = Quaternion(tmpq[0],tmpq[1],tmpq[2],tmpq[3]) + initial_pose.orientation = q; + + # spawn model + if self.urdf_format: + success = gazebo_interface.spawn_urdf_model_client(self.model_name, model_xml, self.robot_namespace, + initial_pose, self.reference_frame, self.gazebo_namespace) + elif self.sdf_format: + success = gazebo_interface.spawn_sdf_model_client(self.model_name, model_xml, self.robot_namespace, + initial_pose, self.reference_frame, self.gazebo_namespace) + else: + rospy.logerr("Error: should not be here in spawner helper script, there is a bug") + sys.exit(0) + + # set model configuration before unpause if user requested + if len(self.joint_names) != 0: + try: + success = gazebo_interface.set_model_configuration_client(self.model_name, self.param_name, + self.joint_names, self.joint_positions, self.gazebo_namespace) + except rospy.ServiceException as e: + rospy.logerr("Set model configuration service call failed: %s", e) + + # unpause physics if user requested + if self.unpause_physics: + rospy.wait_for_service('%s/unpause_physics'%(self.gazebo_namespace)) + try: + unpause_physics = rospy.ServiceProxy('%s/unpause_physics'%(self.gazebo_namespace), Empty) + unpause_physics() + except rospy.ServiceException as e: + rospy.logerr("Unpause physics service call failed: %s", e) + + return + + + def callDeleteService(self): + try: + delete_model = rospy.ServiceProxy('%s/delete_model'%(self.gazebo_namespace), DeleteModel) + delete_model(model_name=self.model_name) + except rospy.ServiceException as e: + rospy.logerr("Delete model service call failed: %s", e) + +if __name__ == "__main__": + if len(sys.argv) < 2: + print(usage()) + else: + print("SpawnModel script started") # make this a print incase roscore has not been started + sm = SpawnModel() + sm.parseUserInputs() + sm.callSpawnService() + + if sm.bond: + rospy.on_shutdown(sm.callDeleteService) + rospy.spin() diff --git a/gazebo_ros_pkgs/gazebo_ros/setup.py b/gazebo_ros_pkgs/gazebo_ros/setup.py new file mode 100755 index 0000000..b437c43 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/setup.py @@ -0,0 +1,9 @@ +#!/usr/bin/env python +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +d = generate_distutils_setup() +d['packages'] = ['gazebo_ros'] +d['package_dir'] = {'':'src'} + +setup(**d) diff --git a/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/__init__.py b/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.py b/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.py new file mode 100755 index 0000000..e466b99 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.py @@ -0,0 +1,52 @@ +#! /usr/bin/env python +# Wrappers around the services provided by rosified gazebo + +import sys +import rospy +import os +import time + +from gazebo_msgs.msg import * +from gazebo_msgs.srv import * +from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Wrench + + +def spawn_sdf_model_client(model_name, model_xml, robot_namespace, initial_pose, reference_frame, gazebo_namespace): + rospy.loginfo("Waiting for service %s/spawn_sdf_model"%gazebo_namespace) + rospy.wait_for_service(gazebo_namespace+'/spawn_sdf_model') + try: + spawn_sdf_model = rospy.ServiceProxy(gazebo_namespace+'/spawn_sdf_model', SpawnModel) + rospy.loginfo("Calling service %s/spawn_sdf_model"%gazebo_namespace) + resp = spawn_sdf_model(model_name, model_xml, robot_namespace, initial_pose, reference_frame) + rospy.loginfo("Spawn status: %s"%resp.status_message) + return resp.success + except rospy.ServiceException as e: + print("Service call failed: %s" % e) + +def spawn_urdf_model_client(model_name, model_xml, robot_namespace, initial_pose, reference_frame, gazebo_namespace): + rospy.loginfo("Waiting for service %s/spawn_urdf_model"%gazebo_namespace) + rospy.wait_for_service(gazebo_namespace+'/spawn_urdf_model') + try: + spawn_urdf_model = rospy.ServiceProxy(gazebo_namespace+'/spawn_urdf_model', SpawnModel) + rospy.loginfo("Calling service %s/spawn_urdf_model"%gazebo_namespace) + resp = spawn_urdf_model(model_name, model_xml, robot_namespace, initial_pose, reference_frame) + rospy.loginfo("Spawn status: %s"%resp.status_message) + return resp.success + except rospy.ServiceException as e: + print("Service call failed: %s" % e) + +def set_model_configuration_client(model_name, model_param_name, joint_names, joint_positions, gazebo_namespace): + rospy.loginfo("Waiting for service %s/set_model_configuration"%gazebo_namespace) + rospy.wait_for_service(gazebo_namespace+'/set_model_configuration') + rospy.loginfo("temporary hack to **fix** the -J joint position option (issue #93), sleeping for 1 second to avoid race condition."); + time.sleep(1) + try: + set_model_configuration = rospy.ServiceProxy(gazebo_namespace+'/set_model_configuration', SetModelConfiguration) + rospy.loginfo("Calling service %s/set_model_configuration"%gazebo_namespace) + resp = set_model_configuration(model_name, model_param_name, joint_names, joint_positions) + rospy.loginfo("Set model configuration status: %s"%resp.status_message) + + return resp.success + except rospy.ServiceException as e: + print("Service call failed: %s" % e) + diff --git a/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.pyc b/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.pyc new file mode 100755 index 0000000..3e5b09a Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros/gazebo_interface.pyc differ diff --git a/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_api_plugin.cpp b/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_api_plugin.cpp new file mode 100755 index 0000000..546c8e4 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_api_plugin.cpp @@ -0,0 +1,2695 @@ +/* + * 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: External interfaces for Gazebo + * Author: Nate Koenig, John Hsu, Dave Coleman + * Date: Jun 10 2013 + */ + +#include +#include +#include + +namespace gazebo +{ + +GazeboRosApiPlugin::GazeboRosApiPlugin() : physics_reconfigure_initialized_(false), + world_created_(false), + stop_(false), + plugin_loaded_(false), + pub_link_states_connection_count_(0), + pub_model_states_connection_count_(0), + pub_clock_frequency_(0) +{ + robot_namespace_.clear(); +} + +GazeboRosApiPlugin::~GazeboRosApiPlugin() +{ + ROS_DEBUG_STREAM_NAMED("api_plugin", "GazeboRosApiPlugin Deconstructor start"); + + // Unload the sigint event + sigint_event_.reset(); + ROS_DEBUG_STREAM_NAMED("api_plugin", "After sigint_event unload"); + + // Don't attempt to unload this plugin if it was never loaded in the Load() function + if (!plugin_loaded_) + { + ROS_DEBUG_STREAM_NAMED("api_plugin", "Deconstructor skipped because never loaded"); + return; + } + + // Disconnect slots + load_gazebo_ros_api_plugin_event_.reset(); + wrench_update_event_.reset(); + force_update_event_.reset(); + time_update_event_.reset(); + ROS_DEBUG_STREAM_NAMED("api_plugin", "Slots disconnected"); + + if (pub_link_states_connection_count_ > 0) // disconnect if there are subscribers on exit + pub_link_states_event_.reset(); + if (pub_model_states_connection_count_ > 0) // disconnect if there are subscribers on exit + pub_model_states_event_.reset(); + ROS_DEBUG_STREAM_NAMED("api_plugin", "Disconnected World Updates"); + + // Stop the multi threaded ROS spinner + async_ros_spin_->stop(); + ROS_DEBUG_STREAM_NAMED("api_plugin", "Async ROS Spin Stopped"); + + // Shutdown the ROS node + nh_->shutdown(); + ROS_DEBUG_STREAM_NAMED("api_plugin", "Node Handle Shutdown"); + + // Shutdown ROS queue + gazebo_callback_queue_thread_->join(); + ROS_DEBUG_STREAM_NAMED("api_plugin", "Callback Queue Joined"); + + // Physics Dynamic Reconfigure + physics_reconfigure_thread_->join(); + ROS_DEBUG_STREAM_NAMED("api_plugin", "Physics reconfigure joined"); + + // Delete Force and Wrench Jobs + lock_.lock(); + for (std::vector::iterator iter = force_joint_jobs_.begin(); iter != force_joint_jobs_.end();) + { + delete (*iter); + iter = force_joint_jobs_.erase(iter); + } + force_joint_jobs_.clear(); + ROS_DEBUG_STREAM_NAMED("api_plugin", "ForceJointJobs deleted"); + for (std::vector::iterator iter = wrench_body_jobs_.begin(); iter != wrench_body_jobs_.end();) + { + delete (*iter); + iter = wrench_body_jobs_.erase(iter); + } + wrench_body_jobs_.clear(); + lock_.unlock(); + ROS_DEBUG_STREAM_NAMED("api_plugin", "WrenchBodyJobs deleted"); + + ROS_DEBUG_STREAM_NAMED("api_plugin", "Unloaded"); +} + +void GazeboRosApiPlugin::shutdownSignal() +{ + ROS_DEBUG_STREAM_NAMED("api_plugin", "shutdownSignal() recieved"); + stop_ = true; +} + +void GazeboRosApiPlugin::Load(int argc, char **argv) +{ + ROS_DEBUG_STREAM_NAMED("api_plugin", "Load"); + + // Added by zhangshuai 2019.05.10 ----Begin + /// use argv to name the gazebo ros node + std::string gazebo_node_name = "gazebo"; + for (int i = 0; i < argc; i++) + { + std::string tmp_string = argv[i]; + std::cout << "******" << i << ": " << tmp_string << std::endl; + if (tmp_string == "-gname") + { + gazebo_node_name = argv[i + 1]; + } + } + // Added by zhangshuai 2019.05.10 ----End + + // connect to sigint event + sigint_event_ = gazebo::event::Events::ConnectSigInt(boost::bind(&GazeboRosApiPlugin::shutdownSignal, this)); + + // setup ros related + if (!ros::isInitialized()) + ros::init(argc, argv, gazebo_node_name, ros::init_options::NoSigintHandler); + else + ROS_ERROR_NAMED("api_plugin", "Something other than this gazebo_ros_api plugin started ros::init(...), command line arguments may not be parsed properly."); + + // check if the ros master is available - required + while (!ros::master::check()) + { + ROS_WARN_STREAM_NAMED("api_plugin", "No ROS master - start roscore to continue..."); + // wait 0.5 second + usleep(500 * 1000); // can't use ROS Time here b/c node handle is not yet initialized + + if (stop_) + { + ROS_WARN_STREAM_NAMED("api_plugin", "Canceled loading Gazebo ROS API plugin by sigint event"); + return; + } + } + + nh_.reset(new ros::NodeHandle("~")); // advertise topics and services in this node's namespace + + // Built-in multi-threaded ROS spinning + async_ros_spin_.reset(new ros::AsyncSpinner(0)); // will use a thread for each CPU core + async_ros_spin_->start(); + + /// \brief setup custom callback queue + gazebo_callback_queue_thread_.reset(new boost::thread(&GazeboRosApiPlugin::gazeboQueueThread, this)); + + /// \brief start a thread for the physics dynamic reconfigure node + physics_reconfigure_thread_.reset(new boost::thread(boost::bind(&GazeboRosApiPlugin::physicsReconfigureThread, this))); + + // below needs the world to be created first + load_gazebo_ros_api_plugin_event_ = gazebo::event::Events::ConnectWorldCreated(boost::bind(&GazeboRosApiPlugin::loadGazeboRosApiPlugin, this, _1)); + + plugin_loaded_ = true; + ROS_INFO_NAMED("api_plugin", "Finished loading Gazebo ROS API Plugin."); +} + +void GazeboRosApiPlugin::loadGazeboRosApiPlugin(std::string world_name) +{ + // make sure things are only called once + lock_.lock(); + if (world_created_) + { + lock_.unlock(); + return; + } + + // set flag to true and load this plugin + world_created_ = true; + lock_.unlock(); + + world_ = gazebo::physics::get_world(world_name); + if (!world_) + { + //ROS_ERROR_NAMED("api_plugin", "world name: [%s]",world->Name().c_str()); + // connect helper function to signal for scheduling torque/forces, etc + ROS_FATAL_NAMED("api_plugin", "cannot load gazebo ros api server plugin, physics::get_world() fails to return world"); + return; + } + + gazebonode_ = gazebo::transport::NodePtr(new gazebo::transport::Node()); + gazebonode_->Init(world_name); + //stat_sub_ = gazebonode_->Subscribe("~/world_stats", &GazeboRosApiPlugin::publishSimTime, this); // TODO: does not work in server plugin? + factory_pub_ = gazebonode_->Advertise("~/factory"); + factory_light_pub_ = gazebonode_->Advertise("~/factory/light"); + light_modify_pub_ = gazebonode_->Advertise("~/light/modify"); + request_pub_ = gazebonode_->Advertise("~/request"); + response_sub_ = gazebonode_->Subscribe("~/response", &GazeboRosApiPlugin::onResponse, this); + + // reset topic connection counts + pub_link_states_connection_count_ = 0; + pub_model_states_connection_count_ = 0; + + /// \brief advertise all services + advertiseServices(); + + // hooks for applying forces, publishing simtime on /clock + wrench_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::wrenchBodySchedulerSlot, this)); + force_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::forceJointSchedulerSlot, this)); + time_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishSimTime, this)); +} + +void GazeboRosApiPlugin::onResponse(ConstResponsePtr &response) +{ +} + +void GazeboRosApiPlugin::gazeboQueueThread() +{ + static const double timeout = 0.001; + while (nh_->ok()) + { + gazebo_queue_.callAvailable(ros::WallDuration(timeout)); + } +} + +void GazeboRosApiPlugin::advertiseServices() +{ + // publish clock for simulated ros time + pub_clock_ = nh_->advertise("/clock", 10); + + // Advertise spawn services on the custom queue + std::string spawn_sdf_model_service_name("spawn_sdf_model"); + ros::AdvertiseServiceOptions spawn_sdf_model_aso = + ros::AdvertiseServiceOptions::create( + spawn_sdf_model_service_name, + boost::bind(&GazeboRosApiPlugin::spawnSDFModel, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + spawn_sdf_model_service_ = nh_->advertiseService(spawn_sdf_model_aso); + + // Advertise spawn services on the custom queue + std::string spawn_urdf_model_service_name("spawn_urdf_model"); + ros::AdvertiseServiceOptions spawn_urdf_model_aso = + ros::AdvertiseServiceOptions::create( + spawn_urdf_model_service_name, + boost::bind(&GazeboRosApiPlugin::spawnURDFModel, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + spawn_urdf_model_service_ = nh_->advertiseService(spawn_urdf_model_aso); + + // Advertise delete services on the custom queue + std::string delete_model_service_name("delete_model"); + ros::AdvertiseServiceOptions delete_aso = + ros::AdvertiseServiceOptions::create( + delete_model_service_name, + boost::bind(&GazeboRosApiPlugin::deleteModel, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + delete_model_service_ = nh_->advertiseService(delete_aso); + + // Advertise delete service for lights on the custom queue + std::string delete_light_service_name("delete_light"); + ros::AdvertiseServiceOptions delete_light_aso = + ros::AdvertiseServiceOptions::create( + delete_light_service_name, + boost::bind(&GazeboRosApiPlugin::deleteLight, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + delete_light_service_ = nh_->advertiseService(delete_light_aso); + + // Advertise more services on the custom queue + std::string get_model_properties_service_name("get_model_properties"); + ros::AdvertiseServiceOptions get_model_properties_aso = + ros::AdvertiseServiceOptions::create( + get_model_properties_service_name, + boost::bind(&GazeboRosApiPlugin::getModelProperties, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + get_model_properties_service_ = nh_->advertiseService(get_model_properties_aso); + + // Advertise more services on the custom queue + std::string get_model_state_service_name("get_model_state"); + ros::AdvertiseServiceOptions get_model_state_aso = + ros::AdvertiseServiceOptions::create( + get_model_state_service_name, + boost::bind(&GazeboRosApiPlugin::getModelState, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + get_model_state_service_ = nh_->advertiseService(get_model_state_aso); + + // Advertise more services on the custom queue + std::string get_world_properties_service_name("get_world_properties"); + ros::AdvertiseServiceOptions get_world_properties_aso = + ros::AdvertiseServiceOptions::create( + get_world_properties_service_name, + boost::bind(&GazeboRosApiPlugin::getWorldProperties, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + get_world_properties_service_ = nh_->advertiseService(get_world_properties_aso); + + // Advertise more services on the custom queue + std::string get_joint_properties_service_name("get_joint_properties"); + ros::AdvertiseServiceOptions get_joint_properties_aso = + ros::AdvertiseServiceOptions::create( + get_joint_properties_service_name, + boost::bind(&GazeboRosApiPlugin::getJointProperties, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + get_joint_properties_service_ = nh_->advertiseService(get_joint_properties_aso); + + // Advertise more services on the custom queue + std::string get_link_properties_service_name("get_link_properties"); + ros::AdvertiseServiceOptions get_link_properties_aso = + ros::AdvertiseServiceOptions::create( + get_link_properties_service_name, + boost::bind(&GazeboRosApiPlugin::getLinkProperties, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + get_link_properties_service_ = nh_->advertiseService(get_link_properties_aso); + + // Advertise more services on the custom queue + std::string get_link_state_service_name("get_link_state"); + ros::AdvertiseServiceOptions get_link_state_aso = + ros::AdvertiseServiceOptions::create( + get_link_state_service_name, + boost::bind(&GazeboRosApiPlugin::getLinkState, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + get_link_state_service_ = nh_->advertiseService(get_link_state_aso); + + // Advertise more services on the custom queue + std::string get_light_properties_service_name("get_light_properties"); + ros::AdvertiseServiceOptions get_light_properties_aso = + ros::AdvertiseServiceOptions::create( + get_light_properties_service_name, + boost::bind(&GazeboRosApiPlugin::getLightProperties, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + get_light_properties_service_ = nh_->advertiseService(get_light_properties_aso); + + // Advertise more services on the custom queue + std::string set_light_properties_service_name("set_light_properties"); + ros::AdvertiseServiceOptions set_light_properties_aso = + ros::AdvertiseServiceOptions::create( + set_light_properties_service_name, + boost::bind(&GazeboRosApiPlugin::setLightProperties, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + set_light_properties_service_ = nh_->advertiseService(set_light_properties_aso); + + // Advertise more services on the custom queue + std::string get_physics_properties_service_name("get_physics_properties"); + ros::AdvertiseServiceOptions get_physics_properties_aso = + ros::AdvertiseServiceOptions::create( + get_physics_properties_service_name, + boost::bind(&GazeboRosApiPlugin::getPhysicsProperties, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + get_physics_properties_service_ = nh_->advertiseService(get_physics_properties_aso); + + // publish complete link states in world frame + ros::AdvertiseOptions pub_link_states_ao = + ros::AdvertiseOptions::create( + "link_states", 10, + boost::bind(&GazeboRosApiPlugin::onLinkStatesConnect, this), + boost::bind(&GazeboRosApiPlugin::onLinkStatesDisconnect, this), + ros::VoidPtr(), &gazebo_queue_); + pub_link_states_ = nh_->advertise(pub_link_states_ao); + + // publish complete model states in world frame + ros::AdvertiseOptions pub_model_states_ao = + ros::AdvertiseOptions::create( + "model_states", 10, + boost::bind(&GazeboRosApiPlugin::onModelStatesConnect, this), + boost::bind(&GazeboRosApiPlugin::onModelStatesDisconnect, this), + ros::VoidPtr(), &gazebo_queue_); + pub_model_states_ = nh_->advertise(pub_model_states_ao); + + // Advertise more services on the custom queue + std::string set_link_properties_service_name("set_link_properties"); + ros::AdvertiseServiceOptions set_link_properties_aso = + ros::AdvertiseServiceOptions::create( + set_link_properties_service_name, + boost::bind(&GazeboRosApiPlugin::setLinkProperties, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + set_link_properties_service_ = nh_->advertiseService(set_link_properties_aso); + + // Advertise more services on the custom queue + std::string set_physics_properties_service_name("set_physics_properties"); + ros::AdvertiseServiceOptions set_physics_properties_aso = + ros::AdvertiseServiceOptions::create( + set_physics_properties_service_name, + boost::bind(&GazeboRosApiPlugin::setPhysicsProperties, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + set_physics_properties_service_ = nh_->advertiseService(set_physics_properties_aso); + + // Advertise more services on the custom queue + std::string set_model_state_service_name("set_model_state"); + ros::AdvertiseServiceOptions set_model_state_aso = + ros::AdvertiseServiceOptions::create( + set_model_state_service_name, + boost::bind(&GazeboRosApiPlugin::setModelState, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + set_model_state_service_ = nh_->advertiseService(set_model_state_aso); + + // Advertise more services on the custom queue + std::string set_model_configuration_service_name("set_model_configuration"); + ros::AdvertiseServiceOptions set_model_configuration_aso = + ros::AdvertiseServiceOptions::create( + set_model_configuration_service_name, + boost::bind(&GazeboRosApiPlugin::setModelConfiguration, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + set_model_configuration_service_ = nh_->advertiseService(set_model_configuration_aso); + + // Advertise more services on the custom queue + std::string set_joint_properties_service_name("set_joint_properties"); + ros::AdvertiseServiceOptions set_joint_properties_aso = + ros::AdvertiseServiceOptions::create( + set_joint_properties_service_name, + boost::bind(&GazeboRosApiPlugin::setJointProperties, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + set_joint_properties_service_ = nh_->advertiseService(set_joint_properties_aso); + + // Advertise more services on the custom queue + std::string set_link_state_service_name("set_link_state"); + ros::AdvertiseServiceOptions set_link_state_aso = + ros::AdvertiseServiceOptions::create( + set_link_state_service_name, + boost::bind(&GazeboRosApiPlugin::setLinkState, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + set_link_state_service_ = nh_->advertiseService(set_link_state_aso); + + // Advertise topic on custom queue + // topic callback version for set_link_state + ros::SubscribeOptions link_state_so = + ros::SubscribeOptions::create( + "set_link_state", 10, + boost::bind(&GazeboRosApiPlugin::updateLinkState, this, _1), + ros::VoidPtr(), &gazebo_queue_); + set_link_state_topic_ = nh_->subscribe(link_state_so); + + // topic callback version for set_model_state + ros::SubscribeOptions model_state_so = + ros::SubscribeOptions::create( + "set_model_state", 10, + boost::bind(&GazeboRosApiPlugin::updateModelState, this, _1), + ros::VoidPtr(), &gazebo_queue_); + set_model_state_topic_ = nh_->subscribe(model_state_so); + + // Advertise more services on the custom queue + std::string pause_physics_service_name("pause_physics"); + ros::AdvertiseServiceOptions pause_physics_aso = + ros::AdvertiseServiceOptions::create( + pause_physics_service_name, + boost::bind(&GazeboRosApiPlugin::pausePhysics, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + pause_physics_service_ = nh_->advertiseService(pause_physics_aso); + + // Advertise more services on the custom queue + std::string unpause_physics_service_name("unpause_physics"); + ros::AdvertiseServiceOptions unpause_physics_aso = + ros::AdvertiseServiceOptions::create( + unpause_physics_service_name, + boost::bind(&GazeboRosApiPlugin::unpausePhysics, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + unpause_physics_service_ = nh_->advertiseService(unpause_physics_aso); + + // Advertise more services on the custom queue + std::string apply_body_wrench_service_name("apply_body_wrench"); + ros::AdvertiseServiceOptions apply_body_wrench_aso = + ros::AdvertiseServiceOptions::create( + apply_body_wrench_service_name, + boost::bind(&GazeboRosApiPlugin::applyBodyWrench, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + apply_body_wrench_service_ = nh_->advertiseService(apply_body_wrench_aso); + + // Advertise more services on the custom queue + std::string apply_joint_effort_service_name("apply_joint_effort"); + ros::AdvertiseServiceOptions apply_joint_effort_aso = + ros::AdvertiseServiceOptions::create( + apply_joint_effort_service_name, + boost::bind(&GazeboRosApiPlugin::applyJointEffort, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + apply_joint_effort_service_ = nh_->advertiseService(apply_joint_effort_aso); + + // Advertise more services on the custom queue + std::string clear_joint_forces_service_name("clear_joint_forces"); + ros::AdvertiseServiceOptions clear_joint_forces_aso = + ros::AdvertiseServiceOptions::create( + clear_joint_forces_service_name, + boost::bind(&GazeboRosApiPlugin::clearJointForces, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + clear_joint_forces_service_ = nh_->advertiseService(clear_joint_forces_aso); + + // Advertise more services on the custom queue + std::string clear_body_wrenches_service_name("clear_body_wrenches"); + ros::AdvertiseServiceOptions clear_body_wrenches_aso = + ros::AdvertiseServiceOptions::create( + clear_body_wrenches_service_name, + boost::bind(&GazeboRosApiPlugin::clearBodyWrenches, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + clear_body_wrenches_service_ = nh_->advertiseService(clear_body_wrenches_aso); + + // Advertise more services on the custom queue + std::string reset_simulation_service_name("reset_simulation"); + ros::AdvertiseServiceOptions reset_simulation_aso = + ros::AdvertiseServiceOptions::create( + reset_simulation_service_name, + boost::bind(&GazeboRosApiPlugin::resetSimulation, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + reset_simulation_service_ = nh_->advertiseService(reset_simulation_aso); + + // Advertise more services on the custom queue + std::string reset_world_service_name("reset_world"); + ros::AdvertiseServiceOptions reset_world_aso = + ros::AdvertiseServiceOptions::create( + reset_world_service_name, + boost::bind(&GazeboRosApiPlugin::resetWorld, this, _1, _2), + ros::VoidPtr(), &gazebo_queue_); + reset_world_service_ = nh_->advertiseService(reset_world_aso); + + // set param for use_sim_time if not set by user already + if (!(nh_->hasParam("/use_sim_time"))) + nh_->setParam("/use_sim_time", true); + + // todo: contemplate setting environment variable ROBOT=sim here??? + nh_->getParam("pub_clock_frequency", pub_clock_frequency_); +#if GAZEBO_MAJOR_VERSION >= 8 + last_pub_clock_time_ = world_->SimTime(); +#else + last_pub_clock_time_ = world_->GetSimTime(); +#endif +} + +void GazeboRosApiPlugin::onLinkStatesConnect() +{ + pub_link_states_connection_count_++; + if (pub_link_states_connection_count_ == 1) // connect on first subscriber + pub_link_states_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishLinkStates, this)); +} + +void GazeboRosApiPlugin::onModelStatesConnect() +{ + pub_model_states_connection_count_++; + if (pub_model_states_connection_count_ == 1) // connect on first subscriber + pub_model_states_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(boost::bind(&GazeboRosApiPlugin::publishModelStates, this)); +} + +void GazeboRosApiPlugin::onLinkStatesDisconnect() +{ + pub_link_states_connection_count_--; + if (pub_link_states_connection_count_ <= 0) // disconnect with no subscribers + { + pub_link_states_event_.reset(); + if (pub_link_states_connection_count_ < 0) // should not be possible + ROS_ERROR_NAMED("api_plugin", "One too mandy disconnect from pub_link_states_ in gazebo_ros.cpp? something weird"); + } +} + +void GazeboRosApiPlugin::onModelStatesDisconnect() +{ + pub_model_states_connection_count_--; + if (pub_model_states_connection_count_ <= 0) // disconnect with no subscribers + { + pub_model_states_event_.reset(); + if (pub_model_states_connection_count_ < 0) // should not be possible + ROS_ERROR_NAMED("api_plugin", "One too mandy disconnect from pub_model_states_ in gazebo_ros.cpp? something weird"); + } +} + +bool GazeboRosApiPlugin::spawnURDFModel(gazebo_msgs::SpawnModel::Request &req, + gazebo_msgs::SpawnModel::Response &res) +{ + // get namespace for the corresponding model plugins + robot_namespace_ = req.robot_namespace; + + // incoming entity string + std::string model_xml = req.model_xml; + + if (!isURDF(model_xml)) + { + ROS_ERROR_NAMED("api_plugin", "SpawnModel: Failure - entity format is invalid."); + res.success = false; + res.status_message = "SpawnModel: Failure - entity format is invalid."; + return false; + } + + /// STRIP DECLARATION from model_xml + /// @todo: does tinyxml have functionality for this? + /// @todo: should gazebo take care of the declaration? + { + std::string open_bracket(""); + size_t pos1 = model_xml.find(open_bracket, 0); + size_t pos2 = model_xml.find(close_bracket, 0); + if (pos1 != std::string::npos && pos2 != std::string::npos) + model_xml.replace(pos1, pos2 - pos1 + 2, std::string("")); + } + + // Remove comments from URDF + { + std::string open_comment(""); + size_t pos1; + while ((pos1 = model_xml.find(open_comment, 0)) != std::string::npos) + { + size_t pos2 = model_xml.find(close_comment, 0); + if (pos2 != std::string::npos) + model_xml.replace(pos1, pos2 - pos1 + 3, std::string("")); + } + } + + // Now, replace package://xxx with the full path to the package + { + std::string package_prefix("package://"); + size_t pos1 = model_xml.find(package_prefix, 0); + while (pos1 != std::string::npos) + { + size_t pos2 = model_xml.find("/", pos1 + 10); + //ROS_DEBUG_NAMED("api_plugin", " pos %d %d",(int)pos1, (int)pos2); + if (pos2 == std::string::npos || pos1 >= pos2) + { + ROS_ERROR_NAMED("api_plugin", "Malformed package name?"); + break; + } + + std::string package_name = model_xml.substr(pos1 + 10, pos2 - pos1 - 10); + //ROS_DEBUG_NAMED("api_plugin", "package name [%s]", package_name.c_str()); + std::string package_path = ros::package::getPath(package_name); + if (package_path.empty()) + { + ROS_FATAL_NAMED("api_plugin", "Package[%s] does not have a path", package_name.c_str()); + res.success = false; + res.status_message = "urdf reference package name does not exist: " + package_name; + return false; + } + ROS_DEBUG_ONCE_NAMED("api_plugin", "Package name [%s] has path [%s]", package_name.c_str(), package_path.c_str()); + + model_xml.replace(pos1, (pos2 - pos1), package_path); + pos1 = model_xml.find(package_prefix, pos1); + } + } + // ROS_DEBUG_NAMED("api_plugin", "Model XML\n\n%s\n\n ",model_xml.c_str()); + + req.model_xml = model_xml; + + // Model is now considered convert to SDF + return spawnSDFModel(req, res); +} + +bool GazeboRosApiPlugin::spawnSDFModel(gazebo_msgs::SpawnModel::Request &req, + gazebo_msgs::SpawnModel::Response &res) +{ + // incoming entity name + std::string model_name = req.model_name; + + // get namespace for the corresponding model plugins + robot_namespace_ = req.robot_namespace; + + // get initial pose of model + ignition::math::Vector3d initial_xyz(req.initial_pose.position.x, req.initial_pose.position.y, req.initial_pose.position.z); + // get initial roll pitch yaw (fixed frame transform) + ignition::math::Quaterniond initial_q(req.initial_pose.orientation.w, req.initial_pose.orientation.x, req.initial_pose.orientation.y, req.initial_pose.orientation.z); + + // refernce frame for initial pose definition, modify initial pose if defined +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::EntityPtr frame = world_->EntityByName(req.reference_frame); +#else + gazebo::physics::EntityPtr frame = world_->GetEntity(req.reference_frame); +#endif + if (frame) + { + // convert to relative pose +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d frame_pose = frame->WorldPose(); +#else + ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); +#endif + initial_xyz = frame_pose.Pos() + frame_pose.Rot().RotateVector(initial_xyz); + initial_q = frame_pose.Rot() * initial_q; + } + + /// @todo: map is really wrong, need to use tf here somehow + else if (req.reference_frame == "" || req.reference_frame == "world" || req.reference_frame == "map" || req.reference_frame == "/map") + { + ROS_DEBUG_NAMED("api_plugin", "SpawnModel: reference_frame is empty/world/map, using inertial frame"); + } + else + { + res.success = false; + res.status_message = "SpawnModel: reference reference_frame not found, did you forget to scope the link by model name?"; + return true; + } + + // incoming robot model string + std::string model_xml = req.model_xml; + + // store resulting Gazebo Model XML to be sent to spawn queue + // get incoming string containg either an URDF or a Gazebo Model XML + // grab from parameter server if necessary convert to SDF if necessary + stripXmlDeclaration(model_xml); + + // put string in TiXmlDocument for manipulation + TiXmlDocument gazebo_model_xml; + gazebo_model_xml.Parse(model_xml.c_str()); + + // optional model manipulations: update initial pose && replace model name + if (isSDF(model_xml)) + { + updateSDFAttributes(gazebo_model_xml, model_name, initial_xyz, initial_q); + + // Walk recursively through the entire SDF, locate plugin tags and + // add robotNamespace as a child with the correct namespace + if (!this->robot_namespace_.empty()) + { + // Get root element for SDF + // TODO: implement the spawning also with and + TiXmlNode *model_tixml = gazebo_model_xml.FirstChild("sdf"); + model_tixml = (!model_tixml) ? gazebo_model_xml.FirstChild("gazebo") : model_tixml; + if (model_tixml) + { + walkChildAddRobotNamespace(model_tixml); + } + else + { + ROS_WARN_NAMED("api_plugin", "Unable to add robot namespace to xml"); + } + } + } + else if (isURDF(model_xml)) + { + updateURDFModelPose(gazebo_model_xml, initial_xyz, initial_q); + updateURDFName(gazebo_model_xml, model_name); + + // Walk recursively through the entire URDF, locate plugin tags and + // add robotNamespace as a child with the correct namespace + if (!this->robot_namespace_.empty()) + { + // Get root element for URDF + TiXmlNode *model_tixml = gazebo_model_xml.FirstChild("robot"); + if (model_tixml) + { + walkChildAddRobotNamespace(model_tixml); + } + else + { + ROS_WARN_NAMED("api_plugin", "Unable to add robot namespace to xml"); + } + } + } + else + { + ROS_ERROR_NAMED("api_plugin", "GazeboRosApiPlugin SpawnModel Failure: input xml format not recognized"); + res.success = false; + res.status_message = "GazeboRosApiPlugin SpawnModel Failure: input model_xml not SDF or URDF, or cannot be converted to Gazebo compatible format."; + return true; + } + + // do spawning check if spawn worked, return response + return spawnAndConform(gazebo_model_xml, model_name, res); +} + +bool GazeboRosApiPlugin::deleteModel(gazebo_msgs::DeleteModel::Request &req, + gazebo_msgs::DeleteModel::Response &res) +{ + // clear forces, etc for the body in question +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::ModelPtr model = world_->ModelByName(req.model_name); +#else + gazebo::physics::ModelPtr model = world_->GetModel(req.model_name); +#endif + if (!model) + { + ROS_ERROR_NAMED("api_plugin", "DeleteModel: model [%s] does not exist", req.model_name.c_str()); + res.success = false; + res.status_message = "DeleteModel: model does not exist"; + return true; + } + + // delete wrench jobs on bodies + for (unsigned int i = 0; i < model->GetChildCount(); i++) + { + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(model->GetChild(i)); + if (body) + { + // look for it in jobs, delete body wrench jobs + clearBodyWrenches(body->GetScopedName()); + } + } + + // delete force jobs on joints + gazebo::physics::Joint_V joints = model->GetJoints(); + for (unsigned int i = 0; i < joints.size(); i++) + { + // look for it in jobs, delete joint force jobs + clearJointForces(joints[i]->GetName()); + } + + // send delete model request + gazebo::msgs::Request *msg = gazebo::msgs::CreateRequest("entity_delete", req.model_name); + request_pub_->Publish(*msg, true); + + ros::Duration model_spawn_timeout(60.0); + ros::Time timeout = ros::Time::now() + model_spawn_timeout; + // wait and verify that model is deleted + while (true) + { + if (ros::Time::now() > timeout) + { + res.success = false; + res.status_message = "DeleteModel: Model pushed to delete queue, but delete service timed out waiting for model to disappear from simulation"; + return true; + } + { + //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex()); +#if GAZEBO_MAJOR_VERSION >= 8 + if (!world_->ModelByName(req.model_name)) + break; +#else + if (!world_->GetModel(req.model_name)) + break; +#endif + } + ROS_DEBUG_NAMED("api_plugin", "Waiting for model deletion (%s)", req.model_name.c_str()); + usleep(1000); + } + + // set result + res.success = true; + res.status_message = "DeleteModel: successfully deleted model"; + return true; +} + +bool GazeboRosApiPlugin::deleteLight(gazebo_msgs::DeleteLight::Request &req, + gazebo_msgs::DeleteLight::Response &res) +{ +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::LightPtr phy_light = world_->LightByName(req.light_name); +#else + gazebo::physics::LightPtr phy_light = world_->Light(req.light_name); +#endif + + if (phy_light == NULL) + { + res.success = false; + res.status_message = "DeleteLight: Requested light " + req.light_name + " not found!"; + } + else + { + gazebo::msgs::Request *msg = gazebo::msgs::CreateRequest("entity_delete", req.light_name); + request_pub_->Publish(*msg, true); + + res.success = false; + + for (int i = 0; i < 100; i++) + { +#if GAZEBO_MAJOR_VERSION >= 8 + phy_light = world_->LightByName(req.light_name); +#else + phy_light = world_->Light(req.light_name); +#endif + if (phy_light == NULL) + { + res.success = true; + res.status_message = "DeleteLight: " + req.light_name + " successfully deleted"; + return true; + } + // Check every 100ms + usleep(100000); + } + } + + res.status_message = "DeleteLight: Timeout reached while removing light \"" + req.light_name + "\""; + + return true; +} + +bool GazeboRosApiPlugin::getModelState(gazebo_msgs::GetModelState::Request &req, + gazebo_msgs::GetModelState::Response &res) +{ +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::ModelPtr model = world_->ModelByName(req.model_name); + gazebo::physics::EntityPtr frame = world_->EntityByName(req.relative_entity_name); +#else + gazebo::physics::ModelPtr model = world_->GetModel(req.model_name); + gazebo::physics::EntityPtr frame = world_->GetEntity(req.relative_entity_name); +#endif + if (!model) + { + ROS_ERROR_NAMED("api_plugin", "GetModelState: model [%s] does not exist", req.model_name.c_str()); + res.success = false; + res.status_message = "GetModelState: model does not exist"; + return true; + } + else + { + /** + * @brief creates a header for the result + * @author Markus Bader markus.bader@tuwien.ac.at + * @date 21th Nov 2014 + **/ + { + std::map::iterator it = access_count_get_model_state_.find(req.model_name); + if (it == access_count_get_model_state_.end()) + { + access_count_get_model_state_.insert(std::pair(req.model_name, 1)); + res.header.seq = 1; + } + else + { + it->second++; + res.header.seq = it->second; + } + res.header.stamp = ros::Time::now(); + res.header.frame_id = req.relative_entity_name; /// @brief this is a redundant information + } + // get model pose + // get model twist +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d model_pose = model->WorldPose(); + ignition::math::Vector3d model_linear_vel = model->WorldLinearVel(); + ignition::math::Vector3d model_angular_vel = model->WorldAngularVel(); +#else + ignition::math::Pose3d model_pose = model->GetWorldPose().Ign(); + ignition::math::Vector3d model_linear_vel = model->GetWorldLinearVel().Ign(); + ignition::math::Vector3d model_angular_vel = model->GetWorldAngularVel().Ign(); +#endif + ignition::math::Vector3d model_pos = model_pose.Pos(); + ignition::math::Quaterniond model_rot = model_pose.Rot(); + + if (frame) + { + // convert to relative pose, rates +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d frame_pose = frame->WorldPose(); + ignition::math::Vector3d frame_vpos = frame->WorldLinearVel(); // get velocity in gazebo frame + ignition::math::Vector3d frame_veul = frame->WorldAngularVel(); // get velocity in gazebo frame +#else + ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); + ignition::math::Vector3d frame_vpos = frame->GetWorldLinearVel().Ign(); // get velocity in gazebo frame + ignition::math::Vector3d frame_veul = frame->GetWorldAngularVel().Ign(); // get velocity in gazebo frame +#endif + ignition::math::Pose3d model_rel_pose = model_pose - frame_pose; + model_pos = model_rel_pose.Pos(); + model_rot = model_rel_pose.Rot(); + + model_linear_vel = frame_pose.Rot().RotateVectorReverse(model_linear_vel - frame_vpos); + model_angular_vel = frame_pose.Rot().RotateVectorReverse(model_angular_vel - frame_veul); + } + /// @todo: FIXME map is really wrong, need to use tf here somehow + else if (req.relative_entity_name == "" || req.relative_entity_name == "world" || req.relative_entity_name == "map" || req.relative_entity_name == "/map") + { + ROS_DEBUG_NAMED("api_plugin", "GetModelState: relative_entity_name is empty/world/map, using inertial frame"); + } + else + { + res.success = false; + res.status_message = "GetModelState: reference relative_entity_name not found, did you forget to scope the body by model name?"; + return true; + } + + // fill in response + res.pose.position.x = model_pos.X(); + res.pose.position.y = model_pos.Y(); + res.pose.position.z = model_pos.Z(); + res.pose.orientation.w = model_rot.W(); + res.pose.orientation.x = model_rot.X(); + res.pose.orientation.y = model_rot.Y(); + res.pose.orientation.z = model_rot.Z(); + + res.twist.linear.x = model_linear_vel.X(); + res.twist.linear.y = model_linear_vel.Y(); + res.twist.linear.z = model_linear_vel.Z(); + res.twist.angular.x = model_angular_vel.X(); + res.twist.angular.y = model_angular_vel.Y(); + res.twist.angular.z = model_angular_vel.Z(); + + res.success = true; + res.status_message = "GetModelState: got properties"; + return true; + } + return true; +} + +bool GazeboRosApiPlugin::getModelProperties(gazebo_msgs::GetModelProperties::Request &req, + gazebo_msgs::GetModelProperties::Response &res) +{ +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::ModelPtr model = world_->ModelByName(req.model_name); +#else + gazebo::physics::ModelPtr model = world_->GetModel(req.model_name); +#endif + if (!model) + { + ROS_ERROR_NAMED("api_plugin", "GetModelProperties: model [%s] does not exist", req.model_name.c_str()); + res.success = false; + res.status_message = "GetModelProperties: model does not exist"; + return true; + } + else + { + // get model parent name + gazebo::physics::ModelPtr parent_model = boost::dynamic_pointer_cast(model->GetParent()); + if (parent_model) + res.parent_model_name = parent_model->GetName(); + + // get list of child bodies, geoms + res.body_names.clear(); + res.geom_names.clear(); + for (unsigned int i = 0; i < model->GetChildCount(); i++) + { + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(model->GetChild(i)); + if (body) + { + res.body_names.push_back(body->GetName()); + // get list of geoms + for (unsigned int j = 0; j < body->GetChildCount(); j++) + { + gazebo::physics::CollisionPtr geom = boost::dynamic_pointer_cast(body->GetChild(j)); + if (geom) + res.geom_names.push_back(geom->GetName()); + } + } + } + + // get list of joints + res.joint_names.clear(); + + gazebo::physics::Joint_V joints = model->GetJoints(); + for (unsigned int i = 0; i < joints.size(); i++) + res.joint_names.push_back(joints[i]->GetName()); + + // get children model names + res.child_model_names.clear(); + for (unsigned int j = 0; j < model->GetChildCount(); j++) + { + gazebo::physics::ModelPtr child_model = boost::dynamic_pointer_cast(model->GetChild(j)); + if (child_model) + res.child_model_names.push_back(child_model->GetName()); + } + + // is model static + res.is_static = model->IsStatic(); + + res.success = true; + res.status_message = "GetModelProperties: got properties"; + return true; + } + return true; +} + +bool GazeboRosApiPlugin::getWorldProperties(gazebo_msgs::GetWorldProperties::Request &req, + gazebo_msgs::GetWorldProperties::Response &res) +{ + res.model_names.clear(); +#if GAZEBO_MAJOR_VERSION >= 8 + res.sim_time = world_->SimTime().Double(); + for (unsigned int i = 0; i < world_->ModelCount(); i++) + res.model_names.push_back(world_->ModelByIndex(i)->GetName()); +#else + res.sim_time = world_->GetSimTime().Double(); + for (unsigned int i = 0; i < world_->GetModelCount(); i++) + res.model_names.push_back(world_->GetModel(i)->GetName()); +#endif + gzerr << "disablign rendering has not been implemented, rendering is always enabled\n"; + res.rendering_enabled = true; //world->GetRenderEngineEnabled(); + res.success = true; + res.status_message = "GetWorldProperties: got properties"; + return true; +} + +bool GazeboRosApiPlugin::getJointProperties(gazebo_msgs::GetJointProperties::Request &req, + gazebo_msgs::GetJointProperties::Response &res) +{ + gazebo::physics::JointPtr joint; +#if GAZEBO_MAJOR_VERSION >= 8 + for (unsigned int i = 0; i < world_->ModelCount(); i++) + { + joint = world_->ModelByIndex(i)->GetJoint(req.joint_name); +#else + for (unsigned int i = 0; i < world_->GetModelCount(); i++) + { + joint = world_->GetModel(i)->GetJoint(req.joint_name); +#endif + if (joint) + break; + } + + if (!joint) + { + res.success = false; + res.status_message = "GetJointProperties: joint not found"; + return true; + } + else + { + /// @todo: FIXME + res.type = res.REVOLUTE; + + res.damping.clear(); // to be added to gazebo + //res.damping.push_back(joint->GetDamping(0)); + + res.position.clear(); +#if GAZEBO_MAJOR_VERSION >= 8 + res.position.push_back(joint->Position(0)); +#else + res.position.push_back(joint->GetAngle(0).Radian()); +#endif + + res.rate.clear(); // use GetVelocity(i) + res.rate.push_back(joint->GetVelocity(0)); + + res.success = true; + res.status_message = "GetJointProperties: got properties"; + return true; + } +} + +bool GazeboRosApiPlugin::getLinkProperties(gazebo_msgs::GetLinkProperties::Request &req, + gazebo_msgs::GetLinkProperties::Response &res) +{ +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(req.link_name)); +#else + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->GetEntity(req.link_name)); +#endif + if (!body) + { + res.success = false; + res.status_message = "GetLinkProperties: link not found, did you forget to scope the link by model name?"; + return true; + } + else + { + /// @todo: validate + res.gravity_mode = body->GetGravityMode(); + + gazebo::physics::InertialPtr inertia = body->GetInertial(); + +#if GAZEBO_MAJOR_VERSION >= 8 + res.mass = body->GetInertial()->Mass(); + + res.ixx = inertia->IXX(); + res.iyy = inertia->IYY(); + res.izz = inertia->IZZ(); + res.ixy = inertia->IXY(); + res.ixz = inertia->IXZ(); + res.iyz = inertia->IYZ(); + + ignition::math::Vector3d com = body->GetInertial()->CoG(); +#else + res.mass = body->GetInertial()->GetMass(); + + res.ixx = inertia->GetIXX(); + res.iyy = inertia->GetIYY(); + res.izz = inertia->GetIZZ(); + res.ixy = inertia->GetIXY(); + res.ixz = inertia->GetIXZ(); + res.iyz = inertia->GetIYZ(); + + ignition::math::Vector3d com = body->GetInertial()->GetCoG().Ign(); +#endif + res.com.position.x = com.X(); + res.com.position.y = com.Y(); + res.com.position.z = com.Z(); + res.com.orientation.x = 0; // @todo: gazebo do not support rotated inertia yet + res.com.orientation.y = 0; + res.com.orientation.z = 0; + res.com.orientation.w = 1; + + res.success = true; + res.status_message = "GetLinkProperties: got properties"; + return true; + } +} + +bool GazeboRosApiPlugin::getLinkState(gazebo_msgs::GetLinkState::Request &req, + gazebo_msgs::GetLinkState::Response &res) +{ +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(req.link_name)); + gazebo::physics::EntityPtr frame = world_->EntityByName(req.reference_frame); +#else + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->GetEntity(req.link_name)); + gazebo::physics::EntityPtr frame = world_->GetEntity(req.reference_frame); +#endif + + if (!body) + { + res.success = false; + res.status_message = "GetLinkState: link not found, did you forget to scope the link by model name?"; + return true; + } + + // get body pose + // Get inertial rates +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d body_pose = body->WorldPose(); + ignition::math::Vector3d body_vpos = body->WorldLinearVel(); // get velocity in gazebo frame + ignition::math::Vector3d body_veul = body->WorldAngularVel(); // get velocity in gazebo frame +#else + ignition::math::Pose3d body_pose = body->GetWorldPose().Ign(); + ignition::math::Vector3d body_vpos = body->GetWorldLinearVel().Ign(); // get velocity in gazebo frame + ignition::math::Vector3d body_veul = body->GetWorldAngularVel().Ign(); // get velocity in gazebo frame +#endif + + if (frame) + { + // convert to relative pose, rates +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d frame_pose = frame->WorldPose(); + ignition::math::Vector3d frame_vpos = frame->WorldLinearVel(); // get velocity in gazebo frame + ignition::math::Vector3d frame_veul = frame->WorldAngularVel(); // get velocity in gazebo frame +#else + ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); + ignition::math::Vector3d frame_vpos = frame->GetWorldLinearVel().Ign(); // get velocity in gazebo frame + ignition::math::Vector3d frame_veul = frame->GetWorldAngularVel().Ign(); // get velocity in gazebo frame +#endif + body_pose = body_pose - frame_pose; + + body_vpos = frame_pose.Rot().RotateVectorReverse(body_vpos - frame_vpos); + body_veul = frame_pose.Rot().RotateVectorReverse(body_veul - frame_veul); + } + /// @todo: FIXME map is really wrong, need to use tf here somehow + else if (req.reference_frame == "" || req.reference_frame == "world" || req.reference_frame == "map" || req.reference_frame == "/map") + { + ROS_DEBUG_NAMED("api_plugin", "GetLinkState: reference_frame is empty/world/map, using inertial frame"); + } + else + { + res.success = false; + res.status_message = "GetLinkState: reference reference_frame not found, did you forget to scope the link by model name?"; + return true; + } + + res.link_state.link_name = req.link_name; + res.link_state.pose.position.x = body_pose.Pos().X(); + res.link_state.pose.position.y = body_pose.Pos().Y(); + res.link_state.pose.position.z = body_pose.Pos().Z(); + res.link_state.pose.orientation.x = body_pose.Rot().X(); + res.link_state.pose.orientation.y = body_pose.Rot().Y(); + res.link_state.pose.orientation.z = body_pose.Rot().Z(); + res.link_state.pose.orientation.w = body_pose.Rot().W(); + res.link_state.twist.linear.x = body_vpos.X(); + res.link_state.twist.linear.y = body_vpos.Y(); + res.link_state.twist.linear.z = body_vpos.Z(); + res.link_state.twist.angular.x = body_veul.X(); + res.link_state.twist.angular.y = body_veul.Y(); + res.link_state.twist.angular.z = body_veul.Z(); + res.link_state.reference_frame = req.reference_frame; + + res.success = true; + res.status_message = "GetLinkState: got state"; + return true; +} + +bool GazeboRosApiPlugin::getLightProperties(gazebo_msgs::GetLightProperties::Request &req, + gazebo_msgs::GetLightProperties::Response &res) +{ +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::LightPtr phy_light = world_->LightByName(req.light_name); +#else + gazebo::physics::LightPtr phy_light = world_->Light(req.light_name); +#endif + + if (phy_light == NULL) + { + res.success = false; + res.status_message = "getLightProperties: Requested light " + req.light_name + " not found!"; + } + else + { + gazebo::msgs::Light light; + phy_light->FillMsg(light); + + res.diffuse.r = light.diffuse().r(); + res.diffuse.g = light.diffuse().g(); + res.diffuse.b = light.diffuse().b(); + res.diffuse.a = light.diffuse().a(); + + res.attenuation_constant = light.attenuation_constant(); + res.attenuation_linear = light.attenuation_linear(); + res.attenuation_quadratic = light.attenuation_quadratic(); + + res.success = true; + } + + return true; +} + +bool GazeboRosApiPlugin::setLightProperties(gazebo_msgs::SetLightProperties::Request &req, + gazebo_msgs::SetLightProperties::Response &res) +{ +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::LightPtr phy_light = world_->LightByName(req.light_name); +#else + gazebo::physics::LightPtr phy_light = world_->Light(req.light_name); +#endif + + if (phy_light == NULL) + { + res.success = false; + res.status_message = "setLightProperties: Requested light " + req.light_name + " not found!"; + } + else + { + gazebo::msgs::Light light; + + phy_light->FillMsg(light); + + light.mutable_diffuse()->set_r(req.diffuse.r); + light.mutable_diffuse()->set_g(req.diffuse.g); + light.mutable_diffuse()->set_b(req.diffuse.b); + light.mutable_diffuse()->set_a(req.diffuse.a); + + light.set_attenuation_constant(req.attenuation_constant); + light.set_attenuation_linear(req.attenuation_linear); + light.set_attenuation_quadratic(req.attenuation_quadratic); + + light_modify_pub_->Publish(light, true); + + res.success = true; + } + + return true; +} + +bool GazeboRosApiPlugin::setLinkProperties(gazebo_msgs::SetLinkProperties::Request &req, + gazebo_msgs::SetLinkProperties::Response &res) +{ +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(req.link_name)); +#else + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->GetEntity(req.link_name)); +#endif + if (!body) + { + res.success = false; + res.status_message = "SetLinkProperties: link not found, did you forget to scope the link by model name?"; + return true; + } + else + { + gazebo::physics::InertialPtr mass = body->GetInertial(); + // @todo: FIXME: add inertia matrix rotation to Gazebo + // mass.SetInertiaRotation(ignition::math::Quaterniondion(req.com.orientation.w,res.com.orientation.x,req.com.orientation.y req.com.orientation.z)); + mass->SetCoG(ignition::math::Vector3d(req.com.position.x, req.com.position.y, req.com.position.z)); + mass->SetInertiaMatrix(req.ixx, req.iyy, req.izz, req.ixy, req.ixz, req.iyz); + mass->SetMass(req.mass); + body->SetGravityMode(req.gravity_mode); + // @todo: mass change unverified + res.success = true; + res.status_message = "SetLinkProperties: properties set"; + return true; + } +} + +bool GazeboRosApiPlugin::setPhysicsProperties(gazebo_msgs::SetPhysicsProperties::Request &req, + gazebo_msgs::SetPhysicsProperties::Response &res) +{ + // pause simulation if requested + bool is_paused = world_->IsPaused(); + world_->SetPaused(true); + world_->SetGravity(ignition::math::Vector3d(req.gravity.x, req.gravity.y, req.gravity.z)); + + // supported updates +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::PhysicsEnginePtr pe = (world_->Physics()); +#else + gazebo::physics::PhysicsEnginePtr pe = (world_->GetPhysicsEngine()); +#endif + pe->SetMaxStepSize(req.time_step); + pe->SetRealTimeUpdateRate(req.max_update_rate); + + if (pe->GetType() == "ode") + { + // stuff only works in ODE right now + pe->SetAutoDisableFlag(req.ode_config.auto_disable_bodies); + pe->SetParam("precon_iters", int(req.ode_config.sor_pgs_precon_iters)); + pe->SetParam("iters", int(req.ode_config.sor_pgs_iters)); + pe->SetParam("sor", req.ode_config.sor_pgs_w); + pe->SetParam("cfm", req.ode_config.cfm); + pe->SetParam("erp", req.ode_config.erp); + pe->SetParam("contact_surface_layer", + req.ode_config.contact_surface_layer); + pe->SetParam("contact_max_correcting_vel", + req.ode_config.contact_max_correcting_vel); + pe->SetParam("max_contacts", int(req.ode_config.max_contacts)); + + world_->SetPaused(is_paused); + + res.success = true; + res.status_message = "physics engine updated"; + } + else + { + /// \TODO: add support for simbody, dart and bullet physics properties. + ROS_ERROR_NAMED("api_plugin", "ROS set_physics_properties service call does not yet support physics engine [%s].", pe->GetType().c_str()); + res.success = false; + res.status_message = "Physics engine [" + pe->GetType() + "]: set_physics_properties not supported."; + } + return res.success; +} + +bool GazeboRosApiPlugin::getPhysicsProperties(gazebo_msgs::GetPhysicsProperties::Request &req, + gazebo_msgs::GetPhysicsProperties::Response &res) +{ + // supported updates +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::PhysicsEnginePtr pe = (world_->Physics()); +#else + gazebo::physics::PhysicsEnginePtr pe = (world_->GetPhysicsEngine()); +#endif + res.time_step = pe->GetMaxStepSize(); + res.pause = world_->IsPaused(); + res.max_update_rate = pe->GetRealTimeUpdateRate(); + ignition::math::Vector3d gravity = world_->Gravity(); + res.gravity.x = gravity.X(); + res.gravity.y = gravity.Y(); + res.gravity.z = gravity.Z(); + + // stuff only works in ODE right now + if (pe->GetType() == "ode") + { + res.ode_config.auto_disable_bodies = + pe->GetAutoDisableFlag(); + res.ode_config.sor_pgs_precon_iters = boost::any_cast( + pe->GetParam("precon_iters")); + res.ode_config.sor_pgs_iters = boost::any_cast( + pe->GetParam("iters")); + res.ode_config.sor_pgs_w = boost::any_cast( + pe->GetParam("sor")); + res.ode_config.contact_surface_layer = boost::any_cast( + pe->GetParam("contact_surface_layer")); + res.ode_config.contact_max_correcting_vel = boost::any_cast( + pe->GetParam("contact_max_correcting_vel")); + res.ode_config.cfm = boost::any_cast( + pe->GetParam("cfm")); + res.ode_config.erp = boost::any_cast( + pe->GetParam("erp")); + res.ode_config.max_contacts = boost::any_cast( + pe->GetParam("max_contacts")); + + res.success = true; + res.status_message = "GetPhysicsProperties: got properties"; + } + else + { + /// \TODO: add support for simbody, dart and bullet physics properties. + ROS_ERROR_NAMED("api_plugin", "ROS get_physics_properties service call does not yet support physics engine [%s].", pe->GetType().c_str()); + res.success = false; + res.status_message = "Physics engine [" + pe->GetType() + "]: get_physics_properties not supported."; + } + return res.success; +} + +bool GazeboRosApiPlugin::setJointProperties(gazebo_msgs::SetJointProperties::Request &req, + gazebo_msgs::SetJointProperties::Response &res) +{ + /// @todo: current settings only allows for setting of 1DOF joints (e.g. HingeJoint and SliderJoint) correctly. + gazebo::physics::JointPtr joint; +#if GAZEBO_MAJOR_VERSION >= 8 + for (unsigned int i = 0; i < world_->ModelCount(); i++) + { + joint = world_->ModelByIndex(i)->GetJoint(req.joint_name); +#else + for (unsigned int i = 0; i < world_->GetModelCount(); i++) + { + joint = world_->GetModel(i)->GetJoint(req.joint_name); +#endif + if (joint) + break; + } + + if (!joint) + { + res.success = false; + res.status_message = "SetJointProperties: joint not found"; + return true; + } + else + { + for (unsigned int i = 0; i < req.ode_joint_config.damping.size(); i++) + joint->SetDamping(i, req.ode_joint_config.damping[i]); + for (unsigned int i = 0; i < req.ode_joint_config.hiStop.size(); i++) + joint->SetParam("hi_stop", i, req.ode_joint_config.hiStop[i]); + for (unsigned int i = 0; i < req.ode_joint_config.loStop.size(); i++) + joint->SetParam("lo_stop", i, req.ode_joint_config.loStop[i]); + for (unsigned int i = 0; i < req.ode_joint_config.erp.size(); i++) + joint->SetParam("erp", i, req.ode_joint_config.erp[i]); + for (unsigned int i = 0; i < req.ode_joint_config.cfm.size(); i++) + joint->SetParam("cfm", i, req.ode_joint_config.cfm[i]); + for (unsigned int i = 0; i < req.ode_joint_config.stop_erp.size(); i++) + joint->SetParam("stop_erp", i, req.ode_joint_config.stop_erp[i]); + for (unsigned int i = 0; i < req.ode_joint_config.stop_cfm.size(); i++) + joint->SetParam("stop_cfm", i, req.ode_joint_config.stop_cfm[i]); + for (unsigned int i = 0; i < req.ode_joint_config.fudge_factor.size(); i++) + joint->SetParam("fudge_factor", i, req.ode_joint_config.fudge_factor[i]); + for (unsigned int i = 0; i < req.ode_joint_config.fmax.size(); i++) + joint->SetParam("fmax", i, req.ode_joint_config.fmax[i]); + for (unsigned int i = 0; i < req.ode_joint_config.vel.size(); i++) + joint->SetParam("vel", i, req.ode_joint_config.vel[i]); + + res.success = true; + res.status_message = "SetJointProperties: properties set"; + return true; + } +} + +bool GazeboRosApiPlugin::setModelState(gazebo_msgs::SetModelState::Request &req, + gazebo_msgs::SetModelState::Response &res) +{ + ignition::math::Vector3d target_pos(req.model_state.pose.position.x, req.model_state.pose.position.y, req.model_state.pose.position.z); + ignition::math::Quaterniond target_rot(req.model_state.pose.orientation.w, req.model_state.pose.orientation.x, req.model_state.pose.orientation.y, req.model_state.pose.orientation.z); + target_rot.Normalize(); // eliminates invalid rotation (0, 0, 0, 0) + ignition::math::Pose3d target_pose(target_pos, target_rot); + ignition::math::Vector3d target_pos_dot(req.model_state.twist.linear.x, req.model_state.twist.linear.y, req.model_state.twist.linear.z); + ignition::math::Vector3d target_rot_dot(req.model_state.twist.angular.x, req.model_state.twist.angular.y, req.model_state.twist.angular.z); + +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::ModelPtr model = world_->ModelByName(req.model_state.model_name); +#else + gazebo::physics::ModelPtr model = world_->GetModel(req.model_state.model_name); +#endif + if (!model) + { + ROS_ERROR_NAMED("api_plugin", "Updating ModelState: model [%s] does not exist", req.model_state.model_name.c_str()); + res.success = false; + res.status_message = "SetModelState: model does not exist"; + return true; + } + else + { +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::EntityPtr relative_entity = world_->EntityByName(req.model_state.reference_frame); +#else + gazebo::physics::EntityPtr relative_entity = world_->GetEntity(req.model_state.reference_frame); +#endif + if (relative_entity) + { +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d frame_pose = relative_entity->WorldPose(); // - myBody->GetCoMPose(); +#else + ignition::math::Pose3d frame_pose = relative_entity->GetWorldPose().Ign(); // - myBody->GetCoMPose(); +#endif + + target_pose = target_pose + frame_pose; + + // Velocities should be commanded in the requested reference + // frame, so we need to translate them to the world frame + target_pos_dot = frame_pose.Rot().RotateVector(target_pos_dot); + target_rot_dot = frame_pose.Rot().RotateVector(target_rot_dot); + } + /// @todo: FIXME map is really wrong, need to use tf here somehow + else if (req.model_state.reference_frame == "" || req.model_state.reference_frame == "world" || req.model_state.reference_frame == "map" || req.model_state.reference_frame == "/map") + { + ROS_DEBUG_NAMED("api_plugin", "Updating ModelState: reference frame is empty/world/map, usig inertial frame"); + } + else + { + ROS_ERROR_NAMED("api_plugin", "Updating ModelState: for model[%s], specified reference frame entity [%s] does not exist", + req.model_state.model_name.c_str(), req.model_state.reference_frame.c_str()); + res.success = false; + res.status_message = "SetModelState: specified reference frame entity does not exist"; + return true; + } + + //ROS_ERROR_NAMED("api_plugin", "target state: %f %f %f",target_pose.Pos().X(),target_pose.Pos().Y(),target_pose.Pos().Z()); + bool is_paused = world_->IsPaused(); + world_->SetPaused(true); + model->SetWorldPose(target_pose); + world_->SetPaused(is_paused); + //ignition::math::Pose3d p3d = model->WorldPose(); + //ROS_ERROR_NAMED("api_plugin", "model updated state: %f %f %f",p3d.Pos().X(),p3d.Pos().Y(),p3d.Pos().Z()); + + // set model velocity + model->SetLinearVel(target_pos_dot); + model->SetAngularVel(target_rot_dot); + + res.success = true; + res.status_message = "SetModelState: set model state done"; + return true; + } +} + +void GazeboRosApiPlugin::updateModelState(const gazebo_msgs::ModelState::ConstPtr &model_state) +{ + gazebo_msgs::SetModelState::Response res; + gazebo_msgs::SetModelState::Request req; + req.model_state = *model_state; + /*bool success =*/setModelState(req, res); +} + +bool GazeboRosApiPlugin::applyJointEffort(gazebo_msgs::ApplyJointEffort::Request &req, + gazebo_msgs::ApplyJointEffort::Response &res) +{ + gazebo::physics::JointPtr joint; +#if GAZEBO_MAJOR_VERSION >= 8 + for (unsigned int i = 0; i < world_->ModelCount(); i++) + { + joint = world_->ModelByIndex(i)->GetJoint(req.joint_name); +#else + for (unsigned int i = 0; i < world_->GetModelCount(); i++) + { + joint = world_->GetModel(i)->GetJoint(req.joint_name); +#endif + if (joint) + { + GazeboRosApiPlugin::ForceJointJob *fjj = new GazeboRosApiPlugin::ForceJointJob; + fjj->joint = joint; + fjj->force = req.effort; + fjj->start_time = req.start_time; +#if GAZEBO_MAJOR_VERSION >= 8 + if (fjj->start_time < ros::Time(world_->SimTime().Double())) + fjj->start_time = ros::Time(world_->SimTime().Double()); +#else + if (fjj->start_time < ros::Time(world_->GetSimTime().Double())) + fjj->start_time = ros::Time(world_->GetSimTime().Double()); +#endif + fjj->duration = req.duration; + lock_.lock(); + force_joint_jobs_.push_back(fjj); + lock_.unlock(); + + res.success = true; + res.status_message = "ApplyJointEffort: effort set"; + return true; + } + } + + res.success = false; + res.status_message = "ApplyJointEffort: joint not found"; + return true; +} + +bool GazeboRosApiPlugin::resetSimulation(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + world_->Reset(); + return true; +} + +bool GazeboRosApiPlugin::resetWorld(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + world_->ResetEntities(gazebo::physics::Base::MODEL); + return true; +} + +bool GazeboRosApiPlugin::pausePhysics(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + world_->SetPaused(true); + return true; +} + +bool GazeboRosApiPlugin::unpausePhysics(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + world_->SetPaused(false); + return true; +} + +bool GazeboRosApiPlugin::clearJointForces(gazebo_msgs::JointRequest::Request &req, + gazebo_msgs::JointRequest::Response &res) +{ + return clearJointForces(req.joint_name); +} +bool GazeboRosApiPlugin::clearJointForces(std::string joint_name) +{ + bool search = true; + lock_.lock(); + while (search) + { + search = false; + for (std::vector::iterator iter = force_joint_jobs_.begin(); iter != force_joint_jobs_.end(); ++iter) + { + if ((*iter)->joint->GetName() == joint_name) + { + // found one, search through again + search = true; + delete (*iter); + force_joint_jobs_.erase(iter); + break; + } + } + } + lock_.unlock(); + return true; +} + +bool GazeboRosApiPlugin::clearBodyWrenches(gazebo_msgs::BodyRequest::Request &req, + gazebo_msgs::BodyRequest::Response &res) +{ + return clearBodyWrenches(req.body_name); +} +bool GazeboRosApiPlugin::clearBodyWrenches(std::string body_name) +{ + bool search = true; + lock_.lock(); + while (search) + { + search = false; + for (std::vector::iterator iter = wrench_body_jobs_.begin(); iter != wrench_body_jobs_.end(); ++iter) + { + //ROS_ERROR_NAMED("api_plugin", "search %s %s",(*iter)->body->GetScopedName().c_str(), body_name.c_str()); + if ((*iter)->body->GetScopedName() == body_name) + { + // found one, search through again + search = true; + delete (*iter); + wrench_body_jobs_.erase(iter); + break; + } + } + } + lock_.unlock(); + return true; +} + +bool GazeboRosApiPlugin::setModelConfiguration(gazebo_msgs::SetModelConfiguration::Request &req, + gazebo_msgs::SetModelConfiguration::Response &res) +{ + std::string gazebo_model_name = req.model_name; + + // search for model with name +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::ModelPtr gazebo_model = world_->ModelByName(req.model_name); +#else + gazebo::physics::ModelPtr gazebo_model = world_->GetModel(req.model_name); +#endif + if (!gazebo_model) + { + ROS_ERROR_NAMED("api_plugin", "SetModelConfiguration: model [%s] does not exist", gazebo_model_name.c_str()); + res.success = false; + res.status_message = "SetModelConfiguration: model does not exist"; + return true; + } + + if (req.joint_names.size() == req.joint_positions.size()) + { + std::map joint_position_map; + for (unsigned int i = 0; i < req.joint_names.size(); i++) + { + joint_position_map[req.joint_names[i]] = req.joint_positions[i]; + } + + // make the service call to pause gazebo + bool is_paused = world_->IsPaused(); + if (!is_paused) + world_->SetPaused(true); + + gazebo_model->SetJointPositions(joint_position_map); + + // resume paused state before this call + world_->SetPaused(is_paused); + + res.success = true; + res.status_message = "SetModelConfiguration: success"; + return true; + } + else + { + res.success = false; + res.status_message = "SetModelConfiguration: joint name and position list have different lengths"; + return true; + } +} + +bool GazeboRosApiPlugin::setLinkState(gazebo_msgs::SetLinkState::Request &req, + gazebo_msgs::SetLinkState::Response &res) +{ +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(req.link_state.link_name)); + gazebo::physics::LinkPtr frame = boost::dynamic_pointer_cast(world_->EntityByName(req.link_state.reference_frame)); +#else + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->GetEntity(req.link_state.link_name)); + gazebo::physics::EntityPtr frame = world_->GetEntity(req.link_state.reference_frame); +#endif + if (!body) + { + ROS_ERROR_NAMED("api_plugin", "Updating LinkState: link [%s] does not exist", req.link_state.link_name.c_str()); + res.success = false; + res.status_message = "SetLinkState: link does not exist"; + return true; + } + + /// @todo: FIXME map is really wrong, unless using tf here somehow + // get reference frame (body/model(link)) pose and + // transform target pose to absolute world frame + ignition::math::Vector3d target_pos(req.link_state.pose.position.x, req.link_state.pose.position.y, req.link_state.pose.position.z); + ignition::math::Quaterniond target_rot(req.link_state.pose.orientation.w, req.link_state.pose.orientation.x, req.link_state.pose.orientation.y, req.link_state.pose.orientation.z); + ignition::math::Pose3d target_pose(target_pos, target_rot); + ignition::math::Vector3d target_linear_vel(req.link_state.twist.linear.x, req.link_state.twist.linear.y, req.link_state.twist.linear.z); + ignition::math::Vector3d target_angular_vel(req.link_state.twist.angular.x, req.link_state.twist.angular.y, req.link_state.twist.angular.z); + + if (frame) + { +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d frame_pose = frame->WorldPose(); // - myBody->GetCoMPose(); + ignition::math::Vector3d frame_linear_vel = frame->WorldLinearVel(); + ignition::math::Vector3d frame_angular_vel = frame->WorldAngularVel(); +#else + ignition::math::Pose3d frame_pose = frame->GetWorldPose().Ign(); // - myBody->GetCoMPose(); + ignition::math::Vector3d frame_linear_vel = frame->GetWorldLinearVel().Ign(); + ignition::math::Vector3d frame_angular_vel = frame->GetWorldAngularVel().Ign(); +#endif + ignition::math::Vector3d frame_pos = frame_pose.Pos(); + ignition::math::Quaterniond frame_rot = frame_pose.Rot(); + + //std::cout << " debug : " << frame->GetName() << " : " << frame_pose << " : " << target_pose << std::endl; + target_pose = target_pose + frame_pose; + + target_linear_vel -= frame_linear_vel; + target_angular_vel -= frame_angular_vel; + } + else if (req.link_state.reference_frame == "" || req.link_state.reference_frame == "world" || req.link_state.reference_frame == "map" || req.link_state.reference_frame == "/map") + { + ROS_INFO_NAMED("api_plugin", "Updating LinkState: reference_frame is empty/world/map, using inertial frame"); + } + else + { + ROS_ERROR_NAMED("api_plugin", "Updating LinkState: reference_frame is not a valid entity name"); + res.success = false; + res.status_message = "SetLinkState: failed"; + return true; + } + + //std::cout << " debug : " << target_pose << std::endl; + //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex()); + + bool is_paused = world_->IsPaused(); + if (!is_paused) + world_->SetPaused(true); + body->SetWorldPose(target_pose); + world_->SetPaused(is_paused); + + // set body velocity to desired twist + body->SetLinearVel(target_linear_vel); + body->SetAngularVel(target_angular_vel); + + res.success = true; + res.status_message = "SetLinkState: success"; + return true; +} + +void GazeboRosApiPlugin::updateLinkState(const gazebo_msgs::LinkState::ConstPtr &link_state) +{ + gazebo_msgs::SetLinkState::Request req; + gazebo_msgs::SetLinkState::Response res; + req.link_state = *link_state; + /*bool success = */ setLinkState(req, res); +} + +void GazeboRosApiPlugin::transformWrench(ignition::math::Vector3d &target_force, ignition::math::Vector3d &target_torque, + const ignition::math::Vector3d &reference_force, + const ignition::math::Vector3d &reference_torque, + const ignition::math::Pose3d &target_to_reference) +{ + // rotate force into target frame + target_force = target_to_reference.Rot().RotateVector(reference_force); + // rotate torque into target frame + target_torque = target_to_reference.Rot().RotateVector(reference_torque); + + // target force is the refence force rotated by the target->reference transform + target_torque = target_torque + target_to_reference.Pos().Cross(target_force); +} + +bool GazeboRosApiPlugin::applyBodyWrench(gazebo_msgs::ApplyBodyWrench::Request &req, + gazebo_msgs::ApplyBodyWrench::Response &res) +{ +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->EntityByName(req.body_name)); + gazebo::physics::EntityPtr frame = world_->EntityByName(req.reference_frame); +#else + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(world_->GetEntity(req.body_name)); + gazebo::physics::EntityPtr frame = world_->GetEntity(req.reference_frame); +#endif + if (!body) + { + ROS_ERROR_NAMED("api_plugin", "ApplyBodyWrench: body [%s] does not exist", req.body_name.c_str()); + res.success = false; + res.status_message = "ApplyBodyWrench: body does not exist"; + return true; + } + + // target wrench + ignition::math::Vector3d reference_force(req.wrench.force.x, req.wrench.force.y, req.wrench.force.z); + ignition::math::Vector3d reference_torque(req.wrench.torque.x, req.wrench.torque.y, req.wrench.torque.z); + ignition::math::Vector3d reference_point(req.reference_point.x, req.reference_point.y, req.reference_point.z); + + ignition::math::Vector3d target_force; + ignition::math::Vector3d target_torque; + + /// shift wrench to body frame if a non-zero reference point is given + /// @todo: to be more general, should we make the reference point a reference pose? + reference_torque = reference_torque + reference_point.Cross(reference_force); + + /// @todo: FIXME map is really wrong, need to use tf here somehow + if (frame) + { + // get reference frame (body/model(body)) pose and + // transform target pose to absolute world frame + // @todo: need to modify wrench (target force and torque by frame) + // transform wrench from reference_point in reference_frame + // into the reference frame of the body + // first, translate by reference point to the body frame +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d framePose = frame->WorldPose(); +#else + ignition::math::Pose3d framePose = frame->GetWorldPose().Ign(); +#endif +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d bodyPose = body->WorldPose(); +#else + ignition::math::Pose3d bodyPose = body->GetWorldPose().Ign(); +#endif + ignition::math::Pose3d target_to_reference = framePose - bodyPose; + ROS_DEBUG_NAMED("api_plugin", "reference frame for applied wrench: [%f %f %f, %f %f %f]-[%f %f %f, %f %f %f]=[%f %f %f, %f %f %f]", + bodyPose.Pos().X(), + bodyPose.Pos().Y(), + bodyPose.Pos().Z(), + bodyPose.Rot().Euler().X(), + bodyPose.Rot().Euler().Y(), + bodyPose.Rot().Euler().Z(), + framePose.Pos().X(), + framePose.Pos().Y(), + framePose.Pos().Z(), + framePose.Rot().Euler().X(), + framePose.Rot().Euler().Y(), + framePose.Rot().Euler().Z(), + target_to_reference.Pos().X(), + target_to_reference.Pos().Y(), + target_to_reference.Pos().Z(), + target_to_reference.Rot().Euler().X(), + target_to_reference.Rot().Euler().Y(), + target_to_reference.Rot().Euler().Z()); + transformWrench(target_force, target_torque, reference_force, reference_torque, target_to_reference); + ROS_ERROR_NAMED("api_plugin", "wrench defined as [%s]:[%f %f %f, %f %f %f] --> applied as [%s]:[%f %f %f, %f %f %f]", + frame->GetName().c_str(), + reference_force.X(), + reference_force.Y(), + reference_force.Z(), + reference_torque.X(), + reference_torque.Y(), + reference_torque.Z(), + body->GetName().c_str(), + target_force.X(), + target_force.Y(), + target_force.Z(), + target_torque.X(), + target_torque.Y(), + target_torque.Z()); + } + else if (req.reference_frame == "" || req.reference_frame == "world" || req.reference_frame == "map" || req.reference_frame == "/map") + { + ROS_INFO_NAMED("api_plugin", "ApplyBodyWrench: reference_frame is empty/world/map, using inertial frame, transferring from body relative to inertial frame"); + // FIXME: transfer to inertial frame +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d target_to_reference = body->WorldPose(); +#else + ignition::math::Pose3d target_to_reference = body->GetWorldPose().Ign(); +#endif + target_force = reference_force; + target_torque = reference_torque; + } + else + { + ROS_ERROR_NAMED("api_plugin", "ApplyBodyWrench: reference_frame is not a valid entity name"); + res.success = false; + res.status_message = "ApplyBodyWrench: reference_frame not found"; + return true; + } + + // apply wrench + // schedule a job to do below at appropriate times: + // body->SetForce(force) + // body->SetTorque(torque) + GazeboRosApiPlugin::WrenchBodyJob *wej = new GazeboRosApiPlugin::WrenchBodyJob; + wej->body = body; + wej->force = target_force; + wej->torque = target_torque; + wej->start_time = req.start_time; +#if GAZEBO_MAJOR_VERSION >= 8 + if (wej->start_time < ros::Time(world_->SimTime().Double())) + wej->start_time = ros::Time(world_->SimTime().Double()); +#else + if (wej->start_time < ros::Time(world_->GetSimTime().Double())) + wej->start_time = ros::Time(world_->GetSimTime().Double()); +#endif + wej->duration = req.duration; + lock_.lock(); + wrench_body_jobs_.push_back(wej); + lock_.unlock(); + + res.success = true; + res.status_message = ""; + return true; +} + +bool GazeboRosApiPlugin::isURDF(std::string model_xml) +{ + TiXmlDocument doc_in; + doc_in.Parse(model_xml.c_str()); + if (doc_in.FirstChild("robot")) + return true; + else + return false; +} + +bool GazeboRosApiPlugin::isSDF(std::string model_xml) +{ + // FIXME: very crude check + TiXmlDocument doc_in; + doc_in.Parse(model_xml.c_str()); + if (doc_in.FirstChild("gazebo") || + doc_in.FirstChild("sdf")) // sdf + return true; + else + return false; +} + +void GazeboRosApiPlugin::wrenchBodySchedulerSlot() +{ + // MDMutex locks in case model is getting deleted, don't have to do this if we delete jobs first + // boost::recursive_mutex::scoped_lock lock(*world->GetMDMutex()); + lock_.lock(); + for (std::vector::iterator iter = wrench_body_jobs_.begin(); iter != wrench_body_jobs_.end();) + { + // check times and apply wrench if necessary +#if GAZEBO_MAJOR_VERSION >= 8 + ros::Time simTime = ros::Time(world_->SimTime().Double()); +#else + ros::Time simTime = ros::Time(world_->GetSimTime().Double()); +#endif + if (simTime >= (*iter)->start_time) + if (simTime <= (*iter)->start_time + (*iter)->duration || + (*iter)->duration.toSec() < 0.0) + { + if ((*iter)->body) // if body exists + { + (*iter)->body->SetForce((*iter)->force); + (*iter)->body->SetTorque((*iter)->torque); + } + else + (*iter)->duration.fromSec(0.0); // mark for delete + } + + if (simTime > (*iter)->start_time + (*iter)->duration && + (*iter)->duration.toSec() >= 0.0) + { + // remove from queue once expires + delete (*iter); + iter = wrench_body_jobs_.erase(iter); + } + else + ++iter; + } + lock_.unlock(); +} + +void GazeboRosApiPlugin::forceJointSchedulerSlot() +{ + // MDMutex locks in case model is getting deleted, don't have to do this if we delete jobs first + // boost::recursive_mutex::scoped_lock lock(*world->GetMDMutex()); + lock_.lock(); + for (std::vector::iterator iter = force_joint_jobs_.begin(); iter != force_joint_jobs_.end();) + { + // check times and apply force if necessary +#if GAZEBO_MAJOR_VERSION >= 8 + ros::Time simTime = ros::Time(world_->SimTime().Double()); +#else + ros::Time simTime = ros::Time(world_->GetSimTime().Double()); +#endif + if (simTime >= (*iter)->start_time) + if (simTime <= (*iter)->start_time + (*iter)->duration || + (*iter)->duration.toSec() < 0.0) + { + if ((*iter)->joint) // if joint exists + (*iter)->joint->SetForce(0, (*iter)->force); + else + (*iter)->duration.fromSec(0.0); // mark for delete + } + + if (simTime > (*iter)->start_time + (*iter)->duration && + (*iter)->duration.toSec() >= 0.0) + { + // remove from queue once expires + iter = force_joint_jobs_.erase(iter); + } + else + ++iter; + } + lock_.unlock(); +} + +void GazeboRosApiPlugin::publishSimTime(const boost::shared_ptr &msg) +{ + ROS_ERROR_NAMED("api_plugin", "CLOCK2"); +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::common::Time sim_time = world_->SimTime(); +#else + gazebo::common::Time sim_time = world_->GetSimTime(); +#endif + if (pub_clock_frequency_ > 0 && (sim_time - last_pub_clock_time_).Double() < 1.0 / pub_clock_frequency_) + return; + + gazebo::common::Time currentTime = gazebo::msgs::Convert(msg->sim_time()); + rosgraph_msgs::Clock ros_time_; + ros_time_.clock.fromSec(currentTime.Double()); + // publish time to ros + last_pub_clock_time_ = sim_time; + pub_clock_.publish(ros_time_); +} +void GazeboRosApiPlugin::publishSimTime() +{ +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::common::Time sim_time = world_->SimTime(); +#else + gazebo::common::Time sim_time = world_->GetSimTime(); +#endif + if (pub_clock_frequency_ > 0 && (sim_time - last_pub_clock_time_).Double() < 1.0 / pub_clock_frequency_) + return; + +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::common::Time currentTime = world_->SimTime(); +#else + gazebo::common::Time currentTime = world_->GetSimTime(); +#endif + rosgraph_msgs::Clock ros_time_; + ros_time_.clock.fromSec(currentTime.Double()); + // publish time to ros + last_pub_clock_time_ = sim_time; + pub_clock_.publish(ros_time_); +} + +void GazeboRosApiPlugin::publishLinkStates() +{ + gazebo_msgs::LinkStates link_states; + + // fill link_states +#if GAZEBO_MAJOR_VERSION >= 8 + for (unsigned int i = 0; i < world_->ModelCount(); i++) + { + gazebo::physics::ModelPtr model = world_->ModelByIndex(i); +#else + for (unsigned int i = 0; i < world_->GetModelCount(); i++) + { + gazebo::physics::ModelPtr model = world_->GetModel(i); +#endif + + for (unsigned int j = 0; j < model->GetChildCount(); j++) + { + gazebo::physics::LinkPtr body = boost::dynamic_pointer_cast(model->GetChild(j)); + + if (body) + { + link_states.name.push_back(body->GetScopedName()); + geometry_msgs::Pose pose; +#if GAZEBO_MAJOR_VERSION >= 8 + ignition::math::Pose3d body_pose = body->WorldPose(); // - myBody->GetCoMPose(); + ignition::math::Vector3d linear_vel = body->WorldLinearVel(); + ignition::math::Vector3d angular_vel = body->WorldAngularVel(); +#else + ignition::math::Pose3d body_pose = body->GetWorldPose().Ign(); // - myBody->GetCoMPose(); + ignition::math::Vector3d linear_vel = body->GetWorldLinearVel().Ign(); + ignition::math::Vector3d angular_vel = body->GetWorldAngularVel().Ign(); +#endif + ignition::math::Vector3d pos = body_pose.Pos(); + ignition::math::Quaterniond rot = body_pose.Rot(); + pose.position.x = pos.X(); + pose.position.y = pos.Y(); + pose.position.z = pos.Z(); + pose.orientation.w = rot.W(); + pose.orientation.x = rot.X(); + pose.orientation.y = rot.Y(); + pose.orientation.z = rot.Z(); + link_states.pose.push_back(pose); + geometry_msgs::Twist twist; + twist.linear.x = linear_vel.X(); + twist.linear.y = linear_vel.Y(); + twist.linear.z = linear_vel.Z(); + twist.angular.x = angular_vel.X(); + twist.angular.y = angular_vel.Y(); + twist.angular.z = angular_vel.Z(); + link_states.twist.push_back(twist); + } + } + } + + pub_link_states_.publish(link_states); +} + +void GazeboRosApiPlugin::publishModelStates() +{ + gazebo_msgs::ModelStates model_states; + + // fill model_states +#if GAZEBO_MAJOR_VERSION >= 8 + for (unsigned int i = 0; i < world_->ModelCount(); i++) + { + gazebo::physics::ModelPtr model = world_->ModelByIndex(i); + ignition::math::Pose3d model_pose = model->WorldPose(); // - myBody->GetCoMPose(); + ignition::math::Vector3d linear_vel = model->WorldLinearVel(); + ignition::math::Vector3d angular_vel = model->WorldAngularVel(); +#else + for (unsigned int i = 0; i < world_->GetModelCount(); i++) + { + gazebo::physics::ModelPtr model = world_->GetModel(i); + ignition::math::Pose3d model_pose = model->GetWorldPose().Ign(); // - myBody->GetCoMPose(); + ignition::math::Vector3d linear_vel = model->GetWorldLinearVel().Ign(); + ignition::math::Vector3d angular_vel = model->GetWorldAngularVel().Ign(); +#endif + ignition::math::Vector3d pos = model_pose.Pos(); + ignition::math::Quaterniond rot = model_pose.Rot(); + geometry_msgs::Pose pose; + pose.position.x = pos.X(); + pose.position.y = pos.Y(); + pose.position.z = pos.Z(); + pose.orientation.w = rot.W(); + pose.orientation.x = rot.X(); + pose.orientation.y = rot.Y(); + pose.orientation.z = rot.Z(); + model_states.pose.push_back(pose); + model_states.name.push_back(model->GetName()); + geometry_msgs::Twist twist; + twist.linear.x = linear_vel.X(); + twist.linear.y = linear_vel.Y(); + twist.linear.z = linear_vel.Z(); + twist.angular.x = angular_vel.X(); + twist.angular.y = angular_vel.Y(); + twist.angular.z = angular_vel.Z(); + model_states.twist.push_back(twist); + } + pub_model_states_.publish(model_states); +} + +void GazeboRosApiPlugin::physicsReconfigureCallback(gazebo_ros::PhysicsConfig &config, uint32_t level) +{ + if (!physics_reconfigure_initialized_) + { + gazebo_msgs::GetPhysicsProperties srv; + physics_reconfigure_get_client_.call(srv); + + config.time_step = srv.response.time_step; + config.max_update_rate = srv.response.max_update_rate; + config.gravity_x = srv.response.gravity.x; + config.gravity_y = srv.response.gravity.y; + config.gravity_z = srv.response.gravity.z; + config.auto_disable_bodies = srv.response.ode_config.auto_disable_bodies; + config.sor_pgs_precon_iters = srv.response.ode_config.sor_pgs_precon_iters; + config.sor_pgs_iters = srv.response.ode_config.sor_pgs_iters; + config.sor_pgs_rms_error_tol = srv.response.ode_config.sor_pgs_rms_error_tol; + config.sor_pgs_w = srv.response.ode_config.sor_pgs_w; + config.contact_surface_layer = srv.response.ode_config.contact_surface_layer; + config.contact_max_correcting_vel = srv.response.ode_config.contact_max_correcting_vel; + config.cfm = srv.response.ode_config.cfm; + config.erp = srv.response.ode_config.erp; + config.max_contacts = srv.response.ode_config.max_contacts; + physics_reconfigure_initialized_ = true; + } + else + { + bool changed = false; + gazebo_msgs::GetPhysicsProperties srv; + physics_reconfigure_get_client_.call(srv); + + // check for changes + if (config.time_step != srv.response.time_step) + changed = true; + if (config.max_update_rate != srv.response.max_update_rate) + changed = true; + if (config.gravity_x != srv.response.gravity.x) + changed = true; + if (config.gravity_y != srv.response.gravity.y) + changed = true; + if (config.gravity_z != srv.response.gravity.z) + changed = true; + if (config.auto_disable_bodies != srv.response.ode_config.auto_disable_bodies) + changed = true; + if ((uint32_t)config.sor_pgs_precon_iters != srv.response.ode_config.sor_pgs_precon_iters) + changed = true; + if ((uint32_t)config.sor_pgs_iters != srv.response.ode_config.sor_pgs_iters) + changed = true; + if (config.sor_pgs_rms_error_tol != srv.response.ode_config.sor_pgs_rms_error_tol) + changed = true; + if (config.sor_pgs_w != srv.response.ode_config.sor_pgs_w) + changed = true; + if (config.contact_surface_layer != srv.response.ode_config.contact_surface_layer) + changed = true; + if (config.contact_max_correcting_vel != srv.response.ode_config.contact_max_correcting_vel) + changed = true; + if (config.cfm != srv.response.ode_config.cfm) + changed = true; + if (config.erp != srv.response.ode_config.erp) + changed = true; + if ((uint32_t)config.max_contacts != srv.response.ode_config.max_contacts) + changed = true; + + if (changed) + { + // pause simulation if requested + gazebo_msgs::SetPhysicsProperties srv; + srv.request.time_step = config.time_step; + srv.request.max_update_rate = config.max_update_rate; + srv.request.gravity.x = config.gravity_x; + srv.request.gravity.y = config.gravity_y; + srv.request.gravity.z = config.gravity_z; + srv.request.ode_config.auto_disable_bodies = config.auto_disable_bodies; + srv.request.ode_config.sor_pgs_precon_iters = config.sor_pgs_precon_iters; + srv.request.ode_config.sor_pgs_iters = config.sor_pgs_iters; + srv.request.ode_config.sor_pgs_rms_error_tol = config.sor_pgs_rms_error_tol; + srv.request.ode_config.sor_pgs_w = config.sor_pgs_w; + srv.request.ode_config.contact_surface_layer = config.contact_surface_layer; + srv.request.ode_config.contact_max_correcting_vel = config.contact_max_correcting_vel; + srv.request.ode_config.cfm = config.cfm; + srv.request.ode_config.erp = config.erp; + srv.request.ode_config.max_contacts = config.max_contacts; + physics_reconfigure_set_client_.call(srv); + ROS_INFO_NAMED("api_plugin", "physics dynamics reconfigure update complete"); + } + ROS_INFO_NAMED("api_plugin", "physics dynamics reconfigure complete"); + } +} + +void GazeboRosApiPlugin::physicsReconfigureThread() +{ + physics_reconfigure_set_client_ = nh_->serviceClient("/gazebo/set_physics_properties"); + physics_reconfigure_get_client_ = nh_->serviceClient("/gazebo/get_physics_properties"); + + // Wait until the rest of this plugin is loaded and the services are being offered + physics_reconfigure_set_client_.waitForExistence(); + physics_reconfigure_get_client_.waitForExistence(); + + physics_reconfigure_srv_.reset(new dynamic_reconfigure::Server()); + + physics_reconfigure_callback_ = boost::bind(&GazeboRosApiPlugin::physicsReconfigureCallback, this, _1, _2); + physics_reconfigure_srv_->setCallback(physics_reconfigure_callback_); + + ROS_INFO_NAMED("api_plugin", "Physics dynamic reconfigure ready."); +} + +void GazeboRosApiPlugin::stripXmlDeclaration(std::string &model_xml) +{ + // incoming robot model string is a string containing a Gazebo Model XML + /// STRIP DECLARATION from model_xml + /// @todo: does tinyxml have functionality for this? + /// @todo: should gazebo take care of the declaration? + std::string open_bracket(""); + size_t pos1 = model_xml.find(open_bracket, 0); + size_t pos2 = model_xml.find(close_bracket, 0); + if (pos1 != std::string::npos && pos2 != std::string::npos) + model_xml.replace(pos1, pos2 - pos1 + 2, std::string("")); +} + +void GazeboRosApiPlugin::updateSDFAttributes(TiXmlDocument &gazebo_model_xml, + const std::string &model_name, + const ignition::math::Vector3d &initial_xyz, + const ignition::math::Quaterniond &initial_q) +{ + // This function can handle both regular SDF files and SDFs that are used with the + // Gazebo Model Database + + TiXmlElement *pose_element; // This is used by both reguar and database SDFs + + // Check SDF for requires SDF element + TiXmlElement *gazebo_tixml = gazebo_model_xml.FirstChildElement("sdf"); + if (!gazebo_tixml) + { + ROS_WARN_NAMED("api_plugin", "Could not find element in sdf, so name and initial position cannot be applied"); + return; + } + + // Check SDF for optional model element. May not have one + TiXmlElement *model_tixml = gazebo_tixml->FirstChildElement("model"); + if (model_tixml) + { + // Update entity name + if (model_tixml->Attribute("name") != NULL) + { + // removing old entity name + model_tixml->RemoveAttribute("name"); + } + // replace with user specified name + model_tixml->SetAttribute("name", model_name); + } + else + { + // Check SDF for world element + TiXmlElement *world_tixml = gazebo_tixml->FirstChildElement("world"); + if (!world_tixml) + { + ROS_WARN_NAMED("api_plugin", "Could not find or element in sdf, so name and initial position cannot be applied"); + return; + } + // If not element, check SDF for required include element + model_tixml = world_tixml->FirstChildElement("include"); + if (!model_tixml) + { + ROS_WARN_NAMED("api_plugin", "Could not find element in sdf, so name and initial position cannot be applied"); + return; + } + + // Check for name element + TiXmlElement *name_tixml = model_tixml->FirstChildElement("name"); + if (!name_tixml) + { + // Create the name element + name_tixml = new TiXmlElement("name"); + model_tixml->LinkEndChild(name_tixml); + } + + // Set the text within the name element + TiXmlText *text = new TiXmlText(model_name); + name_tixml->LinkEndChild(text); + } + + // Check for the pose element + pose_element = model_tixml->FirstChildElement("pose"); + ignition::math::Pose3d model_pose; + + // Create the pose element if it doesn't exist + // Remove it if it exists, since we are inserting a new one + if (pose_element) + { + // save pose_element in ignition::math::Pose3d and remove child + model_pose = this->parsePose(pose_element->GetText()); + model_tixml->RemoveChild(pose_element); + } + + // Set and link the pose element after adding initial pose + { + // add pose_element Pose to initial pose + ignition::math::Pose3d new_model_pose = model_pose + ignition::math::Pose3d(initial_xyz, initial_q); + + // Create the string of 6 numbers + std::ostringstream pose_stream; + ignition::math::Vector3d model_rpy = new_model_pose.Rot().Euler(); // convert to Euler angles for Gazebo XML + pose_stream << new_model_pose.Pos().X() << " " << new_model_pose.Pos().Y() << " " << new_model_pose.Pos().Z() << " " + << model_rpy.X() << " " << model_rpy.Y() << " " << model_rpy.Z(); + + // Add value to pose element + TiXmlText *text = new TiXmlText(pose_stream.str()); + TiXmlElement *new_pose_element = new TiXmlElement("pose"); + new_pose_element->LinkEndChild(text); + model_tixml->LinkEndChild(new_pose_element); + } +} + +ignition::math::Pose3d GazeboRosApiPlugin::parsePose(const std::string &str) +{ + std::vector pieces; + std::vector vals; + + boost::split(pieces, str, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i) + { + if (pieces[i] != "") + { + try + { + vals.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) + { + sdferr << "xml key [" << str + << "][" << i << "] value [" << pieces[i] + << "] is not a valid double from a 3-tuple\n"; + return ignition::math::Pose3d(); + } + } + } + + if (vals.size() == 6) + return ignition::math::Pose3d(vals[0], vals[1], vals[2], vals[3], vals[4], vals[5]); + else + { + ROS_ERROR_NAMED("api_plugin", "Beware: failed to parse string [%s] as ignition::math::Pose3d, returning zeros.", str.c_str()); + return ignition::math::Pose3d(); + } +} + +ignition::math::Vector3d GazeboRosApiPlugin::parseVector3(const std::string &str) +{ + std::vector pieces; + std::vector vals; + + boost::split(pieces, str, boost::is_any_of(" ")); + for (unsigned int i = 0; i < pieces.size(); ++i) + { + if (pieces[i] != "") + { + try + { + vals.push_back(boost::lexical_cast(pieces[i].c_str())); + } + catch (boost::bad_lexical_cast &e) + { + sdferr << "xml key [" << str + << "][" << i << "] value [" << pieces[i] + << "] is not a valid double from a 3-tuple\n"; + return ignition::math::Vector3d(); + } + } + } + + if (vals.size() == 3) + return ignition::math::Vector3d(vals[0], vals[1], vals[2]); + else + { + ROS_ERROR_NAMED("api_plugin", "Beware: failed to parse string [%s] as ignition::math::Vector3d, returning zeros.", str.c_str()); + return ignition::math::Vector3d(); + } +} + +void GazeboRosApiPlugin::updateURDFModelPose(TiXmlDocument &gazebo_model_xml, + const ignition::math::Vector3d &initial_xyz, + const ignition::math::Quaterniond &initial_q) +{ + TiXmlElement *model_tixml = (gazebo_model_xml.FirstChildElement("robot")); + if (model_tixml) + { + // replace initial pose of model + // find first instance of xyz and rpy, replace with initial pose + TiXmlElement *origin_key = model_tixml->FirstChildElement("origin"); + + if (!origin_key) + { + origin_key = new TiXmlElement("origin"); + model_tixml->LinkEndChild(origin_key); + } + + ignition::math::Vector3d xyz; + ignition::math::Vector3d rpy; + if (origin_key->Attribute("xyz")) + { + xyz = this->parseVector3(origin_key->Attribute("xyz")); + origin_key->RemoveAttribute("xyz"); + } + if (origin_key->Attribute("rpy")) + { + rpy = this->parseVector3(origin_key->Attribute("rpy")); + origin_key->RemoveAttribute("rpy"); + } + + // add xyz, rpy to initial pose + ignition::math::Pose3d model_pose = ignition::math::Pose3d(xyz, ignition::math::Quaterniond(rpy)) + ignition::math::Pose3d(initial_xyz, initial_q); + + std::ostringstream xyz_stream; + xyz_stream << model_pose.Pos().X() << " " << model_pose.Pos().Y() << " " << model_pose.Pos().Z(); + + std::ostringstream rpy_stream; + ignition::math::Vector3d model_rpy = model_pose.Rot().Euler(); // convert to Euler angles for Gazebo XML + rpy_stream << model_rpy.X() << " " << model_rpy.Y() << " " << model_rpy.Z(); + + origin_key->SetAttribute("xyz", xyz_stream.str()); + origin_key->SetAttribute("rpy", rpy_stream.str()); + } + else + ROS_WARN_NAMED("api_plugin", "Could not find element in sdf, so name and initial position is not applied"); +} + +void GazeboRosApiPlugin::updateURDFName(TiXmlDocument &gazebo_model_xml, const std::string &model_name) +{ + TiXmlElement *model_tixml = gazebo_model_xml.FirstChildElement("robot"); + // replace model name if one is specified by the user + if (model_tixml) + { + if (model_tixml->Attribute("name") != NULL) + { + // removing old model name + model_tixml->RemoveAttribute("name"); + } + // replace with user specified name + model_tixml->SetAttribute("name", model_name); + } + else + ROS_WARN_NAMED("api_plugin", "Could not find element in URDF, name not replaced"); +} + +void GazeboRosApiPlugin::walkChildAddRobotNamespace(TiXmlNode *model_xml) +{ + TiXmlNode *child = 0; + child = model_xml->IterateChildren(child); + while (child != NULL) + { + if (child->Type() == TiXmlNode::TINYXML_ELEMENT && + child->ValueStr().compare(std::string("plugin")) == 0) + { + if (child->FirstChildElement("robotNamespace") == NULL) + { + TiXmlElement *child_elem = child->ToElement()->FirstChildElement("robotNamespace"); + while (child_elem) + { + child->ToElement()->RemoveChild(child_elem); + child_elem = child->ToElement()->FirstChildElement("robotNamespace"); + } + TiXmlElement *key = new TiXmlElement("robotNamespace"); + TiXmlText *val = new TiXmlText(robot_namespace_); + key->LinkEndChild(val); + child->ToElement()->LinkEndChild(key); + } + } + walkChildAddRobotNamespace(child); + child = model_xml->IterateChildren(child); + } +} + +bool GazeboRosApiPlugin::spawnAndConform(TiXmlDocument &gazebo_model_xml, const std::string &model_name, + gazebo_msgs::SpawnModel::Response &res) +{ + std::string entity_type = gazebo_model_xml.RootElement()->FirstChild()->Value(); + // Convert the entity type to lower case + std::transform(entity_type.begin(), entity_type.end(), entity_type.begin(), ::tolower); + + bool isLight = (entity_type == "light"); + + // push to factory iface + std::ostringstream stream; + stream << gazebo_model_xml; + std::string gazebo_model_xml_string = stream.str(); + ROS_DEBUG_NAMED("api_plugin.xml", "Gazebo Model XML\n\n%s\n\n ", gazebo_model_xml_string.c_str()); + + // publish to factory topic + gazebo::msgs::Factory msg; + gazebo::msgs::Init(msg, "spawn_model"); + msg.set_sdf(gazebo_model_xml_string); + + //ROS_ERROR_NAMED("api_plugin", "attempting to spawn model name [%s] [%s]", model_name.c_str(),gazebo_model_xml_string.c_str()); + + // FIXME: should use entity_info or add lock to World::receiveMutex + // looking for Model to see if it exists already + gazebo::msgs::Request *entity_info_msg = gazebo::msgs::CreateRequest("entity_info", model_name); + request_pub_->Publish(*entity_info_msg, true); + // todo: should wait for response response_sub_, check to see that if _msg->response == "nonexistant" + +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::ModelPtr model = world_->ModelByName(model_name); + gazebo::physics::LightPtr light = world_->LightByName(model_name); +#else + gazebo::physics::ModelPtr model = world_->GetModel(model_name); + gazebo::physics::LightPtr light = world_->Light(model_name); +#endif + if ((isLight && light != NULL) || (model != NULL)) + { + ROS_ERROR_NAMED("api_plugin", "SpawnModel: Failure - model name %s already exist.", model_name.c_str()); + res.success = false; + res.status_message = "SpawnModel: Failure - entity already exists."; + return true; + } + + // for Gazebo 7 and up, use a different method to spawn lights + if (isLight) + { + // Publish the light message to spawn the light (Gazebo 7 and up) + sdf::SDF sdf_light; + sdf_light.SetFromString(gazebo_model_xml_string); + gazebo::msgs::Light msg = gazebo::msgs::LightFromSDF(sdf_light.Root()->GetElement("light")); + msg.set_name(model_name); + factory_light_pub_->Publish(msg); + } + else + { + // Publish the factory message + factory_pub_->Publish(msg); + } + /// FIXME: should change publish to direct invocation World::LoadModel() and/or + /// change the poll for Model existence to common::Events based check. + + /// \brief poll and wait, verify that the model is spawned within Hardcoded 10 seconds + ros::Duration model_spawn_timeout(10.0); + ros::Time timeout = ros::Time::now() + model_spawn_timeout; + + while (ros::ok()) + { + if (ros::Time::now() > timeout) + { + res.success = false; + res.status_message = "SpawnModel: Entity pushed to spawn queue, but spawn service timed out waiting for entity to appear in simulation under the name " + model_name; + return true; + } + + { + //boost::recursive_mutex::scoped_lock lock(*world->GetMRMutex()); +#if GAZEBO_MAJOR_VERSION >= 8 + if ((isLight && world_->LightByName(model_name) != NULL) || (world_->ModelByName(model_name) != NULL)) +#else + if ((isLight && world_->Light(model_name) != NULL) || (world_->GetModel(model_name) != NULL)) +#endif + break; + } + + ROS_DEBUG_STREAM_ONCE_NAMED("api_plugin", "Waiting for " << timeout - ros::Time::now() + << " for entity " << model_name << " to spawn"); + + usleep(2000); + } + + // set result + res.success = true; + res.status_message = "SpawnModel: Successfully spawned entity"; + return true; +} + +// Register this plugin with the simulator +GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosApiPlugin) +} // namespace gazebo diff --git a/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_paths_plugin.cpp b/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_paths_plugin.cpp new file mode 100755 index 0000000..74e3fbe --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_paths_plugin.cpp @@ -0,0 +1,102 @@ +/* + * 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. + * +*/ +/* + * Author: John Hsu, Nate Koenig, Dave Coleman + * Desc: External interfaces for Gazebo + */ + +#include +#include + +#include +#include + +#include + +namespace gazebo +{ + +typedef std::vector V_string; +typedef std::map M_string; + +class GazeboRosPathsPlugin : public SystemPlugin +{ +public: + GazeboRosPathsPlugin() + { + this->LoadPaths(); + } + + ~GazeboRosPathsPlugin() + { + }; + + void Init() + { + } + + void Load(int argc, char** argv) + { + } + + /** + * @brief Set Gazebo Path/Resources Configurations GAZEBO_MODEL_PATH, PLUGIN_PATH and + GAZEBO_MEDIA_PATH by adding paths to GazeboConfig based on ros::package + */ + void LoadPaths() + { + // set gazebo media paths by adding all packages that exports "gazebo_media_path" for gazebo + gazebo::common::SystemPaths::Instance()->gazeboPathsFromEnv = false; + std::vector gazebo_media_paths; + ros::package::getPlugins("gazebo_ros","gazebo_media_path",gazebo_media_paths); + for (std::vector::iterator iter=gazebo_media_paths.begin(); iter != gazebo_media_paths.end(); iter++) + { + ROS_DEBUG_NAMED("paths_plugin", "Media path %s",iter->c_str()); + gazebo::common::SystemPaths::Instance()->AddGazeboPaths(iter->c_str()); + } + + // set gazebo plugins paths by adding all packages that exports "plugin_path" for gazebo + gazebo::common::SystemPaths::Instance()->pluginPathsFromEnv = false; + std::vector plugin_paths; + ros::package::getPlugins("gazebo_ros","plugin_path",plugin_paths); + for (std::vector::iterator iter=plugin_paths.begin(); iter != plugin_paths.end(); iter++) + { + ROS_DEBUG_NAMED("paths_plugin", "plugin path %s",(*iter).c_str()); + gazebo::common::SystemPaths::Instance()->AddPluginPaths(iter->c_str()); + } + + // set model paths by adding all packages that exports "gazebo_model_path" for gazebo + gazebo::common::SystemPaths::Instance()->modelPathsFromEnv = false; + std::vector model_paths; + ros::package::getPlugins("gazebo_ros","gazebo_model_path",model_paths); + for (std::vector::iterator iter=model_paths.begin(); iter != model_paths.end(); iter++) + { + ROS_DEBUG_NAMED("paths_plugin", "Model path %s",(*iter).c_str()); + gazebo::common::SystemPaths::Instance()->AddModelPaths(iter->c_str()); + } + + // set .gazeborc path to something else, so we don't pick up default ~/.gazeborc + std::string gazeborc = ros::package::getPath("gazebo_ros")+"/.do_not_use_gazeborc"; + setenv("GAZEBORC",gazeborc.c_str(),1); + } + +}; + +// Register this plugin with the simulator +GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosPathsPlugin) + +} diff --git a/gazebo_ros_pkgs/gazebo_ros_control/CHANGELOG.rst b/gazebo_ros_pkgs/gazebo_ros_control/CHANGELOG.rst new file mode 100755 index 0000000..826fe5e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_control/CHANGELOG.rst @@ -0,0 +1,365 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gazebo_ros_control +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.5.15 (2018-02-12) +------------------- +* Fix last gazebo8 warnings! (`#658 `_) +* Fix gazebo8 warnings part 7: ifdef's for Joint::GetAngle and some cleanup (`#642 `_) +* Contributors: Steven Peters + +2.5.14 (2017-12-11) +------------------- +* Replace Events::Disconnect* with pointer reset (`#623 `_) +* Contributors: Steven Peters + +2.5.13 (2017-06-24) +------------------- +* Less exciting console output (`#561 `_) +* Add catkin package(s) to provide the default version of Gazebo (`#571 `_) + * Added catkin package gazebo_dev which provides the cmake config of the installed Gazebo version +* Contributors: Dave Coleman, Jose Luis Rivero + +2.5.12 (2017-04-25) +------------------- +* Fixed broken gazebo_ros_control tutorial link (`#566 `_) +* Contributors: Ian McMahon + +2.5.11 (2017-04-18) +------------------- +* Change build system to set DEPEND on Gazebo/SDFormat (fix catkin warning) + Added missing DEPEND clauses to catkin_package to fix gazebo catkin warning. Note that after the change problems could appear related to -lpthreads errors. This is an known issue related to catkin: https://github.com/ros/catkin/issues/856. +* Make gazebo_ros_control compatible with ros_control with respect to tag (`#550 `_) + * ros_control expects "hardware_interface/PositionJointInterface", i.e. "hardware_interface/" prefix + * add deprecation warning + * improve warning + * fix warning message fix +* Contributors: Andreas Bihlmaier, Dave Coleman + +2.5.10 (2017-03-03) +------------------- +* Revert catkin warnings to fix regressions (problems with catkin -lpthreads errors) + For reference and reasons, please check: + https://discourse.ros.org/t/need-to-sync-new-release-of-rqt-topic-indigo-jade-kinetic/1410/4 + * Revert "Fix gazebo catkin warning, cleanup CMakeLists (`#537 `_)" + This reverts commit 5a0305fcb97864b66bc2e587fc0564435b4f2034. + * Revert "Fix gazebo and sdformat catkin warnings" + This reverts commit 11f95d25dcd32faccd2401d45c722f7794c7542c. +* Contributors: Jose Luis Rivero + +2.5.9 (2017-02-20) +------------------ +* Fix gazebo catkin warning, cleanup CMakeLists (`#537 `_) +* Namespace console output (`#543 `_) +* Print name of joint with wrong interface +* Removed all trailing whitespace +* Change boost::shared_ptr to urdf::JointConstSharedPtr +* Contributors: Bence Magyar, Dave Coleman, Jochen Sprickerhof + +2.5.8 (2016-12-06) +------------------ + +2.5.7 (2016-06-10) +------------------ +* delete CATKIN_IGNORE in gazebo_ros_control (`#456 `_) +* Contributors: Jackie Kay, Jose Luis Rivero + +2.5.3 (2016-04-11) +------------------ + +2.5.2 (2016-02-25) +------------------ +* clean up merge from indigo-devel +* merging from indigo-devel +* Use Joint::SetParam for joint velocity motors + Before gazebo5, Joint::SetVelocity and SetMaxForce + were used to set joint velocity motors. + The API has changed in gazebo5, to use Joint::SetParam + instead. + The functionality is still available through the SetParam API. + cherry-picked from indigo-devel + Add ifdefs to fix build with gazebo2 + It was broken by `#315 `_. + Fixes `#321 `_. +* 2.4.9 +* Generate changelog +* Import changes from jade-branch +* add missing dependencies +* Fix DefaultRobotHWSim puts robotNamespace twice + DefaultRobotHWSim::initSim() member function uses both + namespaced NodeHandle and robot_namespace string to create + parameter names. + For example, if a robotNamespace is "rrbot", + DefaultRobotHWSim tries to get parameters from following names: + - /rrbot/rrbot/gazebo_ros_control/pid_gains/* + - /rrbot/rrbot/joint_limits/* + This commit change these names to: + - /rrbot/gazebo_ros_control/pid_gains/* + - /rrbot/joint_limits/* +* Add ifdefs to fix build with gazebo2 + It was broken by `#315 `_. + Fixes `#321 `_. +* Use Joint::SetParam for joint velocity motors + Before gazebo5, Joint::SetVelocity and SetMaxForce + were used to set joint velocity motors. + The API has changed in gazebo5, to use Joint::SetParam + instead. + The functionality is still available through the SetParam API. +* Set GAZEBO_CXX_FLAGS to fix c++11 compilation errors +* Contributors: Akiyoshi Ochiai, John Hsu, Jose Luis Rivero, Steven Peters, ipa-fxm + +2.5.1 (2015-08-16) +------------------ +* Fix DefaultRobotHWSim puts robotNamespace twice + DefaultRobotHWSim::initSim() member function uses both + namespaced NodeHandle and robot_namespace string to create + parameter names. + For example, if a robotNamespace is "rrbot", + DefaultRobotHWSim tries to get parameters from following names: + - /rrbot/rrbot/gazebo_ros_control/pid_gains/* + - /rrbot/rrbot/joint_limits/* + This commit change these names to: + - /rrbot/gazebo_ros_control/pid_gains/* + - /rrbot/joint_limits/* +* Added a comment about the need of libgazebo5-dev in runtime +* Added elevator plugin +* Use c++11 +* run_depend on libgazebo5-dev (`#323 `_) + Declare the dependency. + It can be fixed later if we don't want it. +* Contributors: Akiyoshi Ochiai, Jose Luis Rivero, Nate Koenig, Steven Peters + +* Fix DefaultRobotHWSim puts robotNamespace twice + DefaultRobotHWSim::initSim() member function uses both + namespaced NodeHandle and robot_namespace string to create + parameter names. + For example, if a robotNamespace is "rrbot", + DefaultRobotHWSim tries to get parameters from following names: + - /rrbot/rrbot/gazebo_ros_control/pid_gains/* + - /rrbot/rrbot/joint_limits/* + This commit change these names to: + - /rrbot/gazebo_ros_control/pid_gains/* + - /rrbot/joint_limits/* +* Added a comment about the need of libgazebo5-dev in runtime +* Added elevator plugin +* Use c++11 +* run_depend on libgazebo5-dev +* Contributors: Akiyoshi Ochiai, Jose Luis Rivero, Nate Koenig, Steven Peters + +2.5.0 (2015-04-30) +------------------ +* run_depend on libgazebo5-dev instead of gazebo5 +* Changed the rosdep key for gazebo to gazebo5, for Jade Gazebo5 will be used. +* Contributors: Steven Peters, William Woodall + +2.4.9 (2015-08-16) +------------------ +* Import changes from jade-branch +* add missing dependencies +* Add ifdefs to fix build with gazebo2 + It was broken by `#315 `_. + Fixes `#321 `_. +* Use Joint::SetParam for joint velocity motors + Before gazebo5, Joint::SetVelocity and SetMaxForce + were used to set joint velocity motors. + The API has changed in gazebo5, to use Joint::SetParam + instead. + The functionality is still available through the SetParam API. +* Set GAZEBO_CXX_FLAGS to fix c++11 compilation errors +* Contributors: Akiyoshi Ochiai, Jose Luis Rivero, Steven Peters, ipa-fxm + +2.4.8 (2015-03-17) +------------------ +* Merge pull request `#244 `_ from cottsay/control-urdf-fix + gazebo_ros_control: add urdf to downstream catkin deps +* Added emergency stop support. +* Contributors: Adolfo Rodriguez Tsouroukdissian, Jim Rothrock, Scott K Logan + +2.4.7 (2014-12-15) +------------------ +* move declaration for DefaultRobotHWSim to header file +* Contributors: ipa-fxm + +2.4.6 (2014-09-01) +------------------ +* Update default_robot_hw_sim.cpp +* Reduced changes +* Fix to work with gazebo3 +* Fix build with gazebo4 and indigo +* Update package.xml + Add new maintainer. +* Contributors: Adolfo Rodriguez Tsouroukdissian, Jose Luis Rivero, Nate Koenig, hsu + +2.4.5 (2014-08-18) +------------------ +* Fix typo: GAZEBO_VERSION_MAJOR -> GAZEBO_MAJOR_VERSION +* Port fix_build branch for indigo-devel + See pull request `#221 `_ +* Contributors: Jose Luis Rivero, Steven Peters + +2.4.4 (2014-07-18) +------------------ +* Update package.xml + Add new maintainer. +* Should fix build error for binary releases. + See: http://www.ros.org/debbuild/indigo.html?q=gazebo_ros_control +* Updated package.xml +* gazebo_ros_control: default_robot_hw_sim: Suppressing pid error message + Depends on `ros-controls/control_toolbox#21 `_ +* Revert 4776545, as it belongs in indigo-devel. +* Fix repo names in package.xml's +* gazebo_ros_control: default_robot_hw_sim: Suppressing pid error message, depends on `ros-controls/control_toolbox#21 `_ +* gazebo_ros_control: Add dependency on angles +* gazebo_ros_control: Add build-time dependency on gazebo + This fixes a regression caused by a889ef8b768861231a67b78781514d834f631b8e +* Contributors: Adolfo Rodriguez Tsouroukdissian, Alexander Bubeck, Dave Coleman, Jon Binney, Jonathan Bohren, Scott K Logan + +2.4.3 (2014-05-12) +------------------ +* Compatibility with Indigo's ros_control. + Also fixes `#184 `_. +* Remove build-time dependency on gazebo_ros. +* Fix broken build due to wrong rosconsole macro use +* Contributors: Adolfo Rodriguez Tsouroukdissian + +2.4.2 (2014-03-27) +------------------ +* merging from hydro-devel +* bump patch version for indigo-devel to 2.4.1 +* merging from indigo-devel after 2.3.4 release +* Merge branch 'hydro-devel' of github.com:ros-simulation/gazebo_ros_pkgs into indigo-devel +* "2.4.0" +* catkin_generate_changelog +* Contributors: John Hsu + +2.4.1 (2013-11-13) +------------------ + +2.3.5 (2014-03-26) +------------------ +* Removed some debugging code. +* joint->SetAngle() and joint->SetVelocity() are now used to control + position-controlled joints and velocity-controlled joints that do not + have PID gain values stored on the Parameter Server. +* Position-controlled and velocity-controlled joints now use PID controllers + instead of calling SetAngle() or SetVelocity(). readSim() now longer calls + angles::shortest_angular_distance() when a joint is prismatic. + PLUGINLIB_EXPORT_CLASS is now used to register the plugin. +* gazebo_ros_control now depends on control_toolbox. +* Added support for the position hardware interface. Completed support for the + velocity hardware interface. +* Removed the "support more hardware interfaces" line. +* Contributors: Jim Rothrock + +2.3.4 (2013-11-13) +------------------ +* rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. +* Merge pull request `#144 `_ from meyerj/fix-125 + Fixed `#125 `_: ``gazebo_ros_control``: controlPeriod greater than the simulation period causes unexpected results +* Merge pull request `#134 `_ from meyerj/gazebo-ros-control-use-model-nh + ``gazebo_ros_control``: Use the model NodeHandle to get the ``robot_description`` parameter +* ``gazebo_ros_control``: added GazeboRosControlPlugin::Reset() method that resets the timestamps on world reset +* ``gazebo_ros_control``: call writeSim() for each Gazebo world update independent of the control period +* ``gazebo_ros_pkgs``: use GetMaxStepSize() for the Gazebo simulation period +* ``gazebo_ros_control``: use the model NodeHandle to get the ``robot_description`` parameter +* Add missing ``run_depend`` to urdf in ``gazebo_ros_control`` +* Remove dependency to meta-package ``ros_controllers`` + +2.4.0 (2013-10-14) +------------------ + +2.3.3 (2013-10-10) +------------------ +* Eliminated a joint_name variable and replaced it with `joint_names_[j]`. + Modified some lines so that they fit in 100 columns. These changes were made + in order to be consistent with the rest of the file. +* Merge remote-tracking branch 'upstream/hydro-devel' into hydro-devel +* joint_limits_interface is now used to enforce limits on effort-controlled + joints. +* Added "joint_limits_interface" and "urdf" to the component list. +* Additional parameters are passed to `robot_hw_sim->initSim()`. These parameters + are used by the joint limits interface. +* Added "joint_limits_interface" and "urdf" to the build dependency list. +* Added the robot_namespace and urdf_model parameters to `initSim()`. +* Added the urdf_string parameter to `parseTransmissionsFromURDF()`. + +2.3.2 (2013-09-19) +------------------ + +2.3.1 (2013-08-27) +------------------ +* Cleaned up template, fixes for header files +* Renamed plugin to match file name, tweaked CMakeLists +* Created a header file for the ros_control gazebo plugin + +2.3.0 (2013-08-12) +------------------ +* Renamed ros_control_plugin, updated documentation + +2.2.1 (2013-07-29) +------------------ + +2.2.0 (2013-07-29) +------------------ +* Standardized the way ROS nodes are initialized in gazebo plugins +* Remove find_package(SDF) from CMakeLists.txt + It is sufficient to find gazebo, which will export the information + about the SDFormat package. +* Merge branch 'hydro-devel' into tranmission_parsing +* Doc and debug update +* Merged hydro-devel +* Hid debug info +* Merged from Hydro-devel +* Merge branch 'hydro-devel' into tranmission_parsing +* Moved trasmission parsing to ros_control + +2.1.5 (2013-07-18) +------------------ + +2.1.4 (2013-07-14) +------------------ +* Fixed for Jenkins broken dependency on SDF in ros_control + +2.1.3 (2013-07-13) +------------------ + +2.1.2 (2013-07-12) +------------------ +* Cleaned up CMakeLists.txt for all gazebo_ros_pkgs +* 2.1.1 + +2.1.1 (2013-07-10 19:11) +------------------------ +* Fixed errors and deprecation warnings from Gazebo 1.9 and the sdformat split +* making RobotHWSim::initSim pure virtual +* Cleaning up code +* Adding install targets + +2.1.0 (2013-06-27) +------------------ +* Made version match the rest of gazebo_ros_pkgs per bloom +* Added dependency on ros_controllers +* Clarifying language in readme +* Made default period Gazebo's period +* Made control period optional +* Tweaked README +* Added support for reading tags and other cleaning up +* Renamed RobotSim to RobotHWSim +* Renaming all gazebo_ros_control stuff to be in the same package +* Refactoring gazebo_ros_control packages into a single package, removing exampls (they will go elsewhere) +* updating readme for gazebo_ros_control +* Merging in gazebo_ros_control +* making gazebo_ros_control a metapackage +* Moving readme +* Merging readmes +* eating this +* Merging gazebo_ros_control and ros_control_gazebo + +2.0.2 (2013-06-20) +------------------ + +2.0.1 (2013-06-19) +------------------ + +2.0.0 (2013-06-18) +------------------ diff --git a/gazebo_ros_pkgs/gazebo_ros_control/CMakeLists.txt b/gazebo_ros_pkgs/gazebo_ros_control/CMakeLists.txt new file mode 100755 index 0000000..58304f3 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_control/CMakeLists.txt @@ -0,0 +1,62 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gazebo_ros_control) + +# Load catkin and all dependencies required for this package +find_package(catkin REQUIRED COMPONENTS + gazebo_dev + roscpp + std_msgs + control_toolbox + controller_manager + hardware_interface + transmission_interface + pluginlib + joint_limits_interface + urdf + angles +) + +catkin_package( + CATKIN_DEPENDS + roscpp + std_msgs + controller_manager + control_toolbox + pluginlib + hardware_interface + transmission_interface + joint_limits_interface + urdf + angles + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} default_robot_hw_sim +) + +link_directories( + ${catkin_LIBRARY_DIRS} +) + +include_directories(include + ${Boost_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS} +) + +## Libraries +add_library(${PROJECT_NAME} src/gazebo_ros_control_plugin.cpp) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +add_library(default_robot_hw_sim src/default_robot_hw_sim.cpp) +target_link_libraries(default_robot_hw_sim ${catkin_LIBRARIES}) + +## Install +install(TARGETS ${PROJECT_NAME} default_robot_hw_sim + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(FILES robot_hw_sim_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/gazebo_ros_pkgs/gazebo_ros_control/README.md b/gazebo_ros_pkgs/gazebo_ros_control/README.md new file mode 100755 index 0000000..90c989b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_control/README.md @@ -0,0 +1,13 @@ +# Gazebo ros_control Interfaces + +This is a ROS package for integrating the `ros_control` controller architecture +with the [Gazebo](http://gazebosim.org/) simulator. + +This package provides a Gazebo plugin which instantiates a ros_control +controller manager and connects it to a Gazebo model. + +[Documentation](http://gazebosim.org/tutorials?tut=ros_control) is provided on Gazebo's website. + +## Future Direction + + - Implement transmissions diff --git a/gazebo_ros_pkgs/gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h b/gazebo_ros_pkgs/gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h new file mode 100755 index 0000000..acfd10e --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h @@ -0,0 +1,147 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * Copyright (c) 2013, The Johns Hopkins University + * All rights reserved. + * + * 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 the Open Source Robotics Foundation + * 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: Dave Coleman, Jonathan Bohren + Desc: Hardware Interface for any simulated robot in Gazebo +*/ + +#ifndef _GAZEBO_ROS_CONTROL___DEFAULT_ROBOT_HW_SIM_H_ +#define _GAZEBO_ROS_CONTROL___DEFAULT_ROBOT_HW_SIM_H_ + +// ros_control +#include +#include +#include +#include +#include +#include +#include + +// Gazebo +#include +#include +#include + +// ROS +#include +#include +#include + +// gazebo_ros_control +#include + +// URDF +#include + + + +namespace gazebo_ros_control +{ + +class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim +{ +public: + + virtual bool initSim( + const std::string& robot_namespace, + ros::NodeHandle model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model *const urdf_model, + std::vector transmissions); + + virtual void readSim(ros::Time time, ros::Duration period); + + virtual void writeSim(ros::Time time, ros::Duration period); + + virtual void eStopActive(const bool active); + +protected: + // Methods used to control a joint. + enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID}; + + // Register the limits of the joint specified by joint_name and joint_handle. The limits are + // retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. + // Return the joint's type, lower position limit, upper position limit, and effort limit. + void registerJointLimits(const std::string& joint_name, + const hardware_interface::JointHandle& joint_handle, + const ControlMethod ctrl_method, + const ros::NodeHandle& joint_limit_nh, + const urdf::Model *const urdf_model, + int *const joint_type, double *const lower_limit, + double *const upper_limit, double *const effort_limit); + + unsigned int n_dof_; + + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + hardware_interface::PositionJointInterface pj_interface_; + hardware_interface::VelocityJointInterface vj_interface_; + + joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_; + joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_; + joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_; + joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_; + joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_; + joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_; + + std::vector joint_names_; + std::vector joint_types_; + std::vector joint_lower_limits_; + std::vector joint_upper_limits_; + std::vector joint_effort_limits_; + std::vector joint_control_methods_; + std::vector pid_controllers_; + std::vector joint_position_; + std::vector joint_velocity_; + std::vector joint_effort_; + std::vector joint_effort_command_; + std::vector joint_position_command_; + std::vector last_joint_position_command_; + std::vector joint_velocity_command_; + + std::vector sim_joints_; + + std::string physics_type_; + + // e_stop_active_ is true if the emergency stop is active. + bool e_stop_active_, last_e_stop_active_; +}; + +typedef boost::shared_ptr DefaultRobotHWSimPtr; + +} + +#endif // #ifndef __GAZEBO_ROS_CONTROL_PLUGIN_DEFAULT_ROBOT_HW_SIM_H_ diff --git a/gazebo_ros_pkgs/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h b/gazebo_ros_pkgs/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h new file mode 100755 index 0000000..0c11eaf --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h @@ -0,0 +1,131 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * Copyright (c) 2013, The Johns Hopkins University + * All rights reserved. + * + * 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 the Open Source Robotics Foundation + * 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: Dave Coleman, Jonathan Bohren + Desc: Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in + using pluginlib +*/ + +// Boost +#include +#include + +// ROS +#include +#include +#include + +// Gazebo +#include +#include +#include + +// ros_control +#include +#include +#include + +namespace gazebo_ros_control +{ + +class GazeboRosControlPlugin : public gazebo::ModelPlugin +{ +public: + + virtual ~GazeboRosControlPlugin(); + + // Overloaded Gazebo entry point + virtual void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf); + + // Called by the world update start event + void Update(); + + // Called on world reset + virtual void Reset(); + + // Get the URDF XML from the parameter server + std::string getURDF(std::string param_name) const; + + // Get Transmissions from the URDF + bool parseTransmissionsFromURDF(const std::string& urdf_string); + +protected: + void eStopCB(const std_msgs::BoolConstPtr& e_stop_active); + + // Node Handles + ros::NodeHandle model_nh_; // namespaces to robot name + + // Pointer to the model + gazebo::physics::ModelPtr parent_model_; + sdf::ElementPtr sdf_; + + // deferred load in case ros is blocking + boost::thread deferred_load_thread_; + + // Pointer to the update event connection + gazebo::event::ConnectionPtr update_connection_; + + // Interface loader + boost::shared_ptr > robot_hw_sim_loader_; + void load_robot_hw_sim_srv(); + + // Strings + std::string robot_namespace_; + std::string robot_description_; + + // Transmissions in this plugin's scope + std::vector transmissions_; + + // Robot simulator interface + std::string robot_hw_sim_type_str_; + boost::shared_ptr robot_hw_sim_; + + // Controller manager + boost::shared_ptr controller_manager_; + + // Timing + ros::Duration control_period_; + ros::Time last_update_sim_time_ros_; + ros::Time last_write_sim_time_ros_; + + // e_stop_active_ is true if the emergency stop is active. + bool e_stop_active_, last_e_stop_active_; + ros::Subscriber e_stop_sub_; // Emergency stop subscriber + +}; + + +} diff --git a/gazebo_ros_pkgs/gazebo_ros_control/include/gazebo_ros_control/robot_hw_sim.h b/gazebo_ros_pkgs/gazebo_ros_control/include/gazebo_ros_control/robot_hw_sim.h new file mode 100755 index 0000000..4e91447 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_control/include/gazebo_ros_control/robot_hw_sim.h @@ -0,0 +1,119 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * Copyright (c) 2013, The Johns Hopkins University + * All rights reserved. + * + * 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 the Open Source Robotics Foundation + * 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. + *********************************************************************/ + +/// \brief Plugin template for hardware interfaces for ros_control and Gazebo + +/// \author Jonathan Bohren +/// \author Dave Coleman + +#ifndef __ROS_CONTROL_GAZEBO_ROBOT_HW_SIM_H +#define __ROS_CONTROL_GAZEBO_ROBOT_HW_SIM_H + +#include +#include +#include +#include +#include + +namespace gazebo_ros_control { + + // Struct for passing loaded joint data + struct JointData + { + std::string name_; + std::string hardware_interface_; + + JointData(const std::string& name, const std::string& hardware_interface) : + name_(name), + hardware_interface_(hardware_interface) + {} + }; + + /// \brief Gazebo plugin version of RobotHW + /// + /// An object of class RobotHWSim represents a robot's simulated hardware. + class RobotHWSim : public hardware_interface::RobotHW + { + public: + + virtual ~RobotHWSim() { } + + /// \brief Initialize the simulated robot hardware + /// + /// Initialize the simulated robot hardware. + /// + /// \param robot_namespace Robot namespace. + /// \param model_nh Model node handle. + /// \param parent_model Parent model. + /// \param urdf_model URDF model. + /// \param transmissions Transmissions. + /// + /// \return \c true if the simulated robot hardware is initialized successfully, \c false if not. + virtual bool initSim( + const std::string& robot_namespace, + ros::NodeHandle model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model *const urdf_model, + std::vector transmissions) = 0; + + /// \brief Read state data from the simulated robot hardware + /// + /// Read state data, such as joint positions and velocities, from the simulated robot hardware. + /// + /// \param time Simulation time. + /// \param period Time since the last simulation step. + virtual void readSim(ros::Time time, ros::Duration period) = 0; + + /// \brief Write commands to the simulated robot hardware + /// + /// Write commands, such as joint position and velocity commands, to the simulated robot hardware. + /// + /// \param time Simulation time. + /// \param period Time since the last simulation step. + virtual void writeSim(ros::Time time, ros::Duration period) = 0; + + /// \brief Set the emergency stop state + /// + /// Set the simulated robot's emergency stop state. The default implementation of this function does nothing. + /// + /// \param active \c true if the emergency stop is active, \c false if not. + virtual void eStopActive(const bool active) {} + + }; + +} + +#endif // ifndef __ROS_CONTROL_GAZEBO_ROBOT_HW_SIM_H diff --git a/gazebo_ros_pkgs/gazebo_ros_control/package.xml b/gazebo_ros_pkgs/gazebo_ros_control/package.xml new file mode 100755 index 0000000..7c59f21 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_control/package.xml @@ -0,0 +1,36 @@ + + + gazebo_ros_control + 2.5.15 + gazebo_ros_control + + Jose Luis Rivero + + BSD + + http://ros.org/wiki/gazebo_ros_control + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs + + Jonathan Bohren + Dave Coleman + + catkin + + gazebo_dev + gazebo_ros + roscpp + std_msgs + control_toolbox + controller_manager + pluginlib + hardware_interface + transmission_interface + joint_limits_interface + urdf + angles + + + + + diff --git a/gazebo_ros_pkgs/gazebo_ros_control/robot_hw_sim_plugins.xml b/gazebo_ros_pkgs/gazebo_ros_control/robot_hw_sim_plugins.xml new file mode 100755 index 0000000..5034ee1 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_control/robot_hw_sim_plugins.xml @@ -0,0 +1,11 @@ + + + + + A default robot simulation interface which constructs joint handles from an SDF/URDF. + + + diff --git a/gazebo_ros_pkgs/gazebo_ros_control/src/default_robot_hw_sim.cpp b/gazebo_ros_pkgs/gazebo_ros_control/src/default_robot_hw_sim.cpp new file mode 100755 index 0000000..4d3f799 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_control/src/default_robot_hw_sim.cpp @@ -0,0 +1,518 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * Copyright (c) 2013, The Johns Hopkins University + * All rights reserved. + * + * 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 the Open Source Robotics Foundation + * 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: Dave Coleman, Jonathan Bohren + Desc: Hardware Interface for any simulated robot in Gazebo +*/ + + +#include +#include + + +namespace +{ + +double clamp(const double val, const double min_val, const double max_val) +{ + return std::min(std::max(val, min_val), max_val); +} + +} + +namespace gazebo_ros_control +{ + + +bool DefaultRobotHWSim::initSim( + const std::string& robot_namespace, + ros::NodeHandle model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model *const urdf_model, + std::vector transmissions) +{ + // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each + // parameter's name is "joint_limits/". An example is "joint_limits/axle_joint". + const ros::NodeHandle joint_limit_nh(model_nh); + + // Resize vectors to our DOF + n_dof_ = transmissions.size(); + joint_names_.resize(n_dof_); + joint_types_.resize(n_dof_); + joint_lower_limits_.resize(n_dof_); + joint_upper_limits_.resize(n_dof_); + joint_effort_limits_.resize(n_dof_); + joint_control_methods_.resize(n_dof_); + pid_controllers_.resize(n_dof_); + joint_position_.resize(n_dof_); + joint_velocity_.resize(n_dof_); + joint_effort_.resize(n_dof_); + joint_effort_command_.resize(n_dof_); + joint_position_command_.resize(n_dof_); + joint_velocity_command_.resize(n_dof_); + + // Initialize values + for(unsigned int j=0; j < n_dof_; j++) + { + // Check that this transmission has one joint + if(transmissions[j].joints_.size() == 0) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ + << " has no associated joints."); + continue; + } + else if(transmissions[j].joints_.size() > 1) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_ + << " has more than one joint. Currently the default robot hardware simulation " + << " interface only supports one."); + continue; + } + + std::vector joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_; + if (joint_interfaces.empty() && + !(transmissions[j].actuators_.empty()) && + !(transmissions[j].actuators_[0].hardware_interfaces_.empty())) + { + // TODO: Deprecate HW interface specification in actuators in ROS J + joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The element of tranmission " << + transmissions[j].name_ << " should be nested inside the element, not . " << + "The transmission will be properly loaded, but please update " << + "your robot model to remain compatible with future versions of the plugin."); + } + if (joint_interfaces.empty()) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << + " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " << + "Not adding it to the robot hardware simulation."); + continue; + } + else if (joint_interfaces.size() > 1) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ << + " of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " << + "Currently the default robot hardware simulation interface only supports one. Using the first entry"); + //continue; + } + + // Add data from transmission + joint_names_[j] = transmissions[j].joints_[0].name_; + joint_position_[j] = 1.0; + joint_velocity_[j] = 0.0; + joint_effort_[j] = 1.0; // N/m for continuous joints + joint_effort_command_[j] = 0.0; + joint_position_command_[j] = 0.0; + joint_velocity_command_[j] = 0.0; + + const std::string& hardware_interface = joint_interfaces.front(); + + // Debug + ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j] + << "' of type '" << hardware_interface << "'"); + + // Create joint state interface for all joints + js_interface_.registerHandle(hardware_interface::JointStateHandle( + joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); + + // Decide what kind of command interface this actuator/joint has + hardware_interface::JointHandle joint_handle; + if(hardware_interface == "EffortJointInterface" || hardware_interface == "hardware_interface/EffortJointInterface") + { + // Create effort joint interface + joint_control_methods_[j] = EFFORT; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_effort_command_[j]); + ej_interface_.registerHandle(joint_handle); + } + else if(hardware_interface == "PositionJointInterface" || hardware_interface == "hardware_interface/PositionJointInterface") + { + // Create position joint interface + joint_control_methods_[j] = POSITION; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_position_command_[j]); + pj_interface_.registerHandle(joint_handle); + } + else if(hardware_interface == "VelocityJointInterface" || hardware_interface == "hardware_interface/VelocityJointInterface") + { + // Create velocity joint interface + joint_control_methods_[j] = VELOCITY; + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_velocity_command_[j]); + vj_interface_.registerHandle(joint_handle); + } + else + { + ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '" + << hardware_interface << "' while loading interfaces for " << joint_names_[j] ); + return false; + } + + if(hardware_interface == "EffortJointInterface" || hardware_interface == "PositionJointInterface" || hardware_interface == "VelocityJointInterface") { + ROS_WARN_STREAM("Deprecated syntax, please prepend 'hardware_interface/' to '" << hardware_interface << "' within the tag in joint '" << joint_names_[j] << "'."); + } + + // Get the gazebo joint that corresponds to the robot joint. + //ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim", "Getting pointer to gazebo joint: " + // << joint_names_[j]); + gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); + if (!joint) + { + ROS_ERROR_STREAM_NAMED("default_robot_hw", "This robot has a joint named \"" << joint_names_[j] + << "\" which is not in the gazebo model."); + return false; + } + sim_joints_.push_back(joint); + + // get physics engine type +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics(); +#else + gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine(); +#endif + physics_type_ = physics->GetType(); + if (physics_type_.empty()) + { + ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "No physics type found."); + } + + registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j], + joint_limit_nh, urdf_model, + &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j], + &joint_effort_limits_[j]); + if (joint_control_methods_[j] != EFFORT) + { + // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or + // joint->SetParam("vel") to control the joint. + const ros::NodeHandle nh(model_nh, "/gazebo_ros_control/pid_gains/" + + joint_names_[j]); + if (pid_controllers_[j].init(nh, true)) + { + switch (joint_control_methods_[j]) + { + case POSITION: + joint_control_methods_[j] = POSITION_PID; + break; + case VELOCITY: + joint_control_methods_[j] = VELOCITY_PID; + break; + } + } + else + { + // joint->SetParam("fmax") must be called if joint->SetAngle() or joint->SetParam("vel") are + // going to be called. joint->SetParam("fmax") must *not* be called if joint->SetForce() is + // going to be called. +#if GAZEBO_MAJOR_VERSION > 2 + joint->SetParam("fmax", 0, joint_effort_limits_[j]); +#else + joint->SetMaxForce(0, joint_effort_limits_[j]); +#endif + } + } + } + + // Register interfaces + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&pj_interface_); + registerInterface(&vj_interface_); + + // Initialize the emergency stop code. + e_stop_active_ = false; + last_e_stop_active_ = false; + + return true; +} + +void DefaultRobotHWSim::readSim(ros::Time time, ros::Duration period) +{ + for(unsigned int j=0; j < n_dof_; j++) + { + // Gazebo has an interesting API... +#if GAZEBO_MAJOR_VERSION >= 8 + double position = sim_joints_[j]->Position(0); +#else + double position = sim_joints_[j]->GetAngle(0).Radian(); +#endif + if (joint_types_[j] == urdf::Joint::PRISMATIC) + { + joint_position_[j] = position; + } + else + { + joint_position_[j] += angles::shortest_angular_distance(joint_position_[j], + position); + } + joint_velocity_[j] = sim_joints_[j]->GetVelocity(0); + joint_effort_[j] = sim_joints_[j]->GetForce((unsigned int)(0)); + } +} + +void DefaultRobotHWSim::writeSim(ros::Time time, ros::Duration period) +{ + // If the E-stop is active, joints controlled by position commands will maintain their positions. + if (e_stop_active_) + { + if (!last_e_stop_active_) + { + last_joint_position_command_ = joint_position_; + last_e_stop_active_ = true; + } + joint_position_command_ = last_joint_position_command_; + } + else + { + last_e_stop_active_ = false; + } + + ej_sat_interface_.enforceLimits(period); + ej_limits_interface_.enforceLimits(period); + pj_sat_interface_.enforceLimits(period); + pj_limits_interface_.enforceLimits(period); + vj_sat_interface_.enforceLimits(period); + vj_limits_interface_.enforceLimits(period); + + for(unsigned int j=0; j < n_dof_; j++) + { + switch (joint_control_methods_[j]) + { + case EFFORT: + { + const double effort = e_stop_active_ ? 0 : joint_effort_command_[j]; + sim_joints_[j]->SetForce(0, effort); + } + break; + + case POSITION: +#if GAZEBO_MAJOR_VERSION >= 9 + sim_joints_[j]->SetPosition(0, joint_position_command_[j], true); +#else + ROS_WARN_ONCE("The default_robot_hw_sim plugin is using the Joint::SetPosition method without preserving the link velocity."); + ROS_WARN_ONCE("As a result, gravity will not be simulated correctly for your model."); + ROS_WARN_ONCE("Please set gazebo_pid parameters, switch to the VelocityJointInterface or EffortJointInterface, or upgrade to Gazebo 9."); + ROS_WARN_ONCE("For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612"); + sim_joints_[j]->SetPosition(0, joint_position_command_[j]); +#endif + break; + + case POSITION_PID: + { + double error; + switch (joint_types_[j]) + { + case urdf::Joint::REVOLUTE: + angles::shortest_angular_distance_with_limits(joint_position_[j], + joint_position_command_[j], + joint_lower_limits_[j], + joint_upper_limits_[j], + error); + break; + case urdf::Joint::CONTINUOUS: + error = angles::shortest_angular_distance(joint_position_[j], + joint_position_command_[j]); + break; + default: + error = joint_position_command_[j] - joint_position_[j]; + } + + const double effort_limit = joint_effort_limits_[j]; + const double effort = clamp(pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); + sim_joints_[j]->SetForce(0, effort); + } + break; + + case VELOCITY: +#if GAZEBO_MAJOR_VERSION > 2 + if (physics_type_.compare("dart") == 0) + { + sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]); + } + else + { + sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]); + } +#else + sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]); +#endif + break; + + case VELOCITY_PID: + double error; + if (e_stop_active_) + error = -joint_velocity_[j]; + else + error = joint_velocity_command_[j] - joint_velocity_[j]; + const double effort_limit = joint_effort_limits_[j]; + const double effort = clamp(pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); + sim_joints_[j]->SetForce(0, effort); + break; + } + } +} + +void DefaultRobotHWSim::eStopActive(const bool active) +{ + e_stop_active_ = active; +} + +// Register the limits of the joint specified by joint_name and joint_handle. The limits are +// retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. +// Return the joint's type, lower position limit, upper position limit, and effort limit. +void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name, + const hardware_interface::JointHandle& joint_handle, + const ControlMethod ctrl_method, + const ros::NodeHandle& joint_limit_nh, + const urdf::Model *const urdf_model, + int *const joint_type, double *const lower_limit, + double *const upper_limit, double *const effort_limit) +{ + *joint_type = urdf::Joint::UNKNOWN; + *lower_limit = -std::numeric_limits::max(); + *upper_limit = std::numeric_limits::max(); + *effort_limit = std::numeric_limits::max(); + + joint_limits_interface::JointLimits limits; + bool has_limits = false; + joint_limits_interface::SoftJointLimits soft_limits; + bool has_soft_limits = false; + + if (urdf_model != NULL) + { + const urdf::JointConstSharedPtr urdf_joint = urdf_model->getJoint(joint_name); + if (urdf_joint != NULL) + { + *joint_type = urdf_joint->type; + // Get limits from the URDF file. + if (joint_limits_interface::getJointLimits(urdf_joint, limits)) + has_limits = true; + if (joint_limits_interface::getSoftJointLimits(urdf_joint, soft_limits)) + has_soft_limits = true; + } + } + // Get limits from the parameter server. + if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits)) + has_limits = true; + + if (!has_limits) + return; + + if (*joint_type == urdf::Joint::UNKNOWN) + { + // Infer the joint type. + + if (limits.has_position_limits) + { + *joint_type = urdf::Joint::REVOLUTE; + } + else + { + if (limits.angle_wraparound) + *joint_type = urdf::Joint::CONTINUOUS; + else + *joint_type = urdf::Joint::PRISMATIC; + } + } + + if (limits.has_position_limits) + { + *lower_limit = limits.min_position; + *upper_limit = limits.max_position; + } + if (limits.has_effort_limits) + *effort_limit = limits.max_effort; + + if (has_soft_limits) + { + switch (ctrl_method) + { + case EFFORT: + { + const joint_limits_interface::EffortJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + ej_limits_interface_.registerHandle(limits_handle); + } + break; + case POSITION: + { + const joint_limits_interface::PositionJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + pj_limits_interface_.registerHandle(limits_handle); + } + break; + case VELOCITY: + { + const joint_limits_interface::VelocityJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + vj_limits_interface_.registerHandle(limits_handle); + } + break; + } + } + else + { + switch (ctrl_method) + { + case EFFORT: + { + const joint_limits_interface::EffortJointSaturationHandle + sat_handle(joint_handle, limits); + ej_sat_interface_.registerHandle(sat_handle); + } + break; + case POSITION: + { + const joint_limits_interface::PositionJointSaturationHandle + sat_handle(joint_handle, limits); + pj_sat_interface_.registerHandle(sat_handle); + } + break; + case VELOCITY: + { + const joint_limits_interface::VelocityJointSaturationHandle + sat_handle(joint_handle, limits); + vj_sat_interface_.registerHandle(sat_handle); + } + break; + } + } +} + +} + +PLUGINLIB_EXPORT_CLASS(gazebo_ros_control::DefaultRobotHWSim, gazebo_ros_control::RobotHWSim) diff --git a/gazebo_ros_pkgs/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/gazebo_ros_pkgs/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp new file mode 100755 index 0000000..c4d7437 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp @@ -0,0 +1,310 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2013, Open Source Robotics Foundation + * Copyright (c) 2013, The Johns Hopkins University + * All rights reserved. + * + * 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 the Open Source Robotics Foundation + * 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: Dave Coleman, Jonathan Bohren + Desc: Gazebo plugin for ros_control that allows 'hardware_interfaces' to be plugged in + using pluginlib +*/ + +// Boost +#include + +#include +#include + +namespace gazebo_ros_control +{ + +GazeboRosControlPlugin::~GazeboRosControlPlugin() +{ + // Disconnect from gazebo events + update_connection_.reset(); +} + +// Overloaded Gazebo entry point +void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) +{ + ROS_INFO_STREAM_NAMED("gazebo_ros_control","Loading gazebo_ros_control plugin"); + + + // Save pointers to the model + parent_model_ = parent; + sdf_ = sdf; + + // Error message if the model couldn't be found + if (!parent_model_) + { + ROS_ERROR_STREAM_NAMED("loadThread","parent model is NULL"); + return; + } + + // Check that ROS has been initialized + if(!ros::isInitialized()) + { + ROS_FATAL_STREAM_NAMED("gazebo_ros_control","A ROS node for Gazebo has not been initialized, unable to load plugin. " + << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); + return; + } + + // Get namespace for nodehandle + if(sdf_->HasElement("robotNamespace")) + { + robot_namespace_ = sdf_->GetElement("robotNamespace")->Get(); + } + else + { + robot_namespace_ = parent_model_->GetName(); // default + } + + // Get robot_description ROS param name + if (sdf_->HasElement("robotParam")) + { + robot_description_ = sdf_->GetElement("robotParam")->Get(); + } + else + { + robot_description_ = "robot_description"; // default + } + + // Get the robot simulation interface type + if(sdf_->HasElement("robotSimType")) + { + robot_hw_sim_type_str_ = sdf_->Get("robotSimType"); + } + else + { + robot_hw_sim_type_str_ = "gazebo_ros_control/DefaultRobotHWSim"; + ROS_DEBUG_STREAM_NAMED("loadThread","Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<= 8 + ros::Duration gazebo_period(parent_model_->GetWorld()->Physics()->GetMaxStepSize()); +#else + ros::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize()); +#endif + + // Decide the plugin control period + if(sdf_->HasElement("controlPeriod")) + { + control_period_ = ros::Duration(sdf_->Get("controlPeriod")); + + // Check the period against the simulation period + if( control_period_ < gazebo_period ) + { + ROS_ERROR_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("< gazebo_period ) + { + ROS_WARN_STREAM_NAMED("gazebo_ros_control","Desired controller update period ("<HasElement("eStopTopic")) + { + const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get(); + e_stop_sub_ = model_nh_.subscribe(e_stop_topic, 1, &GazeboRosControlPlugin::eStopCB, this); + } + + ROS_INFO_NAMED("gazebo_ros_control", "Starting gazebo_ros_control plugin in namespace: %s", robot_namespace_.c_str()); + + // Read urdf from ros parameter server then + // setup actuators and mechanism control node. + // This call will block if ROS is not properly initialized. + const std::string urdf_string = getURDF(robot_description_); + if (!parseTransmissionsFromURDF(urdf_string)) + { + ROS_ERROR_NAMED("gazebo_ros_control", "Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n"); + return; + } + + // Load the RobotHWSim abstraction to interface the controllers with the gazebo model + try + { + robot_hw_sim_loader_.reset + (new pluginlib::ClassLoader + ("gazebo_ros_control", + "gazebo_ros_control::RobotHWSim")); + + robot_hw_sim_ = robot_hw_sim_loader_->createInstance(robot_hw_sim_type_str_); + urdf::Model urdf_model; + const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL; + + if(!robot_hw_sim_->initSim(robot_namespace_, model_nh_, parent_model_, urdf_model_ptr, transmissions_)) + { + ROS_FATAL_NAMED("gazebo_ros_control","Could not initialize robot simulation interface"); + return; + } + + // Create the controller manager + ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager"); + controller_manager_.reset + (new controller_manager::ControllerManager(robot_hw_sim_.get(), model_nh_)); + + // Listen to the update event. This event is broadcast every simulation iteration. + update_connection_ = + gazebo::event::Events::ConnectWorldUpdateBegin + (boost::bind(&GazeboRosControlPlugin::Update, this)); + + } + catch(pluginlib::LibraryLoadException &ex) + { + ROS_FATAL_STREAM_NAMED("gazebo_ros_control","Failed to create robot simulation interface loader: "<= 8 + gazebo::common::Time gz_time_now = parent_model_->GetWorld()->SimTime(); +#else + gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime(); +#endif + ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec); + ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_; + + robot_hw_sim_->eStopActive(e_stop_active_); + + // Check if we should update the controllers + if(sim_period >= control_period_) { + // Store this simulation time + last_update_sim_time_ros_ = sim_time_ros; + + // Update the robot simulation with the state of the gazebo model + robot_hw_sim_->readSim(sim_time_ros, sim_period); + + // Compute the controller commands + bool reset_ctrlrs; + if (e_stop_active_) + { + reset_ctrlrs = false; + last_e_stop_active_ = true; + } + else + { + if (last_e_stop_active_) + { + reset_ctrlrs = true; + last_e_stop_active_ = false; + } + else + { + reset_ctrlrs = false; + } + } + controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs); + } + + // Update the gazebo model with the result of the controller + // computation + robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_); + last_write_sim_time_ros_ = sim_time_ros; +} + +// Called on world reset +void GazeboRosControlPlugin::Reset() +{ + // Reset timing variables to not pass negative update periods to controllers on world reset + last_update_sim_time_ros_ = ros::Time(); + last_write_sim_time_ros_ = ros::Time(); +} + +// Get the URDF XML from the parameter server +std::string GazeboRosControlPlugin::getURDF(std::string param_name) const +{ + std::string urdf_string; + + // search and wait for robot_description on param server + while (urdf_string.empty()) + { + std::string search_param_name; + if (model_nh_.searchParam(param_name, search_param_name)) + { + ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model" + " URDF in parameter [%s] on the ROS param server.", search_param_name.c_str()); + + model_nh_.getParam(search_param_name, urdf_string); + } + else + { + ROS_INFO_ONCE_NAMED("gazebo_ros_control", "gazebo_ros_control plugin is waiting for model" + " URDF in parameter [%s] on the ROS param server.", robot_description_.c_str()); + + model_nh_.getParam(param_name, urdf_string); + } + + usleep(100000); + } + ROS_DEBUG_STREAM_NAMED("gazebo_ros_control", "Recieved urdf from param server, parsing..."); + + return urdf_string; +} + +// Get Transmissions from the URDF +bool GazeboRosControlPlugin::parseTransmissionsFromURDF(const std::string& urdf_string) +{ + transmission_interface::TransmissionParser::parse(urdf_string, transmissions_); + return true; +} + +// Emergency stop callback +void GazeboRosControlPlugin::eStopCB(const std_msgs::BoolConstPtr& e_stop_active) +{ + e_stop_active_ = e_stop_active->data; +} + + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboRosControlPlugin); +} // namespace diff --git a/gazebo_ros_pkgs/gazebo_ros_pkgs/CHANGELOG.rst b/gazebo_ros_pkgs/gazebo_ros_pkgs/CHANGELOG.rst new file mode 100755 index 0000000..05bb5ad --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_pkgs/CHANGELOG.rst @@ -0,0 +1,165 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gazebo_ros_pkgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.5.15 (2018-02-12) +------------------- + +2.5.14 (2017-12-11) +------------------- + +2.5.13 (2017-06-24) +------------------- +* Add catkin package(s) to provide the default version of Gazebo (`#571 `_) + * Added catkin package gazebo_dev which provides the cmake config of the installed Gazebo version +* Contributors: Jose Luis Rivero + +2.5.12 (2017-04-25) +------------------- + +2.5.11 (2017-04-18) +------------------- + +2.5.10 (2017-03-03) +------------------- + +2.5.9 (2017-02-20) +------------------ + +2.5.8 (2016-12-06) +------------------ + +2.5.7 (2016-06-10) +------------------ + +2.5.6 (2016-04-28) +------------------ + +2.5.5 (2016-04-27) +------------------ +* merge indigo, jade to kinetic-devel +* Update maintainer for Kinetic release +* Contributors: Jose Luis Rivero, Steven Peters + +2.5.3 (2016-04-11) +------------------ + +2.5.2 (2016-02-25) +------------------ +* merging from indigo-devel +* 2.4.9 +* Generate changelog +* Contributors: John Hsu, Jose Luis Rivero + +2.5.1 (2015-08-16) +------------------ + +2.5.0 (2015-04-30) +------------------ + +2.4.10 (2016-02-25) +------------------- + +2.4.9 (2015-08-16) +------------------ + +2.4.8 (2015-03-17) +------------------ + +2.4.7 (2014-12-15) +------------------ +* Update Gazebo/ROS tutorial URL +* Contributors: Jose Luis Rivero + +2.4.6 (2014-09-01) +------------------ + +2.4.5 (2014-08-18) +------------------ + +2.4.4 (2014-07-18) +------------------ +* Updated package.xml +* Fix repo names in package.xml's +* Contributors: Dave Coleman, Jon Binney + +2.4.3 (2014-05-12) +------------------ + +2.4.2 (2014-03-27) +------------------ +* merging from hydro-devel +* bump patch version for indigo-devel to 2.4.1 +* merging from indigo-devel after 2.3.4 release +* "2.4.0" +* catkin_generate_changelog +* Contributors: John Hsu + +2.4.1 (2013-11-13) +------------------ +* rerelease because sdformat became libsdformat, but we also based change on 2.3.4 in hydro-devel. + +2.4.0 (2013-10-14) +------------------ + +2.3.5 (2014-03-26) +------------------ + +2.3.4 (2013-11-13) +------------------ + +2.3.3 (2013-10-10) +------------------ + +2.3.2 (2013-09-19) +------------------ + +2.3.1 (2013-08-27) +------------------ + +2.3.0 (2013-08-12) +------------------ +* Renamed ros_control_plugin, updated documentation + +2.2.1 (2013-07-29) +------------------ + +2.2.0 (2013-07-29) +------------------ + +2.1.5 (2013-07-18) +------------------ + +2.1.4 (2013-07-14) +------------------ + +2.1.3 (2013-07-13) +------------------ + +2.1.2 (2013-07-12) +------------------ +* 2.1.1 + +2.1.1 (2013-07-10 19:11) +------------------------ + +2.1.0 (2013-06-27) +------------------ +* Added args to launch files, documentation +* Updated package.xml + +2.0.2 (2013-06-20) +------------------ + +2.0.1 (2013-06-19) +------------------ +* Incremented version to 2.0.1 +* Updated documentation diagrams + +2.0.0 (2013-06-18) +------------------ +* Changed version to 2.0.0 based on gazebo_simulator being 1.0.0 +* Updated package.xml files for ros.org documentation purposes +* Created new diagram +* Moved diagrams into repository +* Renamed meta package for gazebo_ros_pkgs diff --git a/gazebo_ros_pkgs/gazebo_ros_pkgs/CMakeLists.txt b/gazebo_ros_pkgs/gazebo_ros_pkgs/CMakeLists.txt new file mode 100755 index 0000000..126155b --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_pkgs/CMakeLists.txt @@ -0,0 +1,4 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gazebo_ros_pkgs) +find_package(catkin REQUIRED) +catkin_metapackage() diff --git a/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_api.odg b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_api.odg new file mode 100755 index 0000000..7971949 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_api.odg differ diff --git a/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_api.pdf b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_api.pdf new file mode 100755 index 0000000..8f30a43 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_api.pdf differ diff --git a/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_api.png b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_api.png new file mode 100755 index 0000000..65d1caa Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_api.png differ diff --git a/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_transmission.odg b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_transmission.odg new file mode 100755 index 0000000..f892954 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_transmission.odg differ diff --git a/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_transmission.pdf b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_transmission.pdf new file mode 100755 index 0000000..415d8b9 Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_transmission.pdf differ diff --git a/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_transmission.png b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_transmission.png new file mode 100755 index 0000000..0ad80fe Binary files /dev/null and b/gazebo_ros_pkgs/gazebo_ros_pkgs/documentation/gazebo_ros_transmission.png differ diff --git a/gazebo_ros_pkgs/gazebo_ros_pkgs/package.xml b/gazebo_ros_pkgs/gazebo_ros_pkgs/package.xml new file mode 100755 index 0000000..704e8a8 --- /dev/null +++ b/gazebo_ros_pkgs/gazebo_ros_pkgs/package.xml @@ -0,0 +1,27 @@ + + + gazebo_ros_pkgs + 2.5.15 + Interface for using ROS with the Gazebo simulator. + + Jose Luis Rivero + + BSD,LGPL,Apache 2.0 + + http://gazebosim.org/tutorials?cat=connect_ros + https://github.com/ros-simulation/gazebo_ros_pkgs/issues + https://github.com/ros-simulation/gazebo_ros_pkgs + + John Hsu, Nate Koenig, Dave Coleman> + + catkin + + gazebo_dev + gazebo_msgs + gazebo_plugins + gazebo_ros + + + + +