From 7005501a354ed6145a4a911d3bb3a6d873f032f7 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 28 May 2020 10:23:18 +0200 Subject: [PATCH 1/8] Added initial version of gazebo_ros2_control Signed-off-by: ahcorde --- gazebo_ros2_control/CHANGELOG.rst | 394 +++++++++++ gazebo_ros2_control/CMakeLists.txt | 64 ++ gazebo_ros2_control/README.md | 13 + .../default_robot_hw_sim.h | 173 +++++ .../gazebo_ros2_control_plugin.h | 131 ++++ .../gazebo_ros2_control/robot_hw_sim.h | 124 ++++ gazebo_ros2_control/package.xml | 37 + gazebo_ros2_control/robot_hw_sim_plugins.xml | 11 + .../src/default_robot_hw_sim.cpp | 661 ++++++++++++++++++ .../src/gazebo_ros2_control_plugin.cpp | 508 ++++++++++++++ 10 files changed, 2116 insertions(+) create mode 100644 gazebo_ros2_control/CHANGELOG.rst create mode 100644 gazebo_ros2_control/CMakeLists.txt create mode 100644 gazebo_ros2_control/README.md create mode 100644 gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.h create mode 100644 gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.h create mode 100644 gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.h create mode 100644 gazebo_ros2_control/package.xml create mode 100644 gazebo_ros2_control/robot_hw_sim_plugins.xml create mode 100644 gazebo_ros2_control/src/default_robot_hw_sim.cpp create mode 100644 gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst new file mode 100644 index 00000000..1c94381f --- /dev/null +++ b/gazebo_ros2_control/CHANGELOG.rst @@ -0,0 +1,394 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package gazebo_ros_control +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.5.19 (2019-06-04) +------------------- + +2.5.18 (2019-01-23) +------------------- +* Add unified velocity hardware interface support for other physics engines except ODE +* Add velocity interface support for bullet and dart physics engines + (`#790 `) +* Don't ignore robotNamespace (`#637 `_) + When creating the NodeHandle for reading the PID parameters, the model_nh was always ignored. +* Add physics type for dart with joint velocity interface +* Contributors: Jack Liu, Kevin Allen, Santiago Focke + +2.5.17 (2018-06-07) +------------------- + +2.5.16 (2018-06-04) +------------------- +* add physics type for dart with joint velocity interface (`#693 `_) +* Add warnings when the user is affected by gazebo not preserving world velocity when set positions (`#691 `_) + Issue `#612 `_. Workaround at https://github.com/mintar/mimic_joint_gazebo_tutorial +* Fix for preserving world velocity when set positions for Gazebo9: `#612 `_ + This commit fixes `#612 `_, but only for Gazebo9. + Fixing it for Gazebo7 (the version used in ROS Kinetic) requires the + following PR to be backported to Gazebo 7 and 8: + https://bitbucket.org/osrf/gazebo/pull-requests/2814/fix-issue-2111-by-providing-options-to/diff + +* Contributors: Jack Liu, Martin Günther + +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_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt new file mode 100644 index 00000000..ad599921 --- /dev/null +++ b/gazebo_ros2_control/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.5.0) +project(gazebo_ros2_control) + +find_package(ament_cmake REQUIRED) +find_package(angles REQUIRED) +find_package(controller_manager REQUIRED) +find_package(gazebo_dev REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(joint_limits_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(transmission_interface REQUIRED) +find_package(urdf REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +include_directories(include) + +# Libraries +add_library(${PROJECT_NAME} SHARED src/gazebo_ros2_control_plugin.cpp) +ament_target_dependencies(${PROJECT_NAME} + controller_manager + gazebo_dev + hardware_interface + pluginlib + rclcpp + transmission_interface + urdf + yaml_cpp_vendor +) + +add_library(default_robot_hw_sim SHARED src/default_robot_hw_sim.cpp) +ament_target_dependencies(default_robot_hw_sim + gazebo_dev + rclcpp + joint_limits_interface + transmission_interface + hardware_interface + angles +) + +## Install +install(TARGETS default_robot_hw_sim ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_dependencies(ament_cmake) +ament_export_dependencies(rclcpp) +ament_export_libraries(${PROJECT_NAME} default_robot_hw_sim) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} +) + +pluginlib_export_plugin_description_file(gazebo_ros2_control robot_hw_sim_plugins.xml) + +ament_package() diff --git a/gazebo_ros2_control/README.md b/gazebo_ros2_control/README.md new file mode 100644 index 00000000..90c989b0 --- /dev/null +++ b/gazebo_ros2_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_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.h b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.h new file mode 100644 index 00000000..359ebcdf --- /dev/null +++ b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.h @@ -0,0 +1,173 @@ +/********************************************************************* + * 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 +#if 0 //@todo +#include +#endif +#if 0 //@todo +#include +#include +#endif + +#include +#include +#include +#include + +// Gazebo +#include +#include +#include + +// ROS +#include +#include +#include + +// gazebo_ros_control +#include + +// URDF +#include + + + +namespace gazebo_ros2_control +{ + +class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim +{ +public: + + virtual bool initSim( + const std::string& robot_namespace, + rclcpp::Node::SharedPtr& model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model *const urdf_model, + std::vector transmissions); + + virtual void readSim(rclcpp::Time time, rclcpp::Duration period); + + virtual void writeSim(rclcpp::Time time, rclcpp::Duration period); + + virtual void eStopActive(const bool active); + + // Methods used to control a joint. + enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID}; +protected: + + // 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::JointStateHandle& joint_handle, + const ControlMethod ctrl_method, + const rclcpp::Node::SharedPtr& 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, + double *const vel_limit); + + unsigned int n_dof_; + +#if 0 //@todo + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + hardware_interface::VelocityJointInterface vj_interface_; + hardware_interface::PositionJointInterface pj_interface_; +#endif +#if 0 //@todo + 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_; +#endif + 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_vel_limits_; + std::vector joint_control_methods_; +#if 0 + std::vector pid_controllers_; +#endif + 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 joint_opmodes_; + + std::vector sim_joints_; + + std::vector joint_states_; + std::vector joint_cmds_; + std::vector joint_eff_cmdhandle_; + std::vector joint_vel_cmdhandle_; + std::vector joint_opmodehandles_; + + //limits + std::vector joint_pos_limit_handles_; + std::vector joint_pos_soft_limit_handles_; + std::vector joint_eff_limit_handles_; + std::vector joint_eff_soft_limit_handles_; + std::vector joint_vel_limit_handles_; + //std::vector joint_vel_soft_limit_handles_; + + 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_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.h b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.h new file mode 100644 index 00000000..f90d802c --- /dev/null +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_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 +*/ + +// ROS +#include +#include +#include + +// Gazebo +#include +#include +#include + +// ros_control +#include +#include +#include + +namespace gazebo_ros2_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::shared_ptr e_stop_active); + + // Node Handles + rclcpp::Node::SharedPtr 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_; + std::string param_file; + + // Transmissions in this plugin's scope + std::vector transmissions_; + + // Robot simulator interface + std::string robot_hw_sim_type_str_; + std::shared_ptr robot_hw_sim_; + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + std::thread thread_executor_spin_; //executor_->spin causes lockups, us ethis alternative for now + + // Controller manager + std::shared_ptr controller_manager_; + std::vector> controllers_; + + // Timing + rclcpp::Duration control_period_ = rclcpp::Duration(0); + rclcpp::Time last_update_sim_time_ros_; + rclcpp::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_; + rclcpp::Subscription::SharedPtr e_stop_sub_; // Emergency stop subscriber + +}; + + +} diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.h b/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.h new file mode 100644 index 00000000..01cbfa09 --- /dev/null +++ b/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.h @@ -0,0 +1,124 @@ +/********************************************************************* + * 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_ros2_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::RobotHardware + { + 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, + rclcpp::Node::SharedPtr& 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(rclcpp::Time time, rclcpp::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(rclcpp::Time time, rclcpp::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) {} + + //@note: Avoid using these functions! ros2_control was not built with gazebo_ros_control + //in mind, keep to using readSim and writeSim so we don't have to update the documents for now + virtual hardware_interface::hardware_interface_ret_t init() { } + virtual hardware_interface::hardware_interface_ret_t read() { } + virtual hardware_interface::hardware_interface_ret_t write() { } + }; + +} + +#endif // ifndef __ROS_CONTROL_GAZEBO_ROBOT_HW_SIM_H diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml new file mode 100644 index 00000000..4a075110 --- /dev/null +++ b/gazebo_ros2_control/package.xml @@ -0,0 +1,37 @@ + + + gazebo_ros2_control + 0.0.1 + gazebo_ros2_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 + + ament_cmake + + gazebo_dev + gazebo_ros + rclcpp + std_msgs + control_toolbox + controller_manager + pluginlib + hardware_interface + transmission_interface + joint_limits_interface + urdf + angles + + + ament_cmake + + + diff --git a/gazebo_ros2_control/robot_hw_sim_plugins.xml b/gazebo_ros2_control/robot_hw_sim_plugins.xml new file mode 100644 index 00000000..1787efad --- /dev/null +++ b/gazebo_ros2_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_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp new file mode 100644 index 00000000..fe13a7c6 --- /dev/null +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -0,0 +1,661 @@ +/********************************************************************* + * 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_ros2_control +{ +static const rclcpp::Logger LOGGER = rclcpp::get_logger("default_robot_hw_sim"); + +bool DefaultRobotHWSim::initSim( + const std::string& robot_namespace, + rclcpp::Node::SharedPtr& model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model *const urdf_model, + std::vector transmissions) +{ + std::cerr << "DefaultRobotHWSim::InitSim " << std::endl; + + // 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". + rclcpp::Node::SharedPtr& 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_vel_limits_.resize(n_dof_); + joint_control_methods_.resize(n_dof_); +#if 0 + pid_controllers_.resize(n_dof_); +#endif + 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_); + joint_opmodes_.resize(n_dof_); + + joint_states_.resize(n_dof_); + joint_cmds_.resize(n_dof_); + joint_opmodehandles_.resize(n_dof_); + joint_eff_cmdhandle_.resize(n_dof_); + joint_vel_cmdhandle_.resize(n_dof_); + + joint_pos_limit_handles_.resize(n_dof_); + joint_pos_soft_limit_handles_.resize(n_dof_); + joint_eff_limit_handles_.resize(n_dof_); + joint_eff_soft_limit_handles_.resize(n_dof_); + joint_vel_limit_handles_.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_.empty()) + { + std::cerr << "Transmission " << transmissions[j].name_ + << " has no associated joints."; + continue; + } + else if(transmissions[j].joints_.size() > 1) + { + std::cerr << "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_; + std::cerr << "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()) + { + std::cerr << "Joint " << transmissions[j].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) + { + std::cerr << "Joint " << transmissions[j].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 + std::cerr << "Loading joint '" << joint_names_[j] + << "' of type '" << hardware_interface << "'" << std::endl; + + // Create joint state interface for all joints +#if 0 //@todo + js_interface_.registerHandle(hardware_interface::JointStateHandle( + joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); +#endif + + // Decide what kind of command interface this actuator/joint has + hardware_interface::JointStateHandle joint_handle; + if(hardware_interface == "EffortJointInterface" || hardware_interface == "hardware_interface/EffortJointInterface") + { + // Create effort joint interface + joint_control_methods_[j] = EFFORT; +#if 0 //@todo + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_effort_command_[j]); + + ej_interface_.registerHandle(joint_handle); +#endif + } + else if(hardware_interface == "PositionJointInterface" || hardware_interface == "hardware_interface/PositionJointInterface") + { + // Create position joint interface + joint_control_methods_[j] = POSITION; +#if 0 //@todo + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_position_command_[j]); + pj_interface_.registerHandle(joint_handle); +#endif + } + else if(hardware_interface == "VelocityJointInterface" || hardware_interface == "hardware_interface/VelocityJointInterface") + { + // Create velocity joint interface + joint_control_methods_[j] = VELOCITY; + +#if 0 //@todo + joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), + &joint_velocity_command_[j]); + vj_interface_.registerHandle(joint_handle); +#endif + } + else + { + std::cerr << "No matching hardware interface found for '" + << hardware_interface << "' while loading interfaces for " << joint_names_[j] << std::endl; + return false; + } + if(hardware_interface == "EffortJointInterface" || hardware_interface == "PositionJointInterface" || hardware_interface == "VelocityJointInterface") { + std::cerr << "Deprecated syntax, please prepend 'hardware_interface/' to '" << hardware_interface << "' within the tag in joint '" << joint_names_[j] << "'." << std::endl; + } + + // Get the gazebo joint that corresponds to the robot joint. + //RCLCPP_DEBUG_STREAM(LOGGER, "Getting pointer to gazebo joint: " + // << joint_names_[j]); + gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); + if (!joint) + { + std::cerr << "This robot has a joint named \"" << joint_names_[j] + << "\" which is not in the gazebo model." << std::endl; + return false; + } + sim_joints_.push_back(joint); + joint_position_[j] = joint->Position(0); + joint_velocity_[j] = joint->GetVelocity(0); + + // 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()) + { + std::cerr << "No physics type found." << std::endl; + } + + 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], &joint_vel_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. +#if 0 + const ros::NodeHandle nh(robot_namespace + "//pid_gains/" + + joint_names_[j]); +#endif +#if 0 + 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 +#endif + { + // 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 + } + + } + joint_states_[j] = hardware_interface::JointStateHandle( + joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]); + if (register_joint_state_handle(&joint_states_[j]) != hardware_interface::HW_RET_OK) { + RCLCPP_WARN_ONCE(LOGGER,"cant register jointstatehandle"); + } + + joint_cmds_[j] = hardware_interface::JointCommandHandle( + joint_names_[j], &joint_position_command_[j]); + if (register_joint_command_handle(&joint_cmds_[j]) != hardware_interface::HW_RET_OK) { + RCLCPP_WARN_ONCE(LOGGER,"cant register jointcommandhandle"); + } + + joint_opmodehandles_[j] = hardware_interface::OperationModeHandle( + joint_names_[j], &joint_opmodes_[j]); + if (register_operation_mode_handle(&joint_opmodehandles_[j]) != hardware_interface::HW_RET_OK) { + RCLCPP_WARN_ONCE(LOGGER,"cant register jointopmodehandle"); + } + + joint_limits_interface::JointLimits limits; //hack, refactor registerjointhandle + limits.has_position_limits = true; + limits.min_position = joint_lower_limits_[j]; + limits.max_position = joint_upper_limits_[j]; + limits.max_velocity = joint_vel_limits_[j]; + limits.has_velocity_limits = true; + limits.max_effort = joint_effort_limits_[j]; + limits.has_effort_limits = true; + + joint_pos_limit_handles_[j] = joint_limits_interface::PositionJointSaturationHandle( + joint_states_[j], joint_cmds_[j], limits); + + joint_eff_cmdhandle_[j] = hardware_interface::JointCommandHandle( + joint_names_[j], &joint_effort_command_[j]); + //should be register_joint_effort_command_handle, but there's only 1 buffer for now + // ALEX + if (register_joint_command_handle(&joint_eff_cmdhandle_[j]) != hardware_interface::HW_RET_OK) { + RCLCPP_WARN_ONCE(LOGGER,"cant register jointcommandhandle"); + } + + joint_eff_limit_handles_[j] = joint_limits_interface::EffortJointSaturationHandle( + joint_states_[j], joint_eff_cmdhandle_[j], limits); + + joint_vel_cmdhandle_[j] = hardware_interface::JointCommandHandle( + joint_names_[j], &joint_velocity_command_[j]); + if (register_joint_command_handle(&joint_vel_cmdhandle_[j]) != hardware_interface::HW_RET_OK) { + std::cerr << "cant register jointcommandhandle" << std::endl; + } + + joint_vel_limit_handles_[j] = joint_limits_interface::VelocityJointSaturationHandle( + joint_states_[j], joint_vel_cmdhandle_[j], limits); + //register not implemented + } +#if 0 //@todo + // Register interfaces + registerInterface(&js_interface_); + registerInterface(&ej_interface_); + registerInterface(&pj_interface_); + registerInterface(&vj_interface_); +#endif + // Initialize the emergency stop code. + e_stop_active_ = false; + last_e_stop_active_ = false; + + return true; +} + +void DefaultRobotHWSim::readSim(rclcpp::Time time, rclcpp::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(rclcpp::Time time, rclcpp::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; + } +#if 0 //@todo + 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); +#else + for (auto eff_limit_handle : joint_eff_limit_handles_) + { + eff_limit_handle.enforceLimits(period); + } + for (auto pos_limit_handle : joint_pos_limit_handles_) + { + pos_limit_handle.enforceLimits(period); + } + for (auto vel_limit_handle : joint_vel_limit_handles_) + { + vel_limit_handle.enforceLimits(period); + } + +#endif + 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 + RCLCPP_WARN_ONCE(LOGGER,"The default_robot_hw_sim plugin is using the Joint::SetPosition method without preserving the link velocity."); + RCLCPP_WARN_ONCE(LOGGER,"As a result, gravity will not be simulated correctly for your model."); + RCLCPP_WARN_ONCE(LOGGER,"Please set gazebo_pid parameters, switch to the VelocityJointInterface or EffortJointInterface, or upgrade to Gazebo 9."); + RCLCPP_WARN_ONCE(LOGGER,"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]; +#if 0 //@todo + const double effort = clamp(pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); +#else + const double effort = 0.0; +#endif + sim_joints_[j]->SetForce(0, effort); + } + break; + + case VELOCITY: +#if GAZEBO_MAJOR_VERSION > 2 +// std::cout << "joint_velocity_command_[j]: " << joint_velocity_command_[j] << std::endl; + if (physics_type_.compare("ode") == 0) + { + 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]); + } +#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]; +#if 0 //@todo + const double effort = clamp(pid_controllers_[j].computeCommand(error, period), + -effort_limit, effort_limit); +#else + const double effort = 0.0; +#endif + 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::JointStateHandle& joint_handle, + const ControlMethod ctrl_method, + const rclcpp::Node::SharedPtr& 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, + double *const vel_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->name, joint_limit_nh, limits)) + has_limits = true; + if (joint_limits_interface::getSoftJointLimits(urdf_joint->name, joint_limit_nh, soft_limits)) + has_soft_limits = true; + + //@note (ddeng): these joint limits arent input into the node, so fetch them from the urdf + limits.min_position = urdf_joint->limits->lower; + limits.max_position = urdf_joint->limits->upper; + limits.has_position_limits = true; + + limits.max_velocity = urdf_joint->limits->velocity; + limits.has_velocity_limits = limits.max_velocity == 0.0 ? false : true; + + limits.max_effort = urdf_joint->limits->effort; + limits.has_effort_limits = limits.max_effort == 0.0 ? false : true; + + *lower_limit = limits.min_position; + *upper_limit = limits.max_position; + *effort_limit = limits.max_effort; + *vel_limit = limits.max_velocity; + + //urdf_joint->safety->k_position; + has_limits = true; + } + } + + // Get limits from the parameter server. + //@note: no longer using parameter servers + 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: + { +#if 0 //@todo + const joint_limits_interface::EffortJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + ej_limits_interface_.registerHandle(limits_handle); +#endif + } + break; + case POSITION: + { +#if 0 //@todo + const joint_limits_interface::PositionJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + pj_limits_interface_.registerHandle(limits_handle); +#endif + } + break; + case VELOCITY: + { +#if 0 //@todo + const joint_limits_interface::VelocityJointSoftLimitsHandle + limits_handle(joint_handle, limits, soft_limits); + vj_limits_interface_.registerHandle(limits_handle); +#endif + } + break; + } + } + else + { + switch (ctrl_method) + { + case EFFORT: + { +#if 0 + const joint_limits_interface::EffortJointSaturationHandle + sat_handle(joint_handle, limits); + + ej_sat_interface_.registerHandle(sat_handle); +#endif + } + break; + case POSITION: + { +#if 0 //@todo + const joint_limits_interface::PositionJointSaturationHandle + sat_handle(joint_handle, limits); + + pj_sat_interface_.registerHandle(sat_handle); +#endif + } + break; + case VELOCITY: + { +#if 0 //@todo + const joint_limits_interface::VelocityJointSaturationHandle + sat_handle(joint_handle, limits); + vj_sat_interface_.registerHandle(sat_handle); +#endif + } + break; + } + } +} + +} + +PLUGINLIB_EXPORT_CLASS(gazebo_ros2_control::DefaultRobotHWSim, gazebo_ros2_control::RobotHWSim) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp new file mode 100644 index 00000000..af1dee5d --- /dev/null +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -0,0 +1,508 @@ +/********************************************************************* + * 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 +*/ + +#include +#include + +#include +#include "yaml-cpp/yaml.h" + +namespace gazebo_ros2_control +{ + +GazeboRosControlPlugin::~GazeboRosControlPlugin() +{ + // Disconnect from gazebo events + update_connection_.reset(); +} + +// Overloaded Gazebo entry point +void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) +{ + RCLCPP_INFO_STREAM(rclcpp::get_logger("gazebo_ros2_control"), "Loading gazebo_ros2_control plugin"); + + + // Save pointers to the model + parent_model_ = parent; + sdf_ = sdf; + + // Error message if the model couldn't be found + if (!parent_model_) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadThread"),"parent model is NULL"); + return; + } + + // Check that ROS has been initialized + if(!rclcpp::ok()) + { + RCLCPP_FATAL_STREAM(rclcpp::get_logger("gazebo_ros2_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_ros2_control/DefaultRobotHWSim"; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("loadThread"),"Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<HasElement("legacyModeNS")) { + if( sdf_->GetElement("legacyModeNS")->Get() ){ + robot_ns = ""; + } + }else{ + robot_ns = ""; + RCLCPP_ERROR(rclcpp::get_logger("loadThread"),"GazeboRosControlPlugin missing while using DefaultRobotHWSim, defaults to true.\n" + "This setting assumes you have an old package with an old implementation of DefaultRobotHWSim, " + "where the robotNamespace is disregarded and absolute paths are used instead.\n" + "If you do not want to fix this issue in an old package just set to true.\n" + ); + } + } + + if (sdf_->HasElement("parameters")) { + param_file = sdf_->GetElement("parameters")->Get(); + } else { + RCLCPP_ERROR(rclcpp::get_logger("loadThread"), "No parameter file provided. Configuration might be wrong"); + } + + // Get the Gazebo simulation period +#if GAZEBO_MAJOR_VERSION >= 8 + rclcpp::Duration gazebo_period(parent_model_->GetWorld()->Physics()->GetMaxStepSize()); +#else + rclcpp::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize()); +#endif + + // Decide the plugin control period + if(sdf_->HasElement("controlPeriod")) + { + control_period_ = rclcpp::Duration(sdf_->Get("controlPeriod")); + + // Check the period against the simulation period + if( control_period_ < gazebo_period ) + { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("gazebo_ros2_control"),"Desired controller update period ("< gazebo_period ) + { + RCLCPP_WARN_STREAM(rclcpp::get_logger("gazebo_ros2_control")," Desired controller update period ("<< control_period_.seconds() + <<" s) is slower than the gazebo simulation period ("<< gazebo_period.seconds() <<" s)."); + } + } + else + { + control_period_ = gazebo_period; + RCLCPP_DEBUG_STREAM(rclcpp::get_logger("gazebo_ros2_control"),"Control period not found in URDF/SDF, defaulting to Gazebo period of " + << control_period_.seconds()); + } + + // Get parameters/settings for controllers from ROS param server + model_nh_ = std::make_shared(parent_model_->GetName(), robot_namespace_); + + // Initialize the emergency stop code. + e_stop_active_ = false; + last_e_stop_active_ = false; + if (sdf_->HasElement("eStopTopic")) + { + using std::placeholders::_1; + const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get(); + e_stop_sub_ = model_nh_->create_subscription(e_stop_topic, 1, + std::bind(&GazeboRosControlPlugin::eStopCB, this, _1)); + } + RCLCPP_INFO(rclcpp::get_logger("gazebo_ros2_control"), "Starting gazebo_ros2_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)) + { + RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "Error parsing URDF in gazebo_ros2_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_ros2_control", + "gazebo_ros2_control::RobotHWSim")); + + robot_hw_sim_ = robot_hw_sim_loader_->createSharedInstance(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_)) + { + RCLCPP_FATAL(rclcpp::get_logger("gazebo_ros2_control"),"Could not initialize robot simulation interface"); + return; + } + + executor_ = std::make_shared(); + executor_->add_node(model_nh_); + + // Create the controller manager + RCLCPP_ERROR(rclcpp::get_logger("ros2_control_plugin"),"Loading controller_manager"); +#if 1 //@todo + controller_manager_.reset + (new controller_manager::ControllerManager(robot_hw_sim_, executor_, "gazebo_controller_manager")); +#endif +#if 1 //@todo: Coded example here. should disable when spawn functionality of controller manager is up + auto load_params_from_yaml_node = [](rclcpp_lifecycle::LifecycleNode::SharedPtr lc_node, + YAML::Node & yaml_node, const std::string& prefix) + { + std::function, const std::string& )> + feed_yaml_to_node_rec = + [&](YAML::Node yaml_node, const std::string & key, + std::shared_ptr node, const std::string& prefix) + { + if (node->get_name() != prefix) + return; + static constexpr char separator = '.'; + if (yaml_node.Type() == YAML::NodeType::Scalar) { + std::string val_str = yaml_node.as(); + //const char* val_str = yaml_node.as(); + // std::string val_str; + // YAML::convert::decode(yaml_node, val_str); + + // TODO(ddengster): Do stricter typing for set_parameter value types. + // (ie. Numbers should be converted to int/double/etc instead of strings) + //RCLCPP_ERROR(rclcpp::get_logger("load"),"key: %s val: %s", key.c_str(), val_str.c_str()); + if (!node->has_parameter(key)) + node->declare_parameter(key); + + auto is_number = [](const std::string &str) -> bool { + return str.find_first_not_of("0123456789.-") == std::string::npos; + //@note: bugs with .05 or 15. + }; + + if (is_number(val_str)) + { + std::stringstream ss(val_str); + double v = 0.0; + ss >> v; + node->set_parameter(rclcpp::Parameter(key, v)); + } + else { + node->set_parameter(rclcpp::Parameter(key, val_str)); + } + + return; + } else if (yaml_node.Type() == YAML::NodeType::Map) { + for (auto yaml_node_it : yaml_node) { + std::string newkey = yaml_node_it.first.as(); + //RCLCPP_ERROR(rclcpp::get_logger("load"), "newkey: %s", newkey.c_str()); + if (newkey == prefix || newkey == "ros__parameters") + newkey = ""; + else if (!key.empty()) + newkey = key + separator + newkey; + //RCLCPP_ERROR(rclcpp::get_logger("load"),"newkey (map): %s", newkey.c_str()); + feed_yaml_to_node_rec(yaml_node_it.second, newkey, node, prefix); + } + } else if (yaml_node.Type() == YAML::NodeType::Sequence) { + auto it = yaml_node.begin(); + if (yaml_node.size()) + { + if (it->IsScalar()) + { + //submit as array of parameters + std::vector val; + for (auto yaml_node_it : yaml_node) { + std::string name = yaml_node_it.as(); + val.push_back(name); + //RCLCPP_ERROR(rclcpp::get_logger("load"),"n: %s", name.c_str()); + } + //RCLCPP_ERROR(rclcpp::get_logger("load")," %s array_param: %s", node->get_name(), key.c_str()); + if (!node->has_parameter(key)) + node->declare_parameter(key); + node->set_parameter({rclcpp::Parameter(key, val)}); + + //node->get_parameter(key).as_string_array(); + + if (key == "joints") + { + if (!node->has_parameter("write_op_modes")) + node->declare_parameter("write_op_modes"); + node->set_parameter(rclcpp::Parameter("write_op_modes", val)); + } + } + else + { + size_t index = 0; + for (auto yaml_node_it : yaml_node) { + std::string newkey = std::to_string((index++)); + if (!key.empty()) + newkey = key + separator + newkey; + //RCLCPP_ERROR(rclcpp::get_logger("load"),"prefix_seq: %s newkey: %s", prefix.c_str(), newkey.c_str()); + feed_yaml_to_node_rec(yaml_node_it, newkey, node, prefix); + } + } + } + //RCLCPP_ERROR(rclcpp::get_logger("load")," exit"); + } + }; + if (lc_node->get_name() != prefix) + return; + feed_yaml_to_node_rec(yaml_node, prefix, lc_node, prefix); + }; + auto load_params_from_yaml = [&](rclcpp_lifecycle::LifecycleNode::SharedPtr lc_node, + const std::string & yaml_config_file, const std::string& prefix) + { + if (yaml_config_file.empty()) { + throw std::runtime_error("yaml config file path is empty"); + } + + YAML::Node root_node = YAML::LoadFile(yaml_config_file); + for (auto yaml : root_node) + { + auto nodename = yaml.first.as(); + RCLCPP_ERROR(rclcpp::get_logger("ros_control_plugin"),"nodename: %s", nodename.c_str()); + if (nodename == prefix) + load_params_from_yaml_node(lc_node, yaml.second, prefix); + } + }; + + YAML::Node root_node = YAML::LoadFile(param_file); + for (auto yaml : root_node) + { + auto controller_name = yaml.first.as(); + for (auto yaml_node_it : yaml.second) { //ros__parameters + for (auto yaml_node_it2 : yaml_node_it.second) { + auto param_name = yaml_node_it2.first.as(); + if(param_name == "type") { + auto controller_type = yaml_node_it2.second.as(); + auto controller = controller_manager_->load_controller( + controller_name, + controller_type); + controllers_.push_back(controller); + load_params_from_yaml(controller->get_lifecycle_node(), param_file, controller_name); + } + } + } + } + + auto spin = [this]() + { + while (rclcpp::ok()) + executor_->spin_once(); + }; + thread_executor_spin_ = std::thread(spin); + // there is no async spinner in ROS 2, so we have to put the spin() in its own thread + // auto future_handle = std::async(std::launch::async, spin, executor_); + + if (controller_manager_->configure() != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { + RCLCPP_ERROR(rclcpp::get_logger("cm"), "failed to configure"); + } + if (controller_manager_->activate() != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { + RCLCPP_ERROR(rclcpp::get_logger("cm"), "failed to activate"); + } + +#endif + + // 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) + { + RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"),"Failed to create robot simulation interface loader: %s ", ex.what()); + } + + RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "Loaded gazebo_ros2_control."); +} + +void +spin(std::shared_ptr exe) +{ + exe->spin(); +} + +// Called by the world update start event +void GazeboRosControlPlugin::Update() +{ + // Get the simulation time and period +#if GAZEBO_MAJOR_VERSION >= 8 + gazebo::common::Time gz_time_now = parent_model_->GetWorld()->SimTime(); +#else + gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime(); +#endif + rclcpp::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec); + rclcpp::Duration sim_period = sim_time_ros - last_update_sim_time_ros_; + + robot_hw_sim_->eStopActive(e_stop_active_); + //executor_->spin_once(); + + // 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(); + } + + // 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_ = rclcpp::Time(); + last_write_sim_time_ros_ = rclcpp::Time(); +} + +// Get the URDF XML from the parameter server +std::string GazeboRosControlPlugin::getURDF(std::string param_name) const +{ + std::string urdf_string; + + using namespace std::chrono_literals; + auto parameters_client = std::make_shared(model_nh_, "robot_state_publisher"); + while (!parameters_client->wait_for_service(0.5s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "Interrupted while waiting for the service. Exiting."); + return 0; + } + RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "service not available, waiting again..."); + } + + RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "connected to service!! /robot_state_publisher"); + + // search and wait for robot_description on param server + while (urdf_string.empty()) + { + std::string search_param_name; + RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "param_name %s", param_name.c_str()); + + urdf_string = parameters_client->get_parameter(param_name, ""); + if (!urdf_string.empty()) + break; + else + { + RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "gazebo_ros2_control plugin is waiting for model" + " URDF in parameter [%s] on the ROS param server.", search_param_name.c_str()); + } + usleep(100000); + } + RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "Recieved urdf from param server, parsing..."); + // RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "urdf_string %s", urdf_string.c_str()); + + 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::shared_ptr e_stop_active) +{ + e_stop_active_ = e_stop_active->data; +} + + +// Register this plugin with the simulator +GZ_REGISTER_MODEL_PLUGIN(GazeboRosControlPlugin); +} // namespace From 0daf5cbf3d41d5d9e64b6fbf514fb0a254e25f59 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 28 May 2020 16:03:04 +0200 Subject: [PATCH 2/8] Removed CHANGELOG.rst Signed-off-by: ahcorde --- gazebo_ros2_control/CHANGELOG.rst | 394 ------------------------------ 1 file changed, 394 deletions(-) delete mode 100644 gazebo_ros2_control/CHANGELOG.rst diff --git a/gazebo_ros2_control/CHANGELOG.rst b/gazebo_ros2_control/CHANGELOG.rst deleted file mode 100644 index 1c94381f..00000000 --- a/gazebo_ros2_control/CHANGELOG.rst +++ /dev/null @@ -1,394 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package gazebo_ros_control -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -2.5.19 (2019-06-04) -------------------- - -2.5.18 (2019-01-23) -------------------- -* Add unified velocity hardware interface support for other physics engines except ODE -* Add velocity interface support for bullet and dart physics engines - (`#790 `) -* Don't ignore robotNamespace (`#637 `_) - When creating the NodeHandle for reading the PID parameters, the model_nh was always ignored. -* Add physics type for dart with joint velocity interface -* Contributors: Jack Liu, Kevin Allen, Santiago Focke - -2.5.17 (2018-06-07) -------------------- - -2.5.16 (2018-06-04) -------------------- -* add physics type for dart with joint velocity interface (`#693 `_) -* Add warnings when the user is affected by gazebo not preserving world velocity when set positions (`#691 `_) - Issue `#612 `_. Workaround at https://github.com/mintar/mimic_joint_gazebo_tutorial -* Fix for preserving world velocity when set positions for Gazebo9: `#612 `_ - This commit fixes `#612 `_, but only for Gazebo9. - Fixing it for Gazebo7 (the version used in ROS Kinetic) requires the - following PR to be backported to Gazebo 7 and 8: - https://bitbucket.org/osrf/gazebo/pull-requests/2814/fix-issue-2111-by-providing-options-to/diff - -* Contributors: Jack Liu, Martin Günther - -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) ------------------- From 8a1edb51588c2340a74c9562a31d4b2905b2b988 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 28 May 2020 16:05:00 +0200 Subject: [PATCH 3/8] Updated README.md Signed-off-by: ahcorde --- gazebo_ros2_control/README.md | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/gazebo_ros2_control/README.md b/gazebo_ros2_control/README.md index 90c989b0..0fb544ae 100644 --- a/gazebo_ros2_control/README.md +++ b/gazebo_ros2_control/README.md @@ -1,13 +1,7 @@ # Gazebo ros_control Interfaces -This is a ROS package for integrating the `ros_control` controller architecture +This is a ROS 2 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 From a06b9fad10e6b12f4d2cc62000be0a657efa0d62 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 28 May 2020 16:12:47 +0200 Subject: [PATCH 4/8] cleanup code Signed-off-by: ahcorde --- .../src/default_robot_hw_sim.cpp | 27 +++++-------------- .../src/gazebo_ros2_control_plugin.cpp | 13 --------- 2 files changed, 6 insertions(+), 34 deletions(-) diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index fe13a7c6..eb4c4daa 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -42,16 +42,6 @@ #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_ros2_control { static const rclcpp::Logger LOGGER = rclcpp::get_logger("default_robot_hw_sim"); @@ -447,19 +437,16 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) } const double effort_limit = joint_effort_limits_[j]; -#if 0 //@todo - const double effort = clamp(pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); -#else + //TODO(anyone): Restored this when PID controllers is available + // const double effort = std::clamp(pid_controllers_[j].computeCommand(error, period), + // -effort_limit, effort_limit); const double effort = 0.0; -#endif sim_joints_[j]->SetForce(0, effort); } break; case VELOCITY: #if GAZEBO_MAJOR_VERSION > 2 -// std::cout << "joint_velocity_command_[j]: " << joint_velocity_command_[j] << std::endl; if (physics_type_.compare("ode") == 0) { sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]); @@ -480,12 +467,10 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) else error = joint_velocity_command_[j] - joint_velocity_[j]; const double effort_limit = joint_effort_limits_[j]; -#if 0 //@todo - const double effort = clamp(pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); -#else + //TODO(anyone): Restored this when PID controllers is available + // const double effort = std::clamp(pid_controllers_[j].computeCommand(error, period), + // -effort_limit, effort_limit); const double effort = 0.0; -#endif sim_joints_[j]->SetForce(0, effort); break; } diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index af1dee5d..af172c43 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -238,7 +238,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element // TODO(ddengster): Do stricter typing for set_parameter value types. // (ie. Numbers should be converted to int/double/etc instead of strings) - //RCLCPP_ERROR(rclcpp::get_logger("load"),"key: %s val: %s", key.c_str(), val_str.c_str()); if (!node->has_parameter(key)) node->declare_parameter(key); @@ -262,12 +261,10 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } else if (yaml_node.Type() == YAML::NodeType::Map) { for (auto yaml_node_it : yaml_node) { std::string newkey = yaml_node_it.first.as(); - //RCLCPP_ERROR(rclcpp::get_logger("load"), "newkey: %s", newkey.c_str()); if (newkey == prefix || newkey == "ros__parameters") newkey = ""; else if (!key.empty()) newkey = key + separator + newkey; - //RCLCPP_ERROR(rclcpp::get_logger("load"),"newkey (map): %s", newkey.c_str()); feed_yaml_to_node_rec(yaml_node_it.second, newkey, node, prefix); } } else if (yaml_node.Type() == YAML::NodeType::Sequence) { @@ -281,15 +278,11 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element for (auto yaml_node_it : yaml_node) { std::string name = yaml_node_it.as(); val.push_back(name); - //RCLCPP_ERROR(rclcpp::get_logger("load"),"n: %s", name.c_str()); } - //RCLCPP_ERROR(rclcpp::get_logger("load")," %s array_param: %s", node->get_name(), key.c_str()); if (!node->has_parameter(key)) node->declare_parameter(key); node->set_parameter({rclcpp::Parameter(key, val)}); - //node->get_parameter(key).as_string_array(); - if (key == "joints") { if (!node->has_parameter("write_op_modes")) @@ -304,12 +297,10 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element std::string newkey = std::to_string((index++)); if (!key.empty()) newkey = key + separator + newkey; - //RCLCPP_ERROR(rclcpp::get_logger("load"),"prefix_seq: %s newkey: %s", prefix.c_str(), newkey.c_str()); feed_yaml_to_node_rec(yaml_node_it, newkey, node, prefix); } } } - //RCLCPP_ERROR(rclcpp::get_logger("load")," exit"); } }; if (lc_node->get_name() != prefix) @@ -358,8 +349,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element executor_->spin_once(); }; thread_executor_spin_ = std::thread(spin); - // there is no async spinner in ROS 2, so we have to put the spin() in its own thread - // auto future_handle = std::async(std::launch::async, spin, executor_); if (controller_manager_->configure() != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { RCLCPP_ERROR(rclcpp::get_logger("cm"), "failed to configure"); @@ -403,7 +392,6 @@ void GazeboRosControlPlugin::Update() rclcpp::Duration sim_period = sim_time_ros - last_update_sim_time_ros_; robot_hw_sim_->eStopActive(e_stop_active_); - //executor_->spin_once(); // Check if we should update the controllers if(sim_period >= control_period_) { @@ -484,7 +472,6 @@ std::string GazeboRosControlPlugin::getURDF(std::string param_name) const usleep(100000); } RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "Recieved urdf from param server, parsing..."); - // RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "urdf_string %s", urdf_string.c_str()); return urdf_string; } From 1dc21f8275a5b3085df38d32b9d3c9a6b8613662 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 28 May 2020 19:32:51 +0200 Subject: [PATCH 5/8] Added common linters Signed-off-by: ahcorde --- gazebo_ros2_control/CMakeLists.txt | 6 + .../default_robot_hw_sim.h | 173 ------ .../default_robot_hw_sim.hpp | 169 ++++++ .../gazebo_ros2_control_plugin.h | 131 ----- .../gazebo_ros2_control_plugin.hpp | 132 +++++ .../gazebo_ros2_control/robot_hw_sim.h | 124 ----- .../gazebo_ros2_control/robot_hw_sim.hpp | 124 +++++ gazebo_ros2_control/package.xml | 4 + .../src/default_robot_hw_sim.cpp | 445 ++++++++------- .../src/gazebo_ros2_control_plugin.cpp | 525 +++++++++--------- 10 files changed, 926 insertions(+), 907 deletions(-) delete mode 100644 gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.h create mode 100644 gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp delete mode 100644 gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.h create mode 100644 gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp delete mode 100644 gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.h create mode 100644 gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp diff --git a/gazebo_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt index ad599921..93be9710 100644 --- a/gazebo_ros2_control/CMakeLists.txt +++ b/gazebo_ros2_control/CMakeLists.txt @@ -55,6 +55,12 @@ ament_export_dependencies(ament_cmake) ament_export_dependencies(rclcpp) ament_export_libraries(${PROJECT_NAME} default_robot_hw_sim) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME} ) diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.h b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.h deleted file mode 100644 index 359ebcdf..00000000 --- a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.h +++ /dev/null @@ -1,173 +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 -#if 0 //@todo -#include -#endif -#if 0 //@todo -#include -#include -#endif - -#include -#include -#include -#include - -// Gazebo -#include -#include -#include - -// ROS -#include -#include -#include - -// gazebo_ros_control -#include - -// URDF -#include - - - -namespace gazebo_ros2_control -{ - -class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim -{ -public: - - virtual bool initSim( - const std::string& robot_namespace, - rclcpp::Node::SharedPtr& model_nh, - gazebo::physics::ModelPtr parent_model, - const urdf::Model *const urdf_model, - std::vector transmissions); - - virtual void readSim(rclcpp::Time time, rclcpp::Duration period); - - virtual void writeSim(rclcpp::Time time, rclcpp::Duration period); - - virtual void eStopActive(const bool active); - - // Methods used to control a joint. - enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID}; -protected: - - // 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::JointStateHandle& joint_handle, - const ControlMethod ctrl_method, - const rclcpp::Node::SharedPtr& 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, - double *const vel_limit); - - unsigned int n_dof_; - -#if 0 //@todo - hardware_interface::JointStateInterface js_interface_; - hardware_interface::EffortJointInterface ej_interface_; - hardware_interface::VelocityJointInterface vj_interface_; - hardware_interface::PositionJointInterface pj_interface_; -#endif -#if 0 //@todo - 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_; -#endif - 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_vel_limits_; - std::vector joint_control_methods_; -#if 0 - std::vector pid_controllers_; -#endif - 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 joint_opmodes_; - - std::vector sim_joints_; - - std::vector joint_states_; - std::vector joint_cmds_; - std::vector joint_eff_cmdhandle_; - std::vector joint_vel_cmdhandle_; - std::vector joint_opmodehandles_; - - //limits - std::vector joint_pos_limit_handles_; - std::vector joint_pos_soft_limit_handles_; - std::vector joint_eff_limit_handles_; - std::vector joint_eff_soft_limit_handles_; - std::vector joint_vel_limit_handles_; - //std::vector joint_vel_soft_limit_handles_; - - 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_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp new file mode 100644 index 00000000..ba0be2b6 --- /dev/null +++ b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp @@ -0,0 +1,169 @@ +// Copyright (c) 2013, Open Source Robotics Foundation. All rights reserved. +// 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 HOLDER 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_ROS2_CONTROL__DEFAULT_ROBOT_HW_SIM_HPP_ +#define GAZEBO_ROS2_CONTROL__DEFAULT_ROBOT_HW_SIM_HPP_ + +#include +#include + +// ros_control +#if 0 // @todo +#include +#endif +#if 0 // @todo +#include +#include +#endif + +#include "joint_limits_interface/joint_limits.hpp" +#include "joint_limits_interface/joint_limits_interface.hpp" +#include "joint_limits_interface/joint_limits_rosparam.hpp" +#include "joint_limits_interface/joint_limits_urdf.hpp" + +// Gazebo +#include "gazebo/common/common.hh" +#include "gazebo/physics/physics.hh" +#include "gazebo/gazebo.hh" + +// ROS +#include "rclcpp/rclcpp.hpp" +#include "angles/angles.h" +#include "pluginlib/class_list_macros.hpp" + +// gazebo_ros_control +#include "gazebo_ros2_control/robot_hw_sim.hpp" + +// URDF +#include "urdf/model.h" + +namespace gazebo_ros2_control +{ + +class DefaultRobotHWSim : public gazebo_ros2_control::RobotHWSim +{ +public: + virtual bool initSim( + const std::string & robot_namespace, + rclcpp::Node::SharedPtr & model_nh, + gazebo::physics::ModelPtr parent_model, + const urdf::Model * const urdf_model, + std::vector transmissions); + + virtual void readSim(rclcpp::Time time, rclcpp::Duration period); + + virtual void writeSim(rclcpp::Time time, rclcpp::Duration period); + + virtual void eStopActive(const bool active); + + // Methods used to control a joint. + enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID}; + +protected: + // 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::JointStateHandle & joint_handle, + const ControlMethod ctrl_method, + const rclcpp::Node::SharedPtr & 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, + double * const vel_limit); + + unsigned int n_dof_; + +#if 0 // @todo + hardware_interface::JointStateInterface js_interface_; + hardware_interface::EffortJointInterface ej_interface_; + hardware_interface::VelocityJointInterface vj_interface_; + hardware_interface::PositionJointInterface pj_interface_; +#endif +#if 0 // @todo + 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_; +#endif + 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_vel_limits_; + std::vector joint_control_methods_; +#if 0 + std::vector pid_controllers_; +#endif + 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 joint_opmodes_; + + std::vector sim_joints_; + + std::vector joint_states_; + std::vector joint_cmds_; + std::vector joint_eff_cmdhandle_; + std::vector joint_vel_cmdhandle_; + std::vector joint_opmodehandles_; + + // limits + std::vector joint_pos_limit_handles_; + std::vector + joint_pos_soft_limit_handles_; + std::vector joint_eff_limit_handles_; + std::vector + joint_eff_soft_limit_handles_; + std::vector joint_vel_limit_handles_; + std::vector joint_vel_soft_limit_handles_; + + 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; + +} // namespace gazebo_ros2_control + +#endif // GAZEBO_ROS2_CONTROL__DEFAULT_ROBOT_HW_SIM_HPP_ diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.h b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.h deleted file mode 100644 index f90d802c..00000000 --- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_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 -*/ - -// ROS -#include -#include -#include - -// Gazebo -#include -#include -#include - -// ros_control -#include -#include -#include - -namespace gazebo_ros2_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::shared_ptr e_stop_active); - - // Node Handles - rclcpp::Node::SharedPtr 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_; - std::string param_file; - - // Transmissions in this plugin's scope - std::vector transmissions_; - - // Robot simulator interface - std::string robot_hw_sim_type_str_; - std::shared_ptr robot_hw_sim_; - rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; - std::thread thread_executor_spin_; //executor_->spin causes lockups, us ethis alternative for now - - // Controller manager - std::shared_ptr controller_manager_; - std::vector> controllers_; - - // Timing - rclcpp::Duration control_period_ = rclcpp::Duration(0); - rclcpp::Time last_update_sim_time_ros_; - rclcpp::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_; - rclcpp::Subscription::SharedPtr e_stop_sub_; // Emergency stop subscriber - -}; - - -} diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp new file mode 100644 index 00000000..87f6f264 --- /dev/null +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp @@ -0,0 +1,132 @@ +// Copyright (c) 2013, Open Source Robotics Foundation. All rights reserved. +// 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 HOLDER 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 +*/ + +#ifndef GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_ +#define GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_ + +#include +#include +#include + +// ROS +#include "rclcpp/rclcpp.hpp" +#include "pluginlib/class_loader.hpp" +#include "std_msgs/msg/bool.hpp" + +// Gazebo +#include "gazebo/gazebo.hh" +#include "gazebo/physics/physics.hh" +#include "gazebo/common/common.hh" + +// ros_control +#include "gazebo_ros2_control/robot_hw_sim.h" +#include "controller_manager/controller_manager.hpp" +#include "transmission_interface/transmission_parser.hpp" + +namespace gazebo_ros2_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::shared_ptr e_stop_active); + + // Node Handles + rclcpp::Node::SharedPtr 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_; + std::string param_file; + + // Transmissions in this plugin's scope + std::vector transmissions_; + + // Robot simulator interface + std::string robot_hw_sim_type_str_; + std::shared_ptr robot_hw_sim_; + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + // executor_->spin causes lockups, us ethis alternative for now + std::thread thread_executor_spin_; + + // Controller manager + std::shared_ptr controller_manager_; + std::vector> controllers_; + + // Timing + rclcpp::Duration control_period_ = rclcpp::Duration(0); + rclcpp::Time last_update_sim_time_ros_; + rclcpp::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_; + // Emergency stop subscriber + rclcpp::Subscription::SharedPtr e_stop_sub_; +}; +} // namespace gazebo_ros2_control + +#endif // GAZEBO_ROS2_CONTROL__GAZEBO_ROS2_CONTROL_PLUGIN_HPP_ diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.h b/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.h deleted file mode 100644 index 01cbfa09..00000000 --- a/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.h +++ /dev/null @@ -1,124 +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_ros2_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::RobotHardware - { - 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, - rclcpp::Node::SharedPtr& 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(rclcpp::Time time, rclcpp::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(rclcpp::Time time, rclcpp::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) {} - - //@note: Avoid using these functions! ros2_control was not built with gazebo_ros_control - //in mind, keep to using readSim and writeSim so we don't have to update the documents for now - virtual hardware_interface::hardware_interface_ret_t init() { } - virtual hardware_interface::hardware_interface_ret_t read() { } - virtual hardware_interface::hardware_interface_ret_t write() { } - }; - -} - -#endif // ifndef __ROS_CONTROL_GAZEBO_ROBOT_HW_SIM_H diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp new file mode 100644 index 00000000..98ba9470 --- /dev/null +++ b/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp @@ -0,0 +1,124 @@ +// Copyright (c) 2013, Open Source Robotics Foundation. All rights reserved. +// 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 HOLDER 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 GAZEBO_ROS2_CONTROL__ROBOT_HW_SIM_HPP_ +#define GAZEBO_ROS2_CONTROL__ROBOT_HW_SIM_HPP_ + +#include +#include + +#include "gazebo/physics/physics.hh" +#include "rclcpp/rclcpp.hpp" +#include "hardware_interface/robot_hardware.hpp" +#include "transmission_interface/transmission_info.hpp" +#include "urdf/model.h" + +namespace gazebo_ros2_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::RobotHardware +{ +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, + rclcpp::Node::SharedPtr & 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(rclcpp::Time time, rclcpp::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(rclcpp::Time time, rclcpp::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) {} + + // @note: Avoid using these functions! ros2_control was not built with gazebo_ros_control + // in mind, keep to using readSim and writeSim so we don't have to update the documents for now + virtual hardware_interface::hardware_interface_ret_t init() {} + virtual hardware_interface::hardware_interface_ret_t read() {} + virtual hardware_interface::hardware_interface_ret_t write() {} +}; + +} // namespace gazebo_ros2_control + +#endif // GAZEBO_ROS2_CONTROL__ROBOT_HW_SIM_HPP_ diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml index 4a075110..a2be483f 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -30,6 +30,10 @@ urdf angles + ament_lint_common + ament_lint_auto + ament_cmake_gtest + ament_cmake diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index eb4c4daa..a4803408 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -1,63 +1,59 @@ -/********************************************************************* - * 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. - *********************************************************************/ +// Copyright (c) 2013, Open Source Robotics Foundation. All rights reserved. +// 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 HOLDER 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 +#include -#include -#include +#include "gazebo_ros2_control/default_robot_hw_sim.hpp" +#include "urdf/model.h" namespace gazebo_ros2_control { static const rclcpp::Logger LOGGER = rclcpp::get_logger("default_robot_hw_sim"); bool DefaultRobotHWSim::initSim( - const std::string& robot_namespace, - rclcpp::Node::SharedPtr& model_nh, + const std::string & robot_namespace, + rclcpp::Node::SharedPtr & model_nh, gazebo::physics::ModelPtr parent_model, - const urdf::Model *const urdf_model, + const urdf::Model * const urdf_model, std::vector transmissions) { std::cerr << "DefaultRobotHWSim::InitSim " << std::endl; // 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". - rclcpp::Node::SharedPtr& joint_limit_nh = model_nh; + rclcpp::Node::SharedPtr & joint_limit_nh = model_nh; // Resize vectors to our DOF n_dof_ = transmissions.size(); @@ -92,48 +88,45 @@ bool DefaultRobotHWSim::initSim( joint_vel_limit_handles_.resize(n_dof_); // Initialize values - for(unsigned int j=0; j < n_dof_; j++) - { + for (unsigned int j = 0; j < n_dof_; j++) { // Check that this transmission has one joint - if(transmissions[j].joints_.empty()) - { - std::cerr << "Transmission " << transmissions[j].name_ - << " has no associated joints."; + if (transmissions[j].joints_.empty()) { + std::cerr << "Transmission " << transmissions[j].name_ << + " has no associated joints."; continue; - } - else if(transmissions[j].joints_.size() > 1) - { - std::cerr << "Transmission " << transmissions[j].name_ - << " has more than one joint. Currently the default robot hardware simulation " - << " interface only supports one."; + } else if (transmissions[j].joints_.size() > 1) { + std::cerr << "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())) + !(transmissions[j].actuators_.empty()) && + !(transmissions[j].actuators_[0].hardware_interfaces_.empty())) { - // TODO: Deprecate HW interface specification in actuators in ROS J + // TODO(anyone): Deprecate HW interface specification in actuators in ROS J joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_; std::cerr << "The element of tranmission " << - transmissions[j].name_ << " should be nested inside the element, not . " << + 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()) - { + if (joint_interfaces.empty()) { std::cerr << "Joint " << transmissions[j].name_ << - " of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " << + " 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) - { + } else if (joint_interfaces.size() > 1) { std::cerr << "Joint " << transmissions[j].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; + " 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 @@ -145,70 +138,81 @@ bool DefaultRobotHWSim::initSim( joint_position_command_[j] = 0.0; joint_velocity_command_[j] = 0.0; - const std::string& hardware_interface = joint_interfaces.front(); + const std::string & hardware_interface = joint_interfaces.front(); // Debug - std::cerr << "Loading joint '" << joint_names_[j] - << "' of type '" << hardware_interface << "'" << std::endl; + std::cerr << "Loading joint '" << joint_names_[j] << + "' of type '" << hardware_interface << "'" << std::endl; // Create joint state interface for all joints -#if 0 //@todo - js_interface_.registerHandle(hardware_interface::JointStateHandle( +#if 0 // @todo + js_interface_.registerHandle( + hardware_interface::JointStateHandle( joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j])); #endif // Decide what kind of command interface this actuator/joint has hardware_interface::JointStateHandle joint_handle; - if(hardware_interface == "EffortJointInterface" || hardware_interface == "hardware_interface/EffortJointInterface") + if (hardware_interface == "EffortJointInterface" || + hardware_interface == "hardware_interface/EffortJointInterface") { // Create effort joint interface joint_control_methods_[j] = EFFORT; -#if 0 //@todo - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_effort_command_[j]); +#if 0 // @todo + joint_handle = hardware_interface::JointHandle( + js_interface_.getHandle(joint_names_[j]), + &joint_effort_command_[j]); ej_interface_.registerHandle(joint_handle); #endif - } - else if(hardware_interface == "PositionJointInterface" || hardware_interface == "hardware_interface/PositionJointInterface") - { - // Create position joint interface - joint_control_methods_[j] = POSITION; -#if 0 //@todo - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_position_command_[j]); - pj_interface_.registerHandle(joint_handle); + } else { + if (hardware_interface == "PositionJointInterface" || + hardware_interface == "hardware_interface/PositionJointInterface") + { + // Create position joint interface + joint_control_methods_[j] = POSITION; +#if 0 // @todo + joint_handle = hardware_interface::JointHandle( + js_interface_.getHandle(joint_names_[j]), + &joint_position_command_[j]); + pj_interface_.registerHandle(joint_handle); #endif - } - else if(hardware_interface == "VelocityJointInterface" || hardware_interface == "hardware_interface/VelocityJointInterface") - { - // Create velocity joint interface - joint_control_methods_[j] = VELOCITY; - -#if 0 //@todo - joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]), - &joint_velocity_command_[j]); - vj_interface_.registerHandle(joint_handle); + } else { + if (hardware_interface == "VelocityJointInterface" || + hardware_interface == "hardware_interface/VelocityJointInterface") + { + // Create velocity joint interface + joint_control_methods_[j] = VELOCITY; + +#if 0 // @todo + joint_handle = hardware_interface::JointHandle( + js_interface_.getHandle(joint_names_[j]), + &joint_velocity_command_[j]); + vj_interface_.registerHandle(joint_handle); #endif + } else { + std::cerr << "No matching hardware interface found for '" << + hardware_interface << "' while loading interfaces for " << joint_names_[j] << std::endl; + return false; + } + } } - else + if (hardware_interface == "EffortJointInterface" || + hardware_interface == "PositionJointInterface" || + hardware_interface == "VelocityJointInterface") { - std::cerr << "No matching hardware interface found for '" - << hardware_interface << "' while loading interfaces for " << joint_names_[j] << std::endl; - return false; - } - if(hardware_interface == "EffortJointInterface" || hardware_interface == "PositionJointInterface" || hardware_interface == "VelocityJointInterface") { - std::cerr << "Deprecated syntax, please prepend 'hardware_interface/' to '" << hardware_interface << "' within the tag in joint '" << joint_names_[j] << "'." << std::endl; + std::cerr << "Deprecated syntax, please prepend 'hardware_interface/' to '" << + hardware_interface << "' within the tag in joint '" << + joint_names_[j] << "'." << std::endl; } // Get the gazebo joint that corresponds to the robot joint. - //RCLCPP_DEBUG_STREAM(LOGGER, "Getting pointer to gazebo joint: " + // RCLCPP_DEBUG_STREAM(LOGGER, "Getting pointer to gazebo joint: " // << joint_names_[j]); gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]); - if (!joint) - { - std::cerr << "This robot has a joint named \"" << joint_names_[j] - << "\" which is not in the gazebo model." << std::endl; + if (!joint) { + std::cerr << "This robot has a joint named \"" << joint_names_[j] << + "\" which is not in the gazebo model." << std::endl; return false; } sim_joints_.push_back(joint); @@ -222,28 +226,25 @@ bool DefaultRobotHWSim::initSim( gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine(); #endif physics_type_ = physics->GetType(); - if (physics_type_.empty()) - { + if (physics_type_.empty()) { std::cerr << "No physics type found." << std::endl; } - 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], &joint_vel_limits_[j]); - if (joint_control_methods_[j] != EFFORT) - { + 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], &joint_vel_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. #if 0 const ros::NodeHandle nh(robot_namespace + "//pid_gains/" + - joint_names_[j]); + joint_names_[j]); #endif #if 0 - if (pid_controllers_[j].init(nh)) - { - switch (joint_control_methods_[j]) - { + if (pid_controllers_[j].init(nh)) { + switch (joint_control_methods_[j]) { case POSITION: joint_control_methods_[j] = POSITION_PID; break; @@ -251,8 +252,7 @@ bool DefaultRobotHWSim::initSim( joint_control_methods_[j] = VELOCITY_PID; break; } - } - else + } else {} #endif { // joint->SetParam("fmax") must be called if joint->SetAngle() or joint->SetParam("vel") are @@ -264,27 +264,26 @@ bool DefaultRobotHWSim::initSim( joint->SetMaxForce(0, joint_effort_limits_[j]); #endif } - } joint_states_[j] = hardware_interface::JointStateHandle( joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]); if (register_joint_state_handle(&joint_states_[j]) != hardware_interface::HW_RET_OK) { - RCLCPP_WARN_ONCE(LOGGER,"cant register jointstatehandle"); + RCLCPP_WARN_ONCE(LOGGER, "cant register jointstatehandle"); } joint_cmds_[j] = hardware_interface::JointCommandHandle( joint_names_[j], &joint_position_command_[j]); if (register_joint_command_handle(&joint_cmds_[j]) != hardware_interface::HW_RET_OK) { - RCLCPP_WARN_ONCE(LOGGER,"cant register jointcommandhandle"); + RCLCPP_WARN_ONCE(LOGGER, "cant register jointcommandhandle"); } joint_opmodehandles_[j] = hardware_interface::OperationModeHandle( joint_names_[j], &joint_opmodes_[j]); if (register_operation_mode_handle(&joint_opmodehandles_[j]) != hardware_interface::HW_RET_OK) { - RCLCPP_WARN_ONCE(LOGGER,"cant register jointopmodehandle"); + RCLCPP_WARN_ONCE(LOGGER, "cant register jointopmodehandle"); } - joint_limits_interface::JointLimits limits; //hack, refactor registerjointhandle + joint_limits_interface::JointLimits limits; // hack, refactor registerjointhandle limits.has_position_limits = true; limits.min_position = joint_lower_limits_[j]; limits.max_position = joint_upper_limits_[j]; @@ -298,10 +297,9 @@ bool DefaultRobotHWSim::initSim( joint_eff_cmdhandle_[j] = hardware_interface::JointCommandHandle( joint_names_[j], &joint_effort_command_[j]); - //should be register_joint_effort_command_handle, but there's only 1 buffer for now - // ALEX + // should be register_joint_effort_command_handle, but there's only 1 buffer for now if (register_joint_command_handle(&joint_eff_cmdhandle_[j]) != hardware_interface::HW_RET_OK) { - RCLCPP_WARN_ONCE(LOGGER,"cant register jointcommandhandle"); + RCLCPP_WARN_ONCE(LOGGER, "cant register jointcommandhandle"); } joint_eff_limit_handles_[j] = joint_limits_interface::EffortJointSaturationHandle( @@ -315,9 +313,9 @@ bool DefaultRobotHWSim::initSim( joint_vel_limit_handles_[j] = joint_limits_interface::VelocityJointSaturationHandle( joint_states_[j], joint_vel_cmdhandle_[j], limits); - //register not implemented + // register not implemented } -#if 0 //@todo +#if 0 // @todo // Register interfaces registerInterface(&js_interface_); registerInterface(&ej_interface_); @@ -333,22 +331,19 @@ bool DefaultRobotHWSim::initSim( void DefaultRobotHWSim::readSim(rclcpp::Time time, rclcpp::Duration period) { - for(unsigned int j=0; j < n_dof_; j++) - { + 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) - { + if (joint_types_[j] == urdf::Joint::PRISMATIC) { joint_position_[j] = position; - } - else - { - joint_position_[j] += angles::shortest_angular_distance(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)); @@ -358,20 +353,16 @@ void DefaultRobotHWSim::readSim(rclcpp::Time time, rclcpp::Duration period) void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::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_) - { + 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 - { + } else { last_e_stop_active_ = false; } -#if 0 //@todo +#if 0 // @todo ej_sat_interface_.enforceLimits(period); ej_limits_interface_.enforceLimits(period); pj_sat_interface_.enforceLimits(period); @@ -379,24 +370,19 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) vj_sat_interface_.enforceLimits(period); vj_limits_interface_.enforceLimits(period); #else - for (auto eff_limit_handle : joint_eff_limit_handles_) - { + for (auto eff_limit_handle : joint_eff_limit_handles_) { eff_limit_handle.enforceLimits(period); } - for (auto pos_limit_handle : joint_pos_limit_handles_) - { + for (auto pos_limit_handle : joint_pos_limit_handles_) { pos_limit_handle.enforceLimits(period); } - for (auto vel_limit_handle : joint_vel_limit_handles_) - { + for (auto vel_limit_handle : joint_vel_limit_handles_) { vel_limit_handle.enforceLimits(period); } #endif - for(unsigned int j=0; j < n_dof_; j++) - { - switch (joint_control_methods_[j]) - { + 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]; @@ -408,10 +394,20 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) #if GAZEBO_MAJOR_VERSION >= 9 sim_joints_[j]->SetPosition(0, joint_position_command_[j], true); #else - RCLCPP_WARN_ONCE(LOGGER,"The default_robot_hw_sim plugin is using the Joint::SetPosition method without preserving the link velocity."); - RCLCPP_WARN_ONCE(LOGGER,"As a result, gravity will not be simulated correctly for your model."); - RCLCPP_WARN_ONCE(LOGGER,"Please set gazebo_pid parameters, switch to the VelocityJointInterface or EffortJointInterface, or upgrade to Gazebo 9."); - RCLCPP_WARN_ONCE(LOGGER,"For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612"); + RCLCPP_WARN_ONCE( + LOGGER, + "The default_robot_hw_sim plugin is using the Joint::SetPosition method without " + "preserving the link velocity."); + RCLCPP_WARN_ONCE( + LOGGER, + "As a result, gravity will not be simulated correctly for your model."); + RCLCPP_WARN_ONCE( + LOGGER, + "Please set gazebo_pid parameters, switch to the VelocityJointInterface or " + "EffortJointInterface, or upgrade to Gazebo 9."); + RCLCPP_WARN_ONCE( + LOGGER, + "For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612"); sim_joints_[j]->SetPosition(0, joint_position_command_[j]); #endif break; @@ -419,25 +415,26 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) case POSITION_PID: { double error; - switch (joint_types_[j]) - { + 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); + 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]); + 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]; - //TODO(anyone): Restored this when PID controllers is available + // TODO(anyone): Restored this when PID controllers is available // const double effort = std::clamp(pid_controllers_[j].computeCommand(error, period), // -effort_limit, effort_limit); const double effort = 0.0; @@ -447,12 +444,9 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) case VELOCITY: #if GAZEBO_MAJOR_VERSION > 2 - if (physics_type_.compare("ode") == 0) - { + if (physics_type_.compare("ode") == 0) { sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]); - } - else - { + } else { sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]); } #else @@ -462,12 +456,13 @@ void DefaultRobotHWSim::writeSim(rclcpp::Time time, rclcpp::Duration period) case VELOCITY_PID: double error; - if (e_stop_active_) + if (e_stop_active_) { error = -joint_velocity_[j]; - else + } else { error = joint_velocity_command_[j] - joint_velocity_[j]; + } const double effort_limit = joint_effort_limits_[j]; - //TODO(anyone): Restored this when PID controllers is available + // TODO(anyone): Restored this when PID controllers is available // const double effort = std::clamp(pid_controllers_[j].computeCommand(error, period), // -effort_limit, effort_limit); const double effort = 0.0; @@ -485,14 +480,15 @@ void DefaultRobotHWSim::eStopActive(const bool 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::JointStateHandle& joint_handle, - const ControlMethod ctrl_method, - const rclcpp::Node::SharedPtr& 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, - double *const vel_limit) +void DefaultRobotHWSim::registerJointLimits( + const std::string & joint_name, + const hardware_interface::JointStateHandle & joint_handle, + const ControlMethod ctrl_method, + const rclcpp::Node::SharedPtr & 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, + double * const vel_limit) { *joint_type = urdf::Joint::UNKNOWN; *lower_limit = -std::numeric_limits::max(); @@ -505,19 +501,22 @@ void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name, joint_limits_interface::SoftJointLimits soft_limits; bool has_soft_limits = false; - if (urdf_model != NULL) - { + if (urdf_model != NULL) { const urdf::JointConstSharedPtr urdf_joint = urdf_model->getJoint(joint_name); - if (urdf_joint != NULL) - { + if (urdf_joint != NULL) { *joint_type = urdf_joint->type; // Get limits from the URDF file. - if (joint_limits_interface::getJointLimits(urdf_joint->name, joint_limit_nh, limits)) + if (joint_limits_interface::getJointLimits(urdf_joint->name, joint_limit_nh, limits)) { has_limits = true; - if (joint_limits_interface::getSoftJointLimits(urdf_joint->name, joint_limit_nh, soft_limits)) + } + if (joint_limits_interface::getSoftJointLimits( + urdf_joint->name, joint_limit_nh, + soft_limits)) + { has_soft_limits = true; + } - //@note (ddeng): these joint limits arent input into the node, so fetch them from the urdf + // @note (ddeng): these joint limits arent input into the node, so fetch them from the urdf limits.min_position = urdf_joint->limits->lower; limits.max_position = urdf_joint->limits->upper; limits.has_position_limits = true; @@ -533,51 +532,48 @@ void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name, *effort_limit = limits.max_effort; *vel_limit = limits.max_velocity; - //urdf_joint->safety->k_position; + // urdf_joint->safety->k_position; has_limits = true; } } // Get limits from the parameter server. - //@note: no longer using parameter servers - if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits)) + // @note: no longer using parameter servers + if (joint_limits_interface::getJointLimits(joint_name, joint_limit_nh, limits)) { has_limits = true; + } - if (!has_limits) + if (!has_limits) { return; + } - if (*joint_type == urdf::Joint::UNKNOWN) - { + if (*joint_type == urdf::Joint::UNKNOWN) { // Infer the joint type. - if (limits.has_position_limits) - { + if (limits.has_position_limits) { *joint_type = urdf::Joint::REVOLUTE; - } - else - { - if (limits.angle_wraparound) + } else { + if (limits.angle_wraparound) { *joint_type = urdf::Joint::CONTINUOUS; - else + } else { *joint_type = urdf::Joint::PRISMATIC; + } } } - if (limits.has_position_limits) - { + if (limits.has_position_limits) { *lower_limit = limits.min_position; *upper_limit = limits.max_position; } - if (limits.has_effort_limits) + if (limits.has_effort_limits) { *effort_limit = limits.max_effort; + } - if (has_soft_limits) - { - switch (ctrl_method) - { + if (has_soft_limits) { + switch (ctrl_method) { case EFFORT: { -#if 0 //@todo +#if 0 // @todo const joint_limits_interface::EffortJointSoftLimitsHandle limits_handle(joint_handle, limits, soft_limits); ej_limits_interface_.registerHandle(limits_handle); @@ -586,7 +582,7 @@ void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name, break; case POSITION: { -#if 0 //@todo +#if 0 // @todo const joint_limits_interface::PositionJointSoftLimitsHandle limits_handle(joint_handle, limits, soft_limits); pj_limits_interface_.registerHandle(limits_handle); @@ -595,7 +591,7 @@ void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name, break; case VELOCITY: { -#if 0 //@todo +#if 0 // @todo const joint_limits_interface::VelocityJointSoftLimitsHandle limits_handle(joint_handle, limits, soft_limits); vj_limits_interface_.registerHandle(limits_handle); @@ -603,11 +599,8 @@ void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name, } break; } - } - else - { - switch (ctrl_method) - { + } else { + switch (ctrl_method) { case EFFORT: { #if 0 @@ -620,7 +613,7 @@ void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name, break; case POSITION: { -#if 0 //@todo +#if 0 // @todo const joint_limits_interface::PositionJointSaturationHandle sat_handle(joint_handle, limits); @@ -630,7 +623,7 @@ void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name, break; case VELOCITY: { -#if 0 //@todo +#if 0 // @todo const joint_limits_interface::VelocityJointSaturationHandle sat_handle(joint_handle, limits); vj_sat_interface_.registerHandle(sat_handle); @@ -641,6 +634,6 @@ void DefaultRobotHWSim::registerJointLimits(const std::string& joint_name, } } -} +} // namespace gazebo_ros2_control PLUGINLIB_EXPORT_CLASS(gazebo_ros2_control::DefaultRobotHWSim, gazebo_ros2_control::RobotHWSim) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index af172c43..9522a106 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -1,48 +1,44 @@ -/********************************************************************* - * 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. - *********************************************************************/ +// Copyright (c) 2013, Open Source Robotics Foundation. All rights reserved. +// 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 HOLDER 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 */ -#include -#include +#include +#include +#include -#include +#include "gazebo_ros2_control/gazebo_ros2_control_plugin.hpp" + +#include "urdf/model.h" #include "yaml-cpp/yaml.h" namespace gazebo_ros2_control @@ -57,7 +53,9 @@ GazeboRosControlPlugin::~GazeboRosControlPlugin() // Overloaded Gazebo entry point void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) { - RCLCPP_INFO_STREAM(rclcpp::get_logger("gazebo_ros2_control"), "Loading gazebo_ros2_control plugin"); + RCLCPP_INFO_STREAM( + rclcpp::get_logger("gazebo_ros2_control"), + "Loading gazebo_ros2_control plugin"); // Save pointers to the model @@ -65,72 +63,74 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element sdf_ = sdf; // Error message if the model couldn't be found - if (!parent_model_) - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadThread"),"parent model is NULL"); + if (!parent_model_) { + RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadThread"), "parent model is NULL"); return; } // Check that ROS has been initialized - if(!rclcpp::ok()) - { - RCLCPP_FATAL_STREAM(rclcpp::get_logger("gazebo_ros2_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)"); + if (!rclcpp::ok()) { + RCLCPP_FATAL_STREAM( + rclcpp::get_logger( + "gazebo_ros2_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")) - { + if (sdf_->HasElement("robotNamespace")) { robot_namespace_ = sdf_->GetElement("robotNamespace")->Get(); - } - else - { - robot_namespace_ = parent_model_->GetName(); // default + } else { + robot_namespace_ = parent_model_->GetName(); // default } // Get robot_description ROS param name - if (sdf_->HasElement("robotParam")) - { + if (sdf_->HasElement("robotParam")) { robot_description_ = sdf_->GetElement("robotParam")->Get(); - } - else - { - robot_description_ = "robot_description"; // default + } else { + robot_description_ = "robot_description"; // default } // Get the robot simulation interface type - if(sdf_->HasElement("robotSimType")) - { + if (sdf_->HasElement("robotSimType")) { robot_hw_sim_type_str_ = sdf_->Get("robotSimType"); - } - else - { + } else { robot_hw_sim_type_str_ = "gazebo_ros2_control/DefaultRobotHWSim"; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("loadThread"),"Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<HasElement("legacyModeNS")) { - if( sdf_->GetElement("legacyModeNS")->Get() ){ - robot_ns = ""; - } - }else{ - robot_ns = ""; - RCLCPP_ERROR(rclcpp::get_logger("loadThread"),"GazeboRosControlPlugin missing while using DefaultRobotHWSim, defaults to true.\n" - "This setting assumes you have an old package with an old implementation of DefaultRobotHWSim, " - "where the robotNamespace is disregarded and absolute paths are used instead.\n" - "If you do not want to fix this issue in an old package just set to true.\n" - ); + if (robot_hw_sim_type_str_ == "gazebo_ros2_control/DefaultRobotHWSim") { + if (sdf_->HasElement("legacyModeNS")) { + if (sdf_->GetElement("legacyModeNS")->Get() ) { + robot_ns = ""; } + } else { + robot_ns = ""; + RCLCPP_ERROR( + rclcpp::get_logger( + "loadThread"), + "GazeboRosControlPlugin missing while using DefaultRobotHWSim, defaults to" + " true.\nThis setting assumes you have an old package with an old implementation of " + "DefaultRobotHWSim, where the robotNamespace is disregarded and absolute paths are used " + "instead.\n If you do not want to fix this issue in an old package just set " + "to true.\n" + ); + } } if (sdf_->HasElement("parameters")) { param_file = sdf_->GetElement("parameters")->Get(); } else { - RCLCPP_ERROR(rclcpp::get_logger("loadThread"), "No parameter file provided. Configuration might be wrong"); + RCLCPP_ERROR( + rclcpp::get_logger( + "loadThread"), "No parameter file provided. Configuration might be wrong"); } // Get the Gazebo simulation period @@ -141,27 +141,30 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element #endif // Decide the plugin control period - if(sdf_->HasElement("controlPeriod")) - { + if (sdf_->HasElement("controlPeriod")) { control_period_ = rclcpp::Duration(sdf_->Get("controlPeriod")); // Check the period against the simulation period - if( control_period_ < gazebo_period ) - { - RCLCPP_ERROR_STREAM(rclcpp::get_logger("gazebo_ros2_control"),"Desired controller update period ("< gazebo_period) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger( + "gazebo_ros2_control"), + " Desired controller update period (" << control_period_.seconds() << + " s) is slower than the gazebo simulation period (" << gazebo_period.seconds() << " s)."); } - else if( control_period_ > gazebo_period ) - { - RCLCPP_WARN_STREAM(rclcpp::get_logger("gazebo_ros2_control")," Desired controller update period ("<< control_period_.seconds() - <<" s) is slower than the gazebo simulation period ("<< gazebo_period.seconds() <<" s)."); - } - } - else - { + } else { control_period_ = gazebo_period; - RCLCPP_DEBUG_STREAM(rclcpp::get_logger("gazebo_ros2_control"),"Control period not found in URDF/SDF, defaulting to Gazebo period of " - << control_period_.seconds()); + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger( + "gazebo_ros2_control"), + "Control period not found in URDF/SDF, defaulting to Gazebo period of " << + control_period_.seconds()); } // Get parameters/settings for controllers from ROS param server @@ -170,41 +173,50 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element // Initialize the emergency stop code. e_stop_active_ = false; last_e_stop_active_ = false; - if (sdf_->HasElement("eStopTopic")) - { + if (sdf_->HasElement("eStopTopic")) { using std::placeholders::_1; const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get(); - e_stop_sub_ = model_nh_->create_subscription(e_stop_topic, 1, + e_stop_sub_ = model_nh_->create_subscription( + e_stop_topic, 1, std::bind(&GazeboRosControlPlugin::eStopCB, this, _1)); } - RCLCPP_INFO(rclcpp::get_logger("gazebo_ros2_control"), "Starting gazebo_ros2_control plugin in namespace: %s", robot_namespace_.c_str()); + RCLCPP_INFO( + rclcpp::get_logger( + "gazebo_ros2_control"), "Starting gazebo_ros2_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)) - { - RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "Error parsing URDF in gazebo_ros2_control plugin, plugin not active.\n"); + if (!parseTransmissionsFromURDF(urdf_string)) { + RCLCPP_ERROR( + rclcpp::get_logger( + "gazebo_ros2_control"), + "Error parsing URDF in gazebo_ros2_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_ros2_control", - "gazebo_ros2_control::RobotHWSim")); + try { + robot_hw_sim_loader_.reset( + new pluginlib::ClassLoader( + "gazebo_ros2_control", + "gazebo_ros2_control::RobotHWSim")); robot_hw_sim_ = robot_hw_sim_loader_->createSharedInstance(robot_hw_sim_type_str_); urdf::Model urdf_model; - const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL; + 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_)) + if (!robot_hw_sim_->initSim( + robot_ns, model_nh_, parent_model_, urdf_model_ptr, + transmissions_)) { - RCLCPP_FATAL(rclcpp::get_logger("gazebo_ros2_control"),"Could not initialize robot simulation interface"); + RCLCPP_FATAL( + rclcpp::get_logger( + "gazebo_ros2_control"), "Could not initialize robot simulation interface"); return; } @@ -212,127 +224,129 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element executor_->add_node(model_nh_); // Create the controller manager - RCLCPP_ERROR(rclcpp::get_logger("ros2_control_plugin"),"Loading controller_manager"); -#if 1 //@todo - controller_manager_.reset - (new controller_manager::ControllerManager(robot_hw_sim_, executor_, "gazebo_controller_manager")); + RCLCPP_ERROR(rclcpp::get_logger("ros2_control_plugin"), "Loading controller_manager"); +#if 1 // @todo + controller_manager_.reset( + new controller_manager::ControllerManager( + robot_hw_sim_, executor_, + "gazebo_controller_manager")); #endif -#if 1 //@todo: Coded example here. should disable when spawn functionality of controller manager is up +#if 1 + // @todo:Coded example here. should disable when spawn functionality of controller manager is up auto load_params_from_yaml_node = [](rclcpp_lifecycle::LifecycleNode::SharedPtr lc_node, - YAML::Node & yaml_node, const std::string& prefix) - { - std::function, const std::string& )> - feed_yaml_to_node_rec = - [&](YAML::Node yaml_node, const std::string & key, - std::shared_ptr node, const std::string& prefix) - { - if (node->get_name() != prefix) - return; - static constexpr char separator = '.'; - if (yaml_node.Type() == YAML::NodeType::Scalar) { - std::string val_str = yaml_node.as(); - //const char* val_str = yaml_node.as(); - // std::string val_str; - // YAML::convert::decode(yaml_node, val_str); - - // TODO(ddengster): Do stricter typing for set_parameter value types. - // (ie. Numbers should be converted to int/double/etc instead of strings) - if (!node->has_parameter(key)) - node->declare_parameter(key); - - auto is_number = [](const std::string &str) -> bool { - return str.find_first_not_of("0123456789.-") == std::string::npos; - //@note: bugs with .05 or 15. - }; - - if (is_number(val_str)) - { - std::stringstream ss(val_str); - double v = 0.0; - ss >> v; - node->set_parameter(rclcpp::Parameter(key, v)); - } - else { - node->set_parameter(rclcpp::Parameter(key, val_str)); + YAML::Node & yaml_node, const std::string & prefix) + { + std::function, const std::string &)> + feed_yaml_to_node_rec = + [&](YAML::Node yaml_node, const std::string & key, + std::shared_ptr node, const std::string & prefix) + { + if (node->get_name() != prefix) { + return; } + static constexpr char separator = '.'; + if (yaml_node.Type() == YAML::NodeType::Scalar) { + std::string val_str = yaml_node.as(); + // const char* val_str = yaml_node.as(); + // std::string val_str; + // YAML::convert::decode(yaml_node, val_str); + + // TODO(ddengster): Do stricter typing for set_parameter value types. + // (ie. Numbers should be converted to int/double/etc instead of strings) + if (!node->has_parameter(key)) { + node->declare_parameter(key); + } - return; - } else if (yaml_node.Type() == YAML::NodeType::Map) { - for (auto yaml_node_it : yaml_node) { - std::string newkey = yaml_node_it.first.as(); - if (newkey == prefix || newkey == "ros__parameters") - newkey = ""; - else if (!key.empty()) - newkey = key + separator + newkey; - feed_yaml_to_node_rec(yaml_node_it.second, newkey, node, prefix); - } - } else if (yaml_node.Type() == YAML::NodeType::Sequence) { - auto it = yaml_node.begin(); - if (yaml_node.size()) - { - if (it->IsScalar()) - { - //submit as array of parameters - std::vector val; - for (auto yaml_node_it : yaml_node) { - std::string name = yaml_node_it.as(); - val.push_back(name); - } - if (!node->has_parameter(key)) - node->declare_parameter(key); - node->set_parameter({rclcpp::Parameter(key, val)}); - - if (key == "joints") - { - if (!node->has_parameter("write_op_modes")) - node->declare_parameter("write_op_modes"); - node->set_parameter(rclcpp::Parameter("write_op_modes", val)); + auto is_number = [](const std::string & str) -> bool { + return str.find_first_not_of("0123456789.-") == std::string::npos; + // @note: bugs with .05 or 15. + }; + + if (is_number(val_str)) { + std::stringstream ss(val_str); + double v = 0.0; + ss >> v; + node->set_parameter(rclcpp::Parameter(key, v)); + } else { + node->set_parameter(rclcpp::Parameter(key, val_str)); + } + + return; + } else if (yaml_node.Type() == YAML::NodeType::Map) { + for (auto yaml_node_it : yaml_node) { + std::string newkey = yaml_node_it.first.as(); + if (newkey == prefix || newkey == "ros__parameters") { + newkey = ""; + } else if (!key.empty()) { + newkey = key + separator + newkey; } + feed_yaml_to_node_rec(yaml_node_it.second, newkey, node, prefix); } - else - { - size_t index = 0; - for (auto yaml_node_it : yaml_node) { - std::string newkey = std::to_string((index++)); - if (!key.empty()) - newkey = key + separator + newkey; - feed_yaml_to_node_rec(yaml_node_it, newkey, node, prefix); + } else if (yaml_node.Type() == YAML::NodeType::Sequence) { + auto it = yaml_node.begin(); + if (yaml_node.size()) { + if (it->IsScalar()) { + // submit as array of parameters + std::vector val; + for (auto yaml_node_it : yaml_node) { + std::string name = yaml_node_it.as(); + val.push_back(name); + } + if (!node->has_parameter(key)) { + node->declare_parameter(key); + } + node->set_parameter({rclcpp::Parameter(key, val)}); + + if (key == "joints") { + if (!node->has_parameter("write_op_modes")) { + node->declare_parameter("write_op_modes"); + } + node->set_parameter(rclcpp::Parameter("write_op_modes", val)); + } + } else { + size_t index = 0; + for (auto yaml_node_it : yaml_node) { + std::string newkey = std::to_string((index++)); + if (!key.empty()) { + newkey = key + separator + newkey; + } + feed_yaml_to_node_rec(yaml_node_it, newkey, node, prefix); + } } } } - } - }; - if (lc_node->get_name() != prefix) - return; - feed_yaml_to_node_rec(yaml_node, prefix, lc_node, prefix); - }; + }; + if (lc_node->get_name() != prefix) { + return; + } + feed_yaml_to_node_rec(yaml_node, prefix, lc_node, prefix); + }; auto load_params_from_yaml = [&](rclcpp_lifecycle::LifecycleNode::SharedPtr lc_node, - const std::string & yaml_config_file, const std::string& prefix) - { - if (yaml_config_file.empty()) { - throw std::runtime_error("yaml config file path is empty"); - } - - YAML::Node root_node = YAML::LoadFile(yaml_config_file); - for (auto yaml : root_node) + const std::string & yaml_config_file, const std::string & prefix) { - auto nodename = yaml.first.as(); - RCLCPP_ERROR(rclcpp::get_logger("ros_control_plugin"),"nodename: %s", nodename.c_str()); - if (nodename == prefix) - load_params_from_yaml_node(lc_node, yaml.second, prefix); - } - }; + if (yaml_config_file.empty()) { + throw std::runtime_error("yaml config file path is empty"); + } + + YAML::Node root_node = YAML::LoadFile(yaml_config_file); + for (auto yaml : root_node) { + auto nodename = yaml.first.as(); + RCLCPP_ERROR(rclcpp::get_logger("ros_control_plugin"), "nodename: %s", nodename.c_str()); + if (nodename == prefix) { + load_params_from_yaml_node(lc_node, yaml.second, prefix); + } + } + }; YAML::Node root_node = YAML::LoadFile(param_file); - for (auto yaml : root_node) - { + for (auto yaml : root_node) { auto controller_name = yaml.first.as(); - for (auto yaml_node_it : yaml.second) { //ros__parameters + for (auto yaml_node_it : yaml.second) { // ros__parameters for (auto yaml_node_it2 : yaml_node_it.second) { auto param_name = yaml_node_it2.first.as(); - if(param_name == "type") { - auto controller_type = yaml_node_it2.second.as(); + if (param_name == "type") { + auto controller_type = yaml_node_it2.second.as(); auto controller = controller_manager_->load_controller( controller_name, controller_type); @@ -344,30 +358,33 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } auto spin = [this]() - { - while (rclcpp::ok()) - executor_->spin_once(); - }; + { + while (rclcpp::ok()) { + executor_->spin_once(); + } + }; thread_executor_spin_ = std::thread(spin); - if (controller_manager_->configure() != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { + if (controller_manager_->configure() != + controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) + { RCLCPP_ERROR(rclcpp::get_logger("cm"), "failed to configure"); } if (controller_manager_->activate() != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { RCLCPP_ERROR(rclcpp::get_logger("cm"), "failed to activate"); } - #endif - // 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) - { - RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"),"Failed to create robot simulation interface loader: %s ", ex.what()); + gazebo::event::Events::ConnectWorldUpdateBegin( + boost::bind( + &GazeboRosControlPlugin::Update, + this)); + } catch (pluginlib::LibraryLoadException & ex) { + RCLCPP_ERROR( + rclcpp::get_logger( + "gazebo_ros2_control"), "Failed to create robot simulation interface loader: %s ", + ex.what()); } RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "Loaded gazebo_ros2_control."); @@ -394,7 +411,7 @@ void GazeboRosControlPlugin::Update() robot_hw_sim_->eStopActive(e_stop_active_); // Check if we should update the controllers - if(sim_period >= control_period_) { + if (sim_period >= control_period_) { // Store this simulation time last_update_sim_time_ros_ = sim_time_ros; @@ -403,20 +420,14 @@ void GazeboRosControlPlugin::Update() // Compute the controller commands bool reset_ctrlrs; - if (e_stop_active_) - { + if (e_stop_active_) { reset_ctrlrs = false; last_e_stop_active_ = true; - } - else - { - if (last_e_stop_active_) - { + } else { + if (last_e_stop_active_) { reset_ctrlrs = true; last_e_stop_active_ = false; - } - else - { + } else { reset_ctrlrs = false; } } @@ -444,40 +455,49 @@ std::string GazeboRosControlPlugin::getURDF(std::string param_name) const std::string urdf_string; using namespace std::chrono_literals; - auto parameters_client = std::make_shared(model_nh_, "robot_state_publisher"); + auto parameters_client = std::make_shared( + model_nh_, + "robot_state_publisher"); while (!parameters_client->wait_for_service(0.5s)) { if (!rclcpp::ok()) { - RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "Interrupted while waiting for the service. Exiting."); + RCLCPP_ERROR( + rclcpp::get_logger( + "gazebo_ros2_control"), "Interrupted while waiting for the service. Exiting."); return 0; } - RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "service not available, waiting again..."); + RCLCPP_ERROR( + rclcpp::get_logger( + "gazebo_ros2_control"), "service not available, waiting again..."); } - RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "connected to service!! /robot_state_publisher"); + RCLCPP_ERROR( + rclcpp::get_logger( + "gazebo_ros2_control"), "connected to service!! /robot_state_publisher"); // search and wait for robot_description on param server - while (urdf_string.empty()) - { + while (urdf_string.empty()) { std::string search_param_name; RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "param_name %s", param_name.c_str()); urdf_string = parameters_client->get_parameter(param_name, ""); - if (!urdf_string.empty()) + if (!urdf_string.empty()) { break; - else - { - RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "gazebo_ros2_control plugin is waiting for model" + } else { + RCLCPP_ERROR( + rclcpp::get_logger("gazebo_ros2_control"), "gazebo_ros2_control plugin is waiting for model" " URDF in parameter [%s] on the ROS param server.", search_param_name.c_str()); } usleep(100000); } - RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "Recieved urdf from param server, parsing..."); + RCLCPP_ERROR( + rclcpp::get_logger( + "gazebo_ros2_control"), "Recieved urdf from param server, parsing..."); return urdf_string; } // Get Transmissions from the URDF -bool GazeboRosControlPlugin::parseTransmissionsFromURDF(const std::string& urdf_string) +bool GazeboRosControlPlugin::parseTransmissionsFromURDF(const std::string & urdf_string) { transmission_interface::TransmissionParser::parse(urdf_string, transmissions_); return true; @@ -489,7 +509,6 @@ void GazeboRosControlPlugin::eStopCB(const std::shared_ptr e_stop_active_ = e_stop_active->data; } - // Register this plugin with the simulator GZ_REGISTER_MODEL_PLUGIN(GazeboRosControlPlugin); -} // namespace +} // namespace gazebo_ros2_control From 4845065888eddf41aad1e0f2c273c5fc8d7902d7 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 29 May 2020 13:27:09 +0200 Subject: [PATCH 6/8] Added feecback Signed-off-by: ahcorde --- gazebo_ros2_control/README.md | 2 +- .../include/gazebo_ros2_control/default_robot_hw_sim.hpp | 5 ++--- .../gazebo_ros2_control/gazebo_ros2_control_plugin.hpp | 2 +- .../include/gazebo_ros2_control/robot_hw_sim.hpp | 2 +- gazebo_ros2_control/package.xml | 6 +++--- gazebo_ros2_control/src/default_robot_hw_sim.cpp | 2 -- gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 1 - 7 files changed, 8 insertions(+), 12 deletions(-) diff --git a/gazebo_ros2_control/README.md b/gazebo_ros2_control/README.md index 0fb544ae..0dd9548b 100644 --- a/gazebo_ros2_control/README.md +++ b/gazebo_ros2_control/README.md @@ -1,6 +1,6 @@ # Gazebo ros_control Interfaces -This is a ROS 2 package for integrating the `ros_control` controller architecture +This is a ROS 2 package for integrating the [ros2_control](https://github.com/ros-controls/ros2_control) controller architecture with the [Gazebo](http://gazebosim.org/) simulator. This package provides a Gazebo plugin which instantiates a ros_control diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp index ba0be2b6..9225ed92 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp @@ -52,9 +52,8 @@ #include "joint_limits_interface/joint_limits_urdf.hpp" // Gazebo -#include "gazebo/common/common.hh" -#include "gazebo/physics/physics.hh" -#include "gazebo/gazebo.hh" +#include "gazebo/physics/Model.hh" +#include "gazebo/physics/Joint.hh" // ROS #include "rclcpp/rclcpp.hpp" diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp index 87f6f264..2bcb41c4 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp @@ -63,7 +63,7 @@ class GazeboRosControlPlugin : public gazebo::ModelPlugin virtual ~GazeboRosControlPlugin(); // Overloaded Gazebo entry point - virtual void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf); + void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) override; // Called by the world update start event void Update(); diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp index 98ba9470..21235fd4 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp @@ -39,8 +39,8 @@ #include #include "gazebo/physics/physics.hh" -#include "rclcpp/rclcpp.hpp" #include "hardware_interface/robot_hardware.hpp" +#include "rclcpp/rclcpp.hpp" #include "transmission_interface/transmission_info.hpp" #include "urdf/model.h" diff --git a/gazebo_ros2_control/package.xml b/gazebo_ros2_control/package.xml index a2be483f..7c8fabe1 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -4,13 +4,13 @@ 0.0.1 gazebo_ros2_control - Jose Luis Rivero + Alejandro Hernandez 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 + https://github.com/ros-simulation/gazebo_ros2_control/issues + https://github.com/ros-simulation/gazebo_ros2_control/ Jonathan Bohren Dave Coleman diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp index a4803408..1bbee73e 100644 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -49,8 +49,6 @@ bool DefaultRobotHWSim::initSim( const urdf::Model * const urdf_model, std::vector transmissions) { - std::cerr << "DefaultRobotHWSim::InitSim " << std::endl; - // 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". rclcpp::Node::SharedPtr & joint_limit_nh = model_nh; diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index 9522a106..a6b6eaf0 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -57,7 +57,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element rclcpp::get_logger("gazebo_ros2_control"), "Loading gazebo_ros2_control plugin"); - // Save pointers to the model parent_model_ = parent; sdf_ = sdf; From 3b22eac932c249c04a4fb333c0abe983a5515b71 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 29 May 2020 13:55:57 +0200 Subject: [PATCH 7/8] Added PIMPL pattern to GazeboROSControl Signed-off-by: ahcorde --- .../gazebo_ros2_control_plugin.hpp | 69 +----- .../src/gazebo_ros2_control_plugin.cpp | 210 ++++++++++++------ 2 files changed, 151 insertions(+), 128 deletions(-) diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp index 2bcb41c4..ea84615f 100644 --- a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp @@ -56,76 +56,21 @@ namespace gazebo_ros2_control { +class GazeboRosControlPrivate; class GazeboRosControlPlugin : public gazebo::ModelPlugin { public: - virtual ~GazeboRosControlPlugin(); + GazeboRosControlPlugin(); + + ~GazeboRosControlPlugin(); // Overloaded Gazebo entry point void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) override; - // 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::shared_ptr e_stop_active); - - // Node Handles - rclcpp::Node::SharedPtr 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_; - std::string param_file; - - // Transmissions in this plugin's scope - std::vector transmissions_; - - // Robot simulator interface - std::string robot_hw_sim_type_str_; - std::shared_ptr robot_hw_sim_; - rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; - // executor_->spin causes lockups, us ethis alternative for now - std::thread thread_executor_spin_; - - // Controller manager - std::shared_ptr controller_manager_; - std::vector> controllers_; - - // Timing - rclcpp::Duration control_period_ = rclcpp::Duration(0); - rclcpp::Time last_update_sim_time_ros_; - rclcpp::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_; - // Emergency stop subscriber - rclcpp::Subscription::SharedPtr e_stop_sub_; +private: + /// Private data pointer + std::unique_ptr impl_; }; } // namespace gazebo_ros2_control diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index a6b6eaf0..be030ef2 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -44,10 +44,80 @@ namespace gazebo_ros2_control { +class GazeboRosControlPrivate +{ +public: + // 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); + + void eStopCB(const std::shared_ptr e_stop_active); + + // Node Handles + rclcpp::Node::SharedPtr 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_; + std::string param_file_; + + // Transmissions in this plugin's scope + std::vector transmissions_; + + // Robot simulator interface + std::string robot_hw_sim_type_str_; + std::shared_ptr robot_hw_sim_; + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + // executor_->spin causes lockups, us ethis alternative for now + std::thread thread_executor_spin_; + + // Controller manager + std::shared_ptr controller_manager_; + std::vector> controllers_; + + // Timing + rclcpp::Duration control_period_ = rclcpp::Duration(0); + rclcpp::Time last_update_sim_time_ros_; + rclcpp::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_; + // Emergency stop subscriber + rclcpp::Subscription::SharedPtr e_stop_sub_; +}; + +GazeboRosControlPlugin::GazeboRosControlPlugin() +: impl_(std::make_unique()) +{ +} + GazeboRosControlPlugin::~GazeboRosControlPlugin() { // Disconnect from gazebo events - update_connection_.reset(); + impl_->update_connection_.reset(); } // Overloaded Gazebo entry point @@ -58,11 +128,11 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element "Loading gazebo_ros2_control plugin"); // Save pointers to the model - parent_model_ = parent; - sdf_ = sdf; + impl_->parent_model_ = parent; + impl_->sdf_ = sdf; // Error message if the model couldn't be found - if (!parent_model_) { + if (!impl_->parent_model_) { RCLCPP_ERROR_STREAM(rclcpp::get_logger("loadThread"), "parent model is NULL"); return; } @@ -78,36 +148,36 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } // Get namespace for nodehandle - if (sdf_->HasElement("robotNamespace")) { - robot_namespace_ = sdf_->GetElement("robotNamespace")->Get(); + if (impl_->sdf_->HasElement("robotNamespace")) { + impl_->robot_namespace_ = impl_->sdf_->GetElement("robotNamespace")->Get(); } else { - robot_namespace_ = parent_model_->GetName(); // default + impl_->robot_namespace_ = impl_->parent_model_->GetName(); // default } // Get robot_description ROS param name - if (sdf_->HasElement("robotParam")) { - robot_description_ = sdf_->GetElement("robotParam")->Get(); + if (impl_->sdf_->HasElement("robotParam")) { + impl_->robot_description_ = impl_->sdf_->GetElement("robotParam")->Get(); } else { - robot_description_ = "robot_description"; // default + impl_->robot_description_ = "robot_description"; // default } // Get the robot simulation interface type - if (sdf_->HasElement("robotSimType")) { - robot_hw_sim_type_str_ = sdf_->Get("robotSimType"); + if (impl_->sdf_->HasElement("robotSimType")) { + impl_->robot_hw_sim_type_str_ = impl_->sdf_->Get("robotSimType"); } else { - robot_hw_sim_type_str_ = "gazebo_ros2_control/DefaultRobotHWSim"; + impl_->robot_hw_sim_type_str_ = "gazebo_ros2_control/DefaultRobotHWSim"; RCLCPP_DEBUG_STREAM( rclcpp::get_logger( "loadThread"), "Using default plugin for RobotHWSim (none specified in URDF/SDF)\"" << - robot_hw_sim_type_str_ << "\""); + impl_->robot_hw_sim_type_str_ << "\""); } // temporary fix to bug regarding the robotNamespace in default_robot_hw_sim.cpp (see #637) - std::string robot_ns = robot_namespace_; - if (robot_hw_sim_type_str_ == "gazebo_ros2_control/DefaultRobotHWSim") { - if (sdf_->HasElement("legacyModeNS")) { - if (sdf_->GetElement("legacyModeNS")->Get() ) { + std::string robot_ns = impl_->robot_namespace_; + if (impl_->robot_hw_sim_type_str_ == "gazebo_ros2_control/DefaultRobotHWSim") { + if (impl_->sdf_->HasElement("legacyModeNS")) { + if (impl_->sdf_->GetElement("legacyModeNS")->Get() ) { robot_ns = ""; } } else { @@ -124,8 +194,8 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } } - if (sdf_->HasElement("parameters")) { - param_file = sdf_->GetElement("parameters")->Get(); + if (impl_->sdf_->HasElement("parameters")) { + impl_->param_file_ = impl_->sdf_->GetElement("parameters")->Get(); } else { RCLCPP_ERROR( rclcpp::get_logger( @@ -134,61 +204,63 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element // Get the Gazebo simulation period #if GAZEBO_MAJOR_VERSION >= 8 - rclcpp::Duration gazebo_period(parent_model_->GetWorld()->Physics()->GetMaxStepSize()); + rclcpp::Duration gazebo_period(impl_->parent_model_->GetWorld()->Physics()->GetMaxStepSize()); #else - rclcpp::Duration gazebo_period(parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize()); + rclcpp::Duration gazebo_period( + impl_->parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize()); #endif // Decide the plugin control period - if (sdf_->HasElement("controlPeriod")) { - control_period_ = rclcpp::Duration(sdf_->Get("controlPeriod")); + if (impl_->sdf_->HasElement("controlPeriod")) { + impl_->control_period_ = rclcpp::Duration(impl_->sdf_->Get("controlPeriod")); // Check the period against the simulation period - if (control_period_ < gazebo_period) { + if (impl_->control_period_ < gazebo_period) { RCLCPP_ERROR_STREAM( rclcpp::get_logger( "gazebo_ros2_control"), - "Desired controller update period (" << control_period_.seconds() << + "Desired controller update period (" << impl_->control_period_.seconds() << " s) is faster than the gazebo simulation period (" << gazebo_period.seconds() << " s)."); - } else if (control_period_ > gazebo_period) { + } else if (impl_->control_period_ > gazebo_period) { RCLCPP_WARN_STREAM( rclcpp::get_logger( "gazebo_ros2_control"), - " Desired controller update period (" << control_period_.seconds() << + " Desired controller update period (" << impl_->control_period_.seconds() << " s) is slower than the gazebo simulation period (" << gazebo_period.seconds() << " s)."); } } else { - control_period_ = gazebo_period; + impl_->control_period_ = gazebo_period; RCLCPP_DEBUG_STREAM( rclcpp::get_logger( "gazebo_ros2_control"), "Control period not found in URDF/SDF, defaulting to Gazebo period of " << - control_period_.seconds()); + impl_->control_period_.seconds()); } // Get parameters/settings for controllers from ROS param server - model_nh_ = std::make_shared(parent_model_->GetName(), robot_namespace_); + impl_->model_nh_ = std::make_shared( + impl_->parent_model_->GetName(), + impl_->robot_namespace_); // Initialize the emergency stop code. - e_stop_active_ = false; - last_e_stop_active_ = false; - if (sdf_->HasElement("eStopTopic")) { - using std::placeholders::_1; - const std::string e_stop_topic = sdf_->GetElement("eStopTopic")->Get(); - e_stop_sub_ = model_nh_->create_subscription( + impl_->e_stop_active_ = false; + impl_->last_e_stop_active_ = false; + if (impl_->sdf_->HasElement("eStopTopic")) { + const std::string e_stop_topic = impl_->sdf_->GetElement("eStopTopic")->Get(); + impl_->e_stop_sub_ = impl_->model_nh_->create_subscription( e_stop_topic, 1, - std::bind(&GazeboRosControlPlugin::eStopCB, this, _1)); + std::bind(&GazeboRosControlPrivate::eStopCB, impl_.get(), std::placeholders::_1)); } RCLCPP_INFO( rclcpp::get_logger( "gazebo_ros2_control"), "Starting gazebo_ros2_control plugin in namespace: %s", - robot_namespace_.c_str()); + impl_->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)) { + const std::string urdf_string = impl_->getURDF(impl_->robot_description_); + if (!impl_->parseTransmissionsFromURDF(urdf_string)) { RCLCPP_ERROR( rclcpp::get_logger( "gazebo_ros2_control"), @@ -198,20 +270,21 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element // Load the RobotHWSim abstraction to interface the controllers with the gazebo model try { - robot_hw_sim_loader_.reset( + impl_->robot_hw_sim_loader_.reset( new pluginlib::ClassLoader( "gazebo_ros2_control", "gazebo_ros2_control::RobotHWSim")); - robot_hw_sim_ = robot_hw_sim_loader_->createSharedInstance(robot_hw_sim_type_str_); + impl_->robot_hw_sim_ = impl_->robot_hw_sim_loader_->createSharedInstance( + impl_->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_)) + if (!impl_->robot_hw_sim_->initSim( + robot_ns, impl_->model_nh_, impl_->parent_model_, urdf_model_ptr, + impl_->transmissions_)) { RCLCPP_FATAL( rclcpp::get_logger( @@ -219,15 +292,15 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element return; } - executor_ = std::make_shared(); - executor_->add_node(model_nh_); + impl_->executor_ = std::make_shared(); + impl_->executor_->add_node(impl_->model_nh_); // Create the controller manager RCLCPP_ERROR(rclcpp::get_logger("ros2_control_plugin"), "Loading controller_manager"); #if 1 // @todo - controller_manager_.reset( + impl_->controller_manager_.reset( new controller_manager::ControllerManager( - robot_hw_sim_, executor_, + impl_->robot_hw_sim_, impl_->executor_, "gazebo_controller_manager")); #endif #if 1 @@ -338,7 +411,7 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } }; - YAML::Node root_node = YAML::LoadFile(param_file); + YAML::Node root_node = YAML::LoadFile(impl_->param_file_); for (auto yaml : root_node) { auto controller_name = yaml.first.as(); for (auto yaml_node_it : yaml.second) { // ros__parameters @@ -346,11 +419,14 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element auto param_name = yaml_node_it2.first.as(); if (param_name == "type") { auto controller_type = yaml_node_it2.second.as(); - auto controller = controller_manager_->load_controller( + auto controller = impl_->controller_manager_->load_controller( controller_name, controller_type); - controllers_.push_back(controller); - load_params_from_yaml(controller->get_lifecycle_node(), param_file, controller_name); + impl_->controllers_.push_back(controller); + load_params_from_yaml( + controller->get_lifecycle_node(), + impl_->param_file_, + controller_name); } } } @@ -359,26 +435,28 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element auto spin = [this]() { while (rclcpp::ok()) { - executor_->spin_once(); + impl_->executor_->spin_once(); } }; - thread_executor_spin_ = std::thread(spin); + impl_->thread_executor_spin_ = std::thread(spin); - if (controller_manager_->configure() != + if (impl_->controller_manager_->configure() != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { RCLCPP_ERROR(rclcpp::get_logger("cm"), "failed to configure"); } - if (controller_manager_->activate() != controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) { + if (impl_->controller_manager_->activate() != + controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) + { RCLCPP_ERROR(rclcpp::get_logger("cm"), "failed to activate"); } #endif // Listen to the update event. This event is broadcast every simulation iteration. - update_connection_ = + impl_->update_connection_ = gazebo::event::Events::ConnectWorldUpdateBegin( boost::bind( - &GazeboRosControlPlugin::Update, - this)); + &GazeboRosControlPrivate::Update, + impl_.get())); } catch (pluginlib::LibraryLoadException & ex) { RCLCPP_ERROR( rclcpp::get_logger( @@ -396,7 +474,7 @@ spin(std::shared_ptr exe) } // Called by the world update start event -void GazeboRosControlPlugin::Update() +void GazeboRosControlPrivate::Update() { // Get the simulation time and period #if GAZEBO_MAJOR_VERSION >= 8 @@ -441,7 +519,7 @@ void GazeboRosControlPlugin::Update() } // Called on world reset -void GazeboRosControlPlugin::Reset() +void GazeboRosControlPrivate::Reset() { // Reset timing variables to not pass negative update periods to controllers on world reset last_update_sim_time_ros_ = rclcpp::Time(); @@ -449,7 +527,7 @@ void GazeboRosControlPlugin::Reset() } // Get the URDF XML from the parameter server -std::string GazeboRosControlPlugin::getURDF(std::string param_name) const +std::string GazeboRosControlPrivate::getURDF(std::string param_name) const { std::string urdf_string; @@ -496,14 +574,14 @@ std::string GazeboRosControlPlugin::getURDF(std::string param_name) const } // Get Transmissions from the URDF -bool GazeboRosControlPlugin::parseTransmissionsFromURDF(const std::string & urdf_string) +bool GazeboRosControlPrivate::parseTransmissionsFromURDF(const std::string & urdf_string) { transmission_interface::TransmissionParser::parse(urdf_string, transmissions_); return true; } // Emergency stop callback -void GazeboRosControlPlugin::eStopCB(const std::shared_ptr e_stop_active) +void GazeboRosControlPrivate::eStopCB(const std::shared_ptr e_stop_active) { e_stop_active_ = e_stop_active->data; } From 83254fa148bd7053ab8d085d508b32352093e98c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 29 May 2020 13:57:37 +0200 Subject: [PATCH 8/8] Removed comment Signed-off-by: ahcorde --- gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp index be030ef2..27828c31 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -320,9 +320,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element static constexpr char separator = '.'; if (yaml_node.Type() == YAML::NodeType::Scalar) { std::string val_str = yaml_node.as(); - // const char* val_str = yaml_node.as(); - // std::string val_str; - // YAML::convert::decode(yaml_node, val_str); // TODO(ddengster): Do stricter typing for set_parameter value types. // (ie. Numbers should be converted to int/double/etc instead of strings)