From 88e9dceba7569c0ff2109a907cc925e70cc56cce Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Tue, 26 May 2020 09:41:12 -0700 Subject: [PATCH] Remove unported gazebo_ros_control Signed-off-by: Louise Poubel --- .../gazebo_ros_control/CHANGELOG.rst | 396 -------------- .../gazebo_ros_control/CMakeLists.txt | 81 --- .ros1_unported/gazebo_ros_control/README.md | 13 - .../gazebo_ros_control/default_robot_hw_sim.h | 147 ----- .../gazebo_ros_control_plugin.h | 131 ----- .../include/gazebo_ros_control/robot_hw_sim.h | 119 ---- .ros1_unported/gazebo_ros_control/package.xml | 36 -- .../robot_hw_sim_plugins.xml | 11 - .../src/default_robot_hw_sim.cpp | 514 ------------------ .../src/gazebo_ros_control_plugin.cpp | 313 ----------- 10 files changed, 1761 deletions(-) delete mode 100644 .ros1_unported/gazebo_ros_control/CHANGELOG.rst delete mode 100644 .ros1_unported/gazebo_ros_control/CMakeLists.txt delete mode 100644 .ros1_unported/gazebo_ros_control/README.md delete mode 100644 .ros1_unported/gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h delete mode 100644 .ros1_unported/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h delete mode 100644 .ros1_unported/gazebo_ros_control/include/gazebo_ros_control/robot_hw_sim.h delete mode 100644 .ros1_unported/gazebo_ros_control/package.xml delete mode 100644 .ros1_unported/gazebo_ros_control/robot_hw_sim_plugins.xml delete mode 100644 .ros1_unported/gazebo_ros_control/src/default_robot_hw_sim.cpp delete mode 100644 .ros1_unported/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp diff --git a/.ros1_unported/gazebo_ros_control/CHANGELOG.rst b/.ros1_unported/gazebo_ros_control/CHANGELOG.rst deleted file mode 100644 index 95d95b4b3..000000000 --- a/.ros1_unported/gazebo_ros_control/CHANGELOG.rst +++ /dev/null @@ -1,396 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package gazebo_ros_control -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.8.4 (2018-07-06) ------------------- - -2.8.3 (2018-06-04) ------------------- -* Remove legacy in gazebo_ros_control for robotNamespace (`#709 `_) - See pull request `#637 `_ -* Contributors: Jose Luis Rivero - -2.8.2 (2018-05-09) ------------------- -* Fix the build on Ubuntu Artful. (`#715 `_) - Artful has some bugs in its cmake files for Simbody that - cause it to fail the build. If we are on artful, remove - the problematic entries. - Signed-off-by: Chris Lalancette -* Contributors: Chris Lalancette - -2.8.1 (2018-05-05) ------------------- -* Don't ignore robotNamespace in gazebo_ros_control nodes (lunar-devel) (`#706 `_) - This commit restores the intended behavior, i.e., the parameters will now read from /..., where is specified via the robotNamespace plugin parameter or the parent name. -* add physics type for dart with joint velocity interface (`#701 `_) -* Contributors: Jose Luis Rivero - -2.7.4 (2018-02-12) ------------------- -* Fix last gazebo8 warnings! (lunar-devel) (`#664 `_) -* Fix gazebo8 warnings part 7: retry `#642 `_ on lunar (`#660 `_) -* Contributors: Jose Luis Rivero, Steven Peters - -2.7.3 (2017-12-11) ------------------- -* Replace Events::Disconnect* with pointer reset (`#626 `_) -* Contributors: Jose Luis Rivero - -2.7.2 (2017-05-21) ------------------- -* Revert gazebo8 changes in Lunar and back to use gazebo7 (`#583 `_) -* Contributors: Jose Luis Rivero - -2.7.1 (2017-04-28) ------------------- -* Fixes for compilation and warnings in Lunar-devel (`#573 `_) - Multiple fixes for compilation and warnings coming from Gazebo8 and ignition-math3 -* Less exciting console output (`#561 `_) -* Add catkin package(s) to provide the default version of Gazebo - take II (kinetic-devel) (`#571 `_) -* 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/.ros1_unported/gazebo_ros_control/CMakeLists.txt b/.ros1_unported/gazebo_ros_control/CMakeLists.txt deleted file mode 100644 index cdb6e773e..000000000 --- a/.ros1_unported/gazebo_ros_control/CMakeLists.txt +++ /dev/null @@ -1,81 +0,0 @@ -cmake_minimum_required(VERSION 3.6.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 -) - -# Through transitive dependencies in the packages above, gazebo_ros_control -# depends on Simbody. There is a bug in the Ubuntu Artful (17.10) version of -# the Simbody package where it includes /usr/lib/libblas.so and -# /usr/lib/liblapack.so in the CMake list of libraries even though neither of -# those two paths exist (they both really live in /usr/lib/-linux-gnu). -# We remove these two during build-time on artful below; this works because -# they both will get resolved to the proper paths during runtime linking. -find_program(LSB_RELEASE_EXEC lsb_release) -if(NOT LSB_RELEASE_EXEC STREQUAL "LSB_RELEASE_EXEC-NOTFOUND") - execute_process(COMMAND ${LSB_RELEASE_EXEC} -cs - OUTPUT_VARIABLE OS_CODENAME - OUTPUT_STRIP_TRAILING_WHITESPACE - ) - if(OS_CODENAME STREQUAL "artful") - list(FILTER catkin_LIBRARIES EXCLUDE REGEX "/usr/lib/libblas.so") - list(FILTER catkin_LIBRARIES EXCLUDE REGEX "/usr/lib/liblapack.so") - endif() -endif() - -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/.ros1_unported/gazebo_ros_control/README.md b/.ros1_unported/gazebo_ros_control/README.md deleted file mode 100644 index 90c989b07..000000000 --- a/.ros1_unported/gazebo_ros_control/README.md +++ /dev/null @@ -1,13 +0,0 @@ -# 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/.ros1_unported/gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h b/.ros1_unported/gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h deleted file mode 100644 index acfd10e8b..000000000 --- a/.ros1_unported/gazebo_ros_control/include/gazebo_ros_control/default_robot_hw_sim.h +++ /dev/null @@ -1,147 +0,0 @@ -/********************************************************************* - * 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/.ros1_unported/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h b/.ros1_unported/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h deleted file mode 100644 index 0c11eafac..000000000 --- a/.ros1_unported/gazebo_ros_control/include/gazebo_ros_control/gazebo_ros_control_plugin.h +++ /dev/null @@ -1,131 +0,0 @@ -/********************************************************************* - * 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/.ros1_unported/gazebo_ros_control/include/gazebo_ros_control/robot_hw_sim.h b/.ros1_unported/gazebo_ros_control/include/gazebo_ros_control/robot_hw_sim.h deleted file mode 100644 index 4e9144752..000000000 --- a/.ros1_unported/gazebo_ros_control/include/gazebo_ros_control/robot_hw_sim.h +++ /dev/null @@ -1,119 +0,0 @@ -/********************************************************************* - * 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/.ros1_unported/gazebo_ros_control/package.xml b/.ros1_unported/gazebo_ros_control/package.xml deleted file mode 100644 index 47e24e6cc..000000000 --- a/.ros1_unported/gazebo_ros_control/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - - gazebo_ros_control - 2.8.4 - 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/.ros1_unported/gazebo_ros_control/robot_hw_sim_plugins.xml b/.ros1_unported/gazebo_ros_control/robot_hw_sim_plugins.xml deleted file mode 100644 index 5034ee101..000000000 --- a/.ros1_unported/gazebo_ros_control/robot_hw_sim_plugins.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - A default robot simulation interface which constructs joint handles from an SDF/URDF. - - - diff --git a/.ros1_unported/gazebo_ros_control/src/default_robot_hw_sim.cpp b/.ros1_unported/gazebo_ros_control/src/default_robot_hw_sim.cpp deleted file mode 100644 index a58e358ce..000000000 --- a/.ros1_unported/gazebo_ros_control/src/default_robot_hw_sim.cpp +++ /dev/null @@ -1,514 +0,0 @@ -/********************************************************************* - * 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(robot_namespace + "/gazebo_ros_control/pid_gains/" + - joint_names_[j]); - if (pid_controllers_[j].init(nh)) - { - 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 - 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/.ros1_unported/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp b/.ros1_unported/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp deleted file mode 100644 index 26319e1db..000000000 --- a/.ros1_unported/gazebo_ros_control/src/gazebo_ros_control_plugin.cpp +++ /dev/null @@ -1,313 +0,0 @@ -/********************************************************************* - * 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_ns, 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