Added initial version of gazebo_ros2_control

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()

gazebo_ros2_control/README.md

@@ -0,0 +1,7 @@
+# Gazebo ros_control Interfaces
+
+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.

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 <control_toolbox/pid.hpp>
+#endif
+#if 0 //@todo
+#include <hardware_interface/joint_command_interface.hpp>
+#include <hardware_interface/robot_hw.hpp>
+#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.h>
+
+// 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<transmission_interface::TransmissionInfo> 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<std::string> joint_names_;
+  std::vector<int> joint_types_;
+  std::vector<double> joint_lower_limits_;
+  std::vector<double> joint_upper_limits_;
+  std::vector<double> joint_effort_limits_;
+  std::vector<double> joint_vel_limits_;
+  std::vector<ControlMethod> joint_control_methods_;
+#if 0
+  std::vector<control_toolbox::Pid> pid_controllers_;
+#endif
+  std::vector<double> joint_position_;
+  std::vector<double> joint_velocity_;
+  std::vector<double> joint_effort_;
+  std::vector<double> joint_effort_command_;
+  std::vector<double> joint_position_command_;
+  std::vector<double> last_joint_position_command_;
+  std::vector<double> joint_velocity_command_;
+  std::vector<hardware_interface::OperationMode> joint_opmodes_;
+
+  std::vector<gazebo::physics::JointPtr> sim_joints_;
+
+  std::vector<hardware_interface::JointStateHandle> joint_states_;
+  std::vector<hardware_interface::JointCommandHandle> joint_cmds_;
+  std::vector<hardware_interface::JointCommandHandle> joint_eff_cmdhandle_;
+  std::vector<hardware_interface::JointCommandHandle> joint_vel_cmdhandle_;
+  std::vector<hardware_interface::OperationModeHandle> joint_opmodehandles_;
+
+  //limits
+  std::vector<joint_limits_interface::PositionJointSaturationHandle> joint_pos_limit_handles_;
+  std::vector<joint_limits_interface::PositionJointSoftLimitsHandle> joint_pos_soft_limit_handles_;
+  std::vector<joint_limits_interface::EffortJointSaturationHandle> joint_eff_limit_handles_;
+  std::vector<joint_limits_interface::EffortJointSoftLimitsHandle> joint_eff_soft_limit_handles_;
+  std::vector<joint_limits_interface::VelocityJointSaturationHandle> joint_vel_limit_handles_;
+  //std::vector<joint_limits_interface::VelocityJointSoftLimitsHandle> 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<DefaultRobotHWSim> DefaultRobotHWSimPtr;
+
+}
+
+#endif // #ifndef __GAZEBO_ROS_CONTROL_PLUGIN_DEFAULT_ROBOT_HW_SIM_H_

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 <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<std_msgs::msg::Bool> 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<pluginlib::ClassLoader<gazebo_ros2_control::RobotHWSim> > 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<transmission_interface::TransmissionInfo> transmissions_;
+
+  // Robot simulator interface
+  std::string robot_hw_sim_type_str_;
+  std::shared_ptr<gazebo_ros2_control::RobotHWSim> 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::ControllerManager> controller_manager_;
+  std::vector<std::shared_ptr<controller_interface::ControllerInterface>> 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<std_msgs::msg::Bool>::SharedPtr e_stop_sub_;  // Emergency stop subscriber
+
+};
+
+
+}