diff --git a/gazebo_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt new file mode 100644 index 00000000..93be9710 --- /dev/null +++ b/gazebo_ros2_control/CMakeLists.txt @@ -0,0 +1,70 @@ +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) + +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} +) + +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..0dd9548b --- /dev/null +++ b/gazebo_ros2_control/README.md @@ -0,0 +1,7 @@ +# Gazebo ros_control Interfaces + +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 +controller manager and connects it to a Gazebo model. 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..9225ed92 --- /dev/null +++ b/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp @@ -0,0 +1,168 @@ +// 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/physics/Model.hh" +#include "gazebo/physics/Joint.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.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp new file mode 100644 index 00000000..ea84615f --- /dev/null +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_ros2_control_plugin.hpp @@ -0,0 +1,77 @@ +// 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 GazeboRosControlPrivate; + +class GazeboRosControlPlugin : public gazebo::ModelPlugin +{ +public: + GazeboRosControlPlugin(); + + ~GazeboRosControlPlugin(); + + // Overloaded Gazebo entry point + void Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf) override; + +private: + /// Private data pointer + std::unique_ptr impl_; +}; +} // 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.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp new file mode 100644 index 00000000..21235fd4 --- /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 "hardware_interface/robot_hardware.hpp" +#include "rclcpp/rclcpp.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 new file mode 100644 index 00000000..7c8fabe1 --- /dev/null +++ b/gazebo_ros2_control/package.xml @@ -0,0 +1,41 @@ + + + gazebo_ros2_control + 0.0.1 + gazebo_ros2_control + + Alejandro Hernandez + + BSD + + http://ros.org/wiki/gazebo_ros_control + https://github.com/ros-simulation/gazebo_ros2_control/issues + https://github.com/ros-simulation/gazebo_ros2_control/ + + 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_lint_common + ament_lint_auto + ament_cmake_gtest + + + 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..1bbee73e --- /dev/null +++ b/gazebo_ros2_control/src/default_robot_hw_sim.cpp @@ -0,0 +1,637 @@ +// 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 "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, + gazebo::physics::ModelPtr parent_model, + const urdf::Model * const urdf_model, + std::vector transmissions) +{ + // getJointLimits() searches joint_limit_nh for joint limit parameters. The format of each + // parameter's name is "joint_limits/". An example is "joint_limits/axle_joint". + 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(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 . " << + "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 + 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]; + // 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; + sim_joints_[j]->SetForce(0, effort); + } + break; + + case VELOCITY: +#if GAZEBO_MAJOR_VERSION > 2 + 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]; + // 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; + 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; + } + } +} + +} // 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 new file mode 100644 index 00000000..27828c31 --- /dev/null +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -0,0 +1,588 @@ +// 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 "gazebo_ros2_control/gazebo_ros2_control_plugin.hpp" + +#include "urdf/model.h" +#include "yaml-cpp/yaml.h" + +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 + impl_->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 + impl_->parent_model_ = parent; + impl_->sdf_ = sdf; + + // Error message if the model couldn't be found + if (!impl_->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 (impl_->sdf_->HasElement("robotNamespace")) { + impl_->robot_namespace_ = impl_->sdf_->GetElement("robotNamespace")->Get(); + } else { + impl_->robot_namespace_ = impl_->parent_model_->GetName(); // default + } + + // Get robot_description ROS param name + if (impl_->sdf_->HasElement("robotParam")) { + impl_->robot_description_ = impl_->sdf_->GetElement("robotParam")->Get(); + } else { + impl_->robot_description_ = "robot_description"; // default + } + + // Get the robot simulation interface type + if (impl_->sdf_->HasElement("robotSimType")) { + impl_->robot_hw_sim_type_str_ = impl_->sdf_->Get("robotSimType"); + } else { + 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)\"" << + 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 = 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 { + 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 (impl_->sdf_->HasElement("parameters")) { + impl_->param_file_ = impl_->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(impl_->parent_model_->GetWorld()->Physics()->GetMaxStepSize()); +#else + rclcpp::Duration gazebo_period( + impl_->parent_model_->GetWorld()->GetPhysicsEngine()->GetMaxStepSize()); +#endif + + // Decide the plugin control period + if (impl_->sdf_->HasElement("controlPeriod")) { + impl_->control_period_ = rclcpp::Duration(impl_->sdf_->Get("controlPeriod")); + + // Check the period against the simulation period + if (impl_->control_period_ < gazebo_period) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger( + "gazebo_ros2_control"), + "Desired controller update period (" << impl_->control_period_.seconds() << + " s) is faster than the gazebo simulation period (" << gazebo_period.seconds() << " s)."); + } else if (impl_->control_period_ > gazebo_period) { + RCLCPP_WARN_STREAM( + rclcpp::get_logger( + "gazebo_ros2_control"), + " Desired controller update period (" << impl_->control_period_.seconds() << + " s) is slower than the gazebo simulation period (" << gazebo_period.seconds() << " s)."); + } + } else { + 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 " << + impl_->control_period_.seconds()); + } + + // Get parameters/settings for controllers from ROS param server + impl_->model_nh_ = std::make_shared( + impl_->parent_model_->GetName(), + impl_->robot_namespace_); + + // Initialize the emergency stop code. + 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(&GazeboRosControlPrivate::eStopCB, impl_.get(), std::placeholders::_1)); + } + RCLCPP_INFO( + rclcpp::get_logger( + "gazebo_ros2_control"), "Starting gazebo_ros2_control plugin in namespace: %s", + 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 = impl_->getURDF(impl_->robot_description_); + if (!impl_->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 { + impl_->robot_hw_sim_loader_.reset( + new pluginlib::ClassLoader( + "gazebo_ros2_control", + "gazebo_ros2_control::RobotHWSim")); + + 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 (!impl_->robot_hw_sim_->initSim( + robot_ns, impl_->model_nh_, impl_->parent_model_, urdf_model_ptr, + impl_->transmissions_)) + { + RCLCPP_FATAL( + rclcpp::get_logger( + "gazebo_ros2_control"), "Could not initialize robot simulation interface"); + return; + } + + 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 + impl_->controller_manager_.reset( + new controller_manager::ControllerManager( + impl_->robot_hw_sim_, impl_->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(); + + // 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)); + } + + 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)); + } + } 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); + }; + 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(impl_->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 = impl_->controller_manager_->load_controller( + controller_name, + controller_type); + impl_->controllers_.push_back(controller); + load_params_from_yaml( + controller->get_lifecycle_node(), + impl_->param_file_, + controller_name); + } + } + } + } + + auto spin = [this]() + { + while (rclcpp::ok()) { + impl_->executor_->spin_once(); + } + }; + impl_->thread_executor_spin_ = std::thread(spin); + + if (impl_->controller_manager_->configure() != + controller_interface::CONTROLLER_INTERFACE_RET_SUCCESS) + { + RCLCPP_ERROR(rclcpp::get_logger("cm"), "failed to configure"); + } + 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. + impl_->update_connection_ = + gazebo::event::Events::ConnectWorldUpdateBegin( + boost::bind( + &GazeboRosControlPrivate::Update, + impl_.get())); + } 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 GazeboRosControlPrivate::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_); + + // 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 GazeboRosControlPrivate::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 GazeboRosControlPrivate::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..."); + + return urdf_string; +} + +// Get Transmissions from the URDF +bool GazeboRosControlPrivate::parseTransmissionsFromURDF(const std::string & urdf_string) +{ + transmission_interface::TransmissionParser::parse(urdf_string, transmissions_); + return true; +} + +// Emergency stop callback +void GazeboRosControlPrivate::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 gazebo_ros2_control