diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index feb46c4e..0317fcab 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -18,16 +18,13 @@ jobs: cd .. mkdir -p /home/ros2_ws/src cp -r gazebo_ros2_control /home/ros2_ws/src/ - apt-get upgrade -q -y + apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -q -y --no-install-recommends \ dirmngr \ gnupg2 \ lsb-release \ python3-colcon-ros - apt-get upgrade -y cd /home/ros2_ws/src/ - git clone https://github.com/ros-controls/ros2_control - git clone https://github.com/ros-controls/ros2_controllers rosdep update rosdep install --from-paths ./ -i -y --rosdistro foxy \ --ignore-src @@ -36,7 +33,6 @@ jobs: run: | cd /home/ros2_ws/ . /opt/ros/foxy/local_setup.sh - export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH colcon build --packages-up-to gazebo_ros2_control_demos - name: Run tests id: test diff --git a/Docker/Dockerfile b/Docker/Dockerfile index 2b079289..785b3f87 100644 --- a/Docker/Dockerfile +++ b/Docker/Dockerfile @@ -15,16 +15,13 @@ RUN apt-get update && apt-get install -q -y --no-install-recommends \ RUN mkdir -p /home/ros2_ws/src \ && cd /home/ros2_ws/src/ \ && git clone https://github.com/ros-simulation/gazebo_ros2_control \ - && git clone https://github.com/ros-controls/ros2_control \ - && git clone https://github.com/ros-controls/ros2_controllers \ && rosdep update \ && rosdep install --from-paths ./ -i -y --rosdistro foxy \ --ignore-src RUN cd /home/ros2_ws/ \ && . /opt/ros/foxy/setup.sh \ - && export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH \ - && colcon build + && colcon build --merge-install COPY entrypoint.sh /entrypoint.sh ENTRYPOINT ["/entrypoint.sh"] diff --git a/README.md b/README.md index cec0ba74..c86c5636 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,5 @@ # gazebo_ros2_control - This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Gazebo](http://gazebosim.org/) simulator. This package provides a Gazebo plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model. @@ -13,7 +12,7 @@ It is running Gazebo and some other ROS 2 nodes. ## Video + Pictures -![](img/gazebo_ros2_control_position_pid.gif) +![](img/gazebo_ros2_control_position.gif) ## Running @@ -61,20 +60,36 @@ ros2 run gazebo_ros2_control_demos example_position ``` -## Add transmission elements to a URDF +## Add ros2_control tag to a URDF -To use `ros2_control` with your robot, you need to add some additional elements to your URDF. The `` element is used to link actuators to joints, see the `` spec for exact XML format. +To use `ros2_control` with your robot, you need to add some additional elements to your URDF. +You should include the tag `` to access and control the robot interfaces. We should +include -For the purposes of `gazebo_ros2_control` in its current implementation, the only important information in these transmission tags are: + - a specific `` for our robot + - `` tag including the robot controllers: commands and states. - - `` the name must correspond to a joint else where in your URDF - - `` the type of transmission. Currently only `transmission_interface/SimpleTransmission` is implemented. - - `` within the `` and `` tags, this tells the `gazebo_ros2_control` plugin what hardware interface to load (position, velocity or effort interfaces). +```xml + + + gazebo_ros2_control/GazeboSystem + + + + -1000 + 1000 + + + + + + +``` ## Add the gazebo_ros2_control plugin -In addition to the transmission tags, a Gazebo plugin needs to be added to your URDF that -actually parses the transmission tags and loads the appropriate hardware interfaces and +In addition to the `ros2_control` tags, a Gazebo plugin needs to be added to your URDF that +actually parses the `ros2_control` tags and loads the appropriate hardware interfaces and controller manager. By default the `gazebo_ros2_control` plugin is very simple, though it is also extensible via an additional plugin architecture to allow power users to create their own custom robot hardware interfaces between `ros2_control` and Gazebo. @@ -82,26 +97,22 @@ robot hardware interfaces between `ros2_control` and Gazebo. ```xml - gazebo_ros2_control/DefaultRobotHWSim robot_description robot_state_publisher $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml - False ``` The `gazebo_ros2_control` `` tag also has the following optional child elements: - - ``: The period of the controller update (in seconds), defaults to Gazebo's period - ``: The location of the `robot_description` (URDF) on the parameter server, defaults to `robot_description` - ``: Name of the node where the `robot_param` is located, defauls to `robot_state_publisher` - - ``: The pluginlib name of a custom robot sim interface to be used, defaults to `gazebo_ros2_control/DefaultRobotHWSim` - ``: YAML file with the configuration of the controllers #### Default gazebo_ros2_control Behavior -By default, without a `` tag, `gazebo_ros2_control` will attempt to get all of the information it needs to interface with a ros2_control-based controller out of the URDF. This is sufficient for most cases, and good for at least getting started. +By default, without a `` tag, `gazebo_ros2_control` will attempt to get all of the information it needs to interface with a ros2_control-based controller out of the URDF. This is sufficient for most cases, and good for at least getting started. The default behavior provides the following ros2_control interfaces: @@ -113,14 +124,21 @@ The default behavior provides the following ros2_control interfaces: The `gazebo_ros2_control` Gazebo plugin also provides a pluginlib-based interface to implement custom interfaces between Gazebo and `ros2_control` for simulating more complex mechanisms (nonlinear springs, linkages, etc). -These plugins must inherit `gazebo_ros2_control::RobotHWSim` which implements a simulated `ros2_control` `hardware_interface::RobotHW`. RobotHWSim provides API-level access to read and command joint properties in the Gazebo simulator. - -The respective RobotHWSim sub-class is specified in a URDF model and is loaded when the robot model is loaded. For example, the following XML will load the default plugin (same behavior as when using no `` tag): +These plugins must inherit `gazebo_ros2_control::GazeboSystemInterface` which implements a simulated `ros2_control` +`hardware_interface::SystemInterface`. SystemInterface provides API-level access to read and command joint properties. +The respective GazeboSystemInterface sub-class is specified in a URDF model and is loaded when the +robot model is loaded. For example, the following XML will load the default plugin: ```xml + + + gazebo_ros2_control/GazeboSystem + + ... + - gazebo_ros2_control/DefaultRobotHWSim + ... ``` @@ -138,13 +156,13 @@ Use the tag `` inside `` to set the YAML file with the contr ``` This controller publishes the state of all resources registered to a -`hardware_interface::JointStateInterface` to a topic of type `sensor_msgs/msg/JointState`. The following is a basic configuration of the controller. +`hardware_interface::StateInterface` to a topic of type `sensor_msgs/msg/JointState`. +The following is a basic configuration of the controller. ```yaml joint_state_controller: ros__parameters: type: joint_state_controller/JointStateController - publish_rate: 50 ``` This controller creates an action called `/cart_pole_controller/follow_joint_trajectory` of type `control_msgs::action::FollowJointTrajectory`. @@ -157,37 +175,7 @@ cart_pole_controller: - slider_to_cart write_op_modes: - slider_to_cart - state_publish_rate: 25 - action_monitor_rate: 20 - constraints: - stopped_velocity_tolerance: 0.05 - goal_time: 5 -``` - -#### Setting PID gains - -To set the PID gains for a specific joint you need to define them inside ``. Using the generic way of defining parameters with `gazebo_ros`. The name of the parameter correspond the name of the joint followed by a dot and the name of the parameter: `p`, `i`, `d`, `i_clamp_max`, `i_clamp_min` and `antiwindup`. - -```xml - - - - /my_robot - 50.0 - 10.0 - 15.0 - 3.0 - -3.0 - false - e_stop_topic:=emergency_stop - - gazebo_ros2_control/DefaultRobotHWSim - $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml - ... - - ``` - #### Executing the examples There are some examples in the `gazebo_ros2_control_demos` package. These examples allow to launch a cart in a 30 meter rail. @@ -197,9 +185,9 @@ There are some examples in the `gazebo_ros2_control_demos` package. These exampl You can run some of the configuration running the following commands: ```bash -ros2 launch gazebo_ros2_control_demos cart_example_position_pid.launch.py ros2 launch gazebo_ros2_control_demos cart_example_position.launch.py ros2 launch gazebo_ros2_control_demos cart_example_velocity.launch.py +ros2 launch gazebo_ros2_control_demos cart_example_effort.launch.py ``` Send example commands: @@ -209,30 +197,18 @@ When the Gazebo world is launched you can run some of the following commads to m ```bash ros2 run gazebo_ros2_control_demos example_position ros2 run gazebo_ros2_control_demos example_velocity +ros2 run gazebo_ros2_control_demos example_effort ``` -To get or modify the values of the PID controller you can run the following commands: +#### Gazebo + Moveit2 + ROS 2 -```bash -ros2 param get /gazebo_ros2_control slider_to_cart.p -ros2 param get /gazebo_ros2_control slider_to_cart.i -ros2 param get /gazebo_ros2_control slider_to_cart.d -ros2 param get /gazebo_ros2_control slider_to_cart.i_clamp_max -ros2 param get /gazebo_ros2_control slider_to_cart.i_clamp_min -``` +This example works with [ROS 2 Foxy](https://index.ros.org/doc/ros2/Installation/Foxy/). +You should install Moveit2 from sources, the instructions are available in this [link](https://moveit.ros.org/install-moveit2/source/). + +The repository with all the required packages are in the [gazebo_ros_demos](https://github.com/ros-simulation/gazebo_ros_demos/tree/ahcorde/port/ros2). ```bash -ros2 param set /gazebo_ros2_control slider_to_cart.p 50.0 -ros2 param set /gazebo_ros2_control slider_to_cart.i 10.0 -ros2 param set /gazebo_ros2_control slider_to_cart.d 15.0 -ros2 param set /gazebo_ros2_control slider_to_cart.i_clamp_max 3.0 -ros2 param set /gazebo_ros2_control slider_to_cart.i_clamp_min -3.0 +ros2 launch rrbot_moveit_demo_nodes rrbot_demo.launch.py ``` -# NOTES - -`ros2_control` and `ros2_controllers` are in a redesign phase. Some of the packages are unofficial. -In the following links you can track some of the missing features. - - - https://github.com/ros-controls/ros2_control/issues/26 - - https://github.com/ros-controls/ros2_control/issues/32 +![](img/moveit2.gif) diff --git a/gazebo_ros2_control/CMakeLists.txt b/gazebo_ros2_control/CMakeLists.txt index 1d689ffa..0a745663 100644 --- a/gazebo_ros2_control/CMakeLists.txt +++ b/gazebo_ros2_control/CMakeLists.txt @@ -3,15 +3,12 @@ project(gazebo_ros2_control) find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) -find_package(control_toolbox REQUIRED) find_package(controller_manager REQUIRED) find_package(gazebo_dev REQUIRED) find_package(gazebo_ros REQUIRED) find_package(hardware_interface REQUIRED) -find_package(joint_limits_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) -find_package(transmission_interface REQUIRED) find_package(urdf REQUIRED) find_package(yaml_cpp_vendor REQUIRED) @@ -19,36 +16,45 @@ find_package(yaml_cpp_vendor REQUIRED) if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wconversion -Wno-sign-conversion -Wpedantic -Wnon-virtual-dtor -Woverloaded-virtual) +endif() include_directories(include) +link_directories( + ${gazebo_dev_LIBRARY_DIRS} +) # Libraries -add_library(${PROJECT_NAME} SHARED src/gazebo_ros2_control_plugin.cpp) +add_library(${PROJECT_NAME} SHARED + src/gazebo_ros2_control_plugin.cpp +) ament_target_dependencies(${PROJECT_NAME} + angles controller_manager gazebo_dev gazebo_ros 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 +add_library(gazebo_hardware_plugins SHARED + src/gazebo_system.cpp +) +ament_target_dependencies(gazebo_hardware_plugins angles - control_toolbox gazebo_dev hardware_interface - joint_limits_interface rclcpp - transmission_interface ) ## Install -install(TARGETS default_robot_hw_sim ${PROJECT_NAME} +install(TARGETS + ${PROJECT_NAME} + gazebo_hardware_plugins ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -56,7 +62,10 @@ install(TARGETS default_robot_hw_sim ${PROJECT_NAME} ament_export_dependencies(ament_cmake) ament_export_dependencies(rclcpp) -ament_export_libraries(${PROJECT_NAME} default_robot_hw_sim) +ament_export_libraries( + ${PROJECT_NAME} + gazebo_hardware_plugins +) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -68,6 +77,6 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION include/${PROJECT_NAME} ) -pluginlib_export_plugin_description_file(gazebo_ros2_control robot_hw_sim_plugins.xml) +pluginlib_export_plugin_description_file(gazebo_ros2_control gazebo_hardware_plugins.xml) ament_package() diff --git a/gazebo_ros2_control/gazebo_hardware_plugins.xml b/gazebo_ros2_control/gazebo_hardware_plugins.xml new file mode 100644 index 00000000..5dc0497f --- /dev/null +++ b/gazebo_ros2_control/gazebo_hardware_plugins.xml @@ -0,0 +1,10 @@ + + + + GazeboPositionJoint + + + 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 deleted file mode 100644 index a1023cac..00000000 --- a/gazebo_ros2_control/include/gazebo_ros2_control/default_robot_hw_sim.hpp +++ /dev/null @@ -1,168 +0,0 @@ -// 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 -#include -#include - -// ros_control -#include "control_toolbox/pid_ros.hpp" - -#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/Joint.hh" -#include "gazebo/physics/Model.hh" - -// ROS -#include "angles/angles.h" -#include "pluginlib/class_list_macros.hpp" -#include "rclcpp/rclcpp.hpp" - -// URDF -#include "urdf/model.h" - -// gazebo_ros_control -#include "gazebo_ros2_control/robot_hw_sim.hpp" - -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}; - -private: - rclcpp::Node::SharedPtr nh_; - -protected: - /// \brief 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( - size_t joint_nr, - 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); - - /// \brief Refreshes all valid handle references in a collection. - /// Requests from the RobotHardware updated handle references. This is required after - /// any call to RobotHardware::register_joint() and before a handle is used or bound - /// to a controller since the internal implementation of register_joint binds to doubles - /// inside a std::vector and thus growth of that vector invalidates all existing - /// iterators (i.e. handles). - /// See https://github.com/ros-controls/ros2_control/issues/212 - /// If a handle in the collection is unset (no joint name or interface name) then it is skipped. - void bindJointHandles(std::vector & joint_iface_handles); - - unsigned int n_dof_; - 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_; - std::vector pid_controllers_; - std::vector joint_position_; - - /// \brief stores the joint positions on ESTOP activation - /// During ESTOP these positions will override the output position command value. - std::vector last_joint_position_command_; - - /// \brief handles to the joints from within Gazebo - std::vector sim_joints_; - - /// \brief The current positions of the joints - std::vector joint_pos_stateh_; - /// \brief The current velocities of the joints - std::vector joint_vel_stateh_; - /// \brief The current effort forces applied to the joints - std::vector joint_eff_stateh_; - - /// \brief The current position command value (if control mode is POSITION) of the joints - std::vector joint_pos_cmdh_; - /// \brief The current effort command value (if control mode is EFFORT) of the joints - std::vector joint_eff_cmdh_; - /// \brief The current velocity command value (if control mode is VELOCITY) of the joints - std::vector joint_vel_cmdh_; - - /// \brief The operational mode (active/inactive) state of the joints - std::vector joint_opmodes_; - - /// \brief operational mode handles of the joints pointing to values in the joint_opmodes_ - /// collection - std::vector joint_opmodehandles_; - - /// \brief Limits for the joints if defined in the URDF or Node parameters - /// The implementation of the joint limit will be chosen based on the URDF parameters and could be - /// one of the hard (saturation) or soft limits based on the control mode (effort, position or - /// velocity) - std::vector> joint_limit_handles_; - - std::string physics_type_; - bool usingODE; - - // 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 index e2d75a90..5d0b9499 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 @@ -39,19 +39,10 @@ #include #include -// ROS -#include "pluginlib/class_loader.hpp" -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/bool.hpp" +#include "controller_manager/controller_manager.hpp" -// Gazebo #include "gazebo/common/common.hh" -#include "gazebo/physics/physics.hh" - -// ros_control -#include "controller_manager/controller_manager.hpp" -#include "gazebo_ros2_control/robot_hw_sim.hpp" -#include "transmission_interface/transmission_parser.hpp" +#include "gazebo/physics/Model.hh" namespace gazebo_ros2_control { diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp new file mode 100644 index 00000000..3c53af21 --- /dev/null +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp @@ -0,0 +1,76 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_ +#define GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_ + +#include +#include +#include + +#include "angles/angles.h" + +#include "gazebo_ros2_control/gazebo_system_interface.hpp" + +#include "std_msgs/msg/bool.hpp" + +namespace gazebo_ros2_control +{ +// Forward declaration +class GazeboSystemPrivate; + +// These class must inherit `gazebo_ros2_control::GazeboSystemInterface` which implements a +// simulated `ros2_control` `hardware_interface::SystemInterface`. + +class GazeboSystem : public GazeboSystemInterface +{ +public: + // Documentation Inherited + hardware_interface::return_type configure(const hardware_interface::HardwareInfo & system_info) + override; + + // Documentation Inherited + std::vector export_state_interfaces() override; + + // Documentation Inherited + std::vector export_command_interfaces() override; + + // Documentation Inherited + hardware_interface::return_type start() override; + + // Documentation Inherited + hardware_interface::return_type stop() override; + + // Documentation Inherited + hardware_interface::return_type read() override; + + // Documentation Inherited + hardware_interface::return_type write() override; + + // Documentation Inherited + bool initSim( + rclcpp::Node::SharedPtr & model_nh, + gazebo::physics::ModelPtr parent_model, + const hardware_interface::HardwareInfo & hardware_info, + sdf::ElementPtr sdf) override; + +private: + /// \brief Private data class + std::unique_ptr dataPtr; +}; + +} // namespace gazebo_ros2_control + +#endif // GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_ diff --git a/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp new file mode 100644 index 00000000..598f884c --- /dev/null +++ b/gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp @@ -0,0 +1,94 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + + +#ifndef GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_HPP_ +#define GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_HPP_ + +#include +#include +#include + +#include "gazebo/physics/Joint.hh" +#include "gazebo/physics/Model.hh" +#include "gazebo/physics/physics.hh" + +#include "hardware_interface/base_interface.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +#include "rclcpp/rclcpp.hpp" + +// URDF +#include "urdf/model.h" + +namespace gazebo_ros2_control +{ + +template::type> +class SafeEnum +{ +public: + SafeEnum() + : mFlags(0) {} + explicit SafeEnum(ENUM singleFlag) + : mFlags(singleFlag) {} + SafeEnum(const SafeEnum & original) + : mFlags(original.mFlags) {} + + SafeEnum & operator|=(ENUM addValue) {mFlags |= addValue; return *this;} + SafeEnum operator|(ENUM addValue) {SafeEnum result(*this); result |= addValue; return result;} + SafeEnum & operator&=(ENUM maskValue) {mFlags &= maskValue; return *this;} + SafeEnum operator&(ENUM maskValue) {SafeEnum result(*this); result &= maskValue; return result;} + SafeEnum operator~() {SafeEnum result(*this); result.mFlags = ~result.mFlags; return result;} + explicit operator bool() {return mFlags != 0;} + +protected: + UNDERLYING mFlags; +}; + +// SystemInterface provides API-level access to read and command joint properties. +class GazeboSystemInterface + : public hardware_interface::BaseInterface +{ +public: + /// \brief Initilize the system interface + /// param[in] model_nh pointer to the ros2 node + /// param[in] parent_model pointer to the model + /// param[in] control_hardware vector filled with information about robot's control resources + /// param[in] sdf pointer to the SDF + virtual bool initSim( + rclcpp::Node::SharedPtr & model_nh, + gazebo::physics::ModelPtr parent_model, + const hardware_interface::HardwareInfo & hardware_info, + sdf::ElementPtr sdf) = 0; + + // Methods used to control a joint. + enum ControlMethod_ + { + NONE = 0, + POSITION = (1 << 0), + VELOCITY = (1 << 1), + EFFORT = (1 << 2), + }; + + typedef SafeEnum ControlMethod; + +protected: + rclcpp::Node::SharedPtr nh_; +}; + +} // namespace gazebo_ros2_control + +#endif // GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_INTERFACE_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 deleted file mode 100644 index 93d37eb2..00000000 --- a/gazebo_ros2_control/include/gazebo_ros2_control/robot_hw_sim.hpp +++ /dev/null @@ -1,135 +0,0 @@ -// 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() - { - return hardware_interface::return_type::ERROR; - } - - virtual hardware_interface::hardware_interface_ret_t read() - { - return hardware_interface::return_type::ERROR; - } - - virtual hardware_interface::hardware_interface_ret_t write() - { - return hardware_interface::return_type::ERROR; - } -}; - -} // 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 706ddc5b..16dd00a5 100644 --- a/gazebo_ros2_control/package.xml +++ b/gazebo_ros2_control/package.xml @@ -18,16 +18,13 @@ ament_cmake angles - control_toolbox gazebo_dev gazebo_ros controller_manager hardware_interface - joint_limits_interface pluginlib rclcpp std_msgs - transmission_interface urdf yaml_cpp_vendor @@ -37,6 +34,6 @@ ament_cmake - + diff --git a/gazebo_ros2_control/robot_hw_sim_plugins.xml b/gazebo_ros2_control/robot_hw_sim_plugins.xml deleted file mode 100644 index 1787efad..00000000 --- a/gazebo_ros2_control/robot_hw_sim_plugins.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - A default robot simulation interface which constructs joint handles from an SDF/URDF. - - - diff --git a/gazebo_ros2_control/src/default_robot_hw_sim.cpp b/gazebo_ros2_control/src/default_robot_hw_sim.cpp deleted file mode 100644 index 32c76367..00000000 --- a/gazebo_ros2_control/src/default_robot_hw_sim.cpp +++ /dev/null @@ -1,624 +0,0 @@ -// 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 "gazebo_ros2_control/default_robot_hw_sim.hpp" -#include "urdf/model.h" - -namespace gazebo_ros2_control -{ -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". - 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_); - joint_opmodes_.resize(n_dof_); - joint_opmodehandles_.resize(n_dof_); - - joint_pos_stateh_.resize(n_dof_, hardware_interface::JointHandle("position")); - joint_vel_stateh_.resize(n_dof_, hardware_interface::JointHandle("velocity")); - joint_eff_stateh_.resize(n_dof_, hardware_interface::JointHandle("effort")); - joint_pos_cmdh_.resize(n_dof_, hardware_interface::JointHandle("position_command")); - joint_eff_cmdh_.resize(n_dof_, hardware_interface::JointHandle("effort_command")); - joint_vel_cmdh_.resize(n_dof_, hardware_interface::JointHandle("velocity_command")); - - joint_limit_handles_.resize(n_dof_); - - // 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()) { - RCLCPP_ERROR(nh_->get_logger(), "No physics engine configured in Gazebo."); - return false; - } - - // special handing since ODE uses a different set/get interface then the other engines - usingODE = (physics_type_.compare("ode") == 0); - - // Initialize values - for (unsigned int j = 0; j < n_dof_; j++) { - // - // Perform some validation on the URDF joint and actuator spec - // - - // Check that this transmission has one joint - if (transmissions[j].joints.empty()) { - RCLCPP_WARN_STREAM( - nh_->get_logger(), "Transmission " << transmissions[j].name << - " has no associated joints."); - continue; - } else if (transmissions[j].joints.size() > 1) { - RCLCPP_WARN_STREAM( - nh_->get_logger(), "Transmission " << transmissions[j].name << - " has more than one joint. Currently the default robot hardware simulation " << - " interface only supports one."); - continue; - } - - std::string joint_name = joint_names_[j] = transmissions[j].joints[0].name; - - std::vector joint_interfaces = transmissions[j].joints[0].interfaces; - if (joint_interfaces.empty() && - !(transmissions[j].actuators.empty()) && - !(transmissions[j].actuators[0].interfaces.empty())) - { - // TODO(anyone): Deprecate HW interface specification in actuators in ROS J - joint_interfaces = transmissions[j].actuators[0].interfaces; - RCLCPP_WARN_STREAM( - nh_->get_logger(), "The element of transmission " << - 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()) { - RCLCPP_WARN_STREAM( - nh_->get_logger(), "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) { - RCLCPP_WARN_STREAM( - nh_->get_logger(), "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"); - // only a warning, allow joint to continue - } - - std::string hardware_interface = joint_interfaces.front(); - // Decide what kind of command interface this actuator/joint has - if (hardware_interface == "hardware_interface/EffortJointInterface") { - joint_control_methods_[j] = EFFORT; - } else if (hardware_interface == "hardware_interface/PositionJointInterface") { - joint_control_methods_[j] = POSITION; - } else if (hardware_interface == "hardware_interface/VelocityJointInterface") { - joint_control_methods_[j] = VELOCITY; - } else { - RCLCPP_WARN_STREAM( - nh_->get_logger(), "No matching joint interface '" << - hardware_interface << "' for joint " << joint_name); - RCLCPP_INFO( - nh_->get_logger(), - " Expecting one of 'hardware_interface/{EffortJointInterface |" - " PositionJointInterface | VelocityJointInterface}'"); - return false; - } - - // - // Accept this URDF joint as valid and link with the gazebo joint of the same name - // - - // I think it's safe to only skip this joint and not abort if there is no match found - gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name); - if (!simjoint) { - RCLCPP_WARN_STREAM( - nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name << - "' which is not in the gazebo model."); - continue; - } - sim_joints_.push_back(simjoint); - - // Accept this joint and continue configuration - RCLCPP_INFO_STREAM( - nh_->get_logger(), "Loading joint '" << joint_name << "' of type '" << - hardware_interface << "'"); - - /// - /// \brief a helper function for registering joint state or command handles - /// - auto register_joint_interface = [this, &joint_name, &j]( - const char * interface_name, - std::vector & joint_iface_handles) - { - // set joint name and interface on our stored handle - joint_iface_handles[j] = hardware_interface::JointHandle(joint_name, interface_name); - - // register the joint with the underlying RobotHardware implementation - if (register_joint( - joint_name, interface_name, - 0.0) != hardware_interface::return_type::OK) - { - RCLCPP_ERROR_STREAM( - nh_->get_logger(), "cant register " << interface_name << - " state handle for joint " << joint_name); - return false; - } - - // now retrieve the handle with the bound value reference (bound directly to the - // DynamicJointState msg in RobotHardware) - if (get_joint_handle(joint_iface_handles[j]) != hardware_interface::return_type::OK) { - RCLCPP_ERROR_STREAM( - nh_->get_logger(), "state handle " << interface_name << " failure for joint " << - joint_name); - return false; - } - - // verify handle references a target value - if (!joint_iface_handles[j]) { - RCLCPP_ERROR_STREAM( - nh_->get_logger(), interface_name << " handle for joint " << joint_name << - " is null"); - return false; - } - return true; - }; - - // register the state handles - if ( - !register_joint_interface("position", joint_pos_stateh_) || - !register_joint_interface("velocity", joint_vel_stateh_) || - !register_joint_interface("effort", joint_eff_stateh_)) - { - RCLCPP_ERROR_STREAM( - nh_->get_logger(), - "plugin aborting due to previous " << joint_name << " joint registration errors"); - return false; - } - - // Register the command handle - switch (joint_control_methods_[j]) { - case EFFORT: register_joint_interface("effort_command", joint_eff_cmdh_); break; - case POSITION: register_joint_interface("position_command", joint_pos_cmdh_); break; - case VELOCITY: register_joint_interface("velocity_command", joint_vel_cmdh_); break; - } - - // set joints operation mode to ACTIVE and register handle for controlling opmode - joint_opmodes_[j] = hardware_interface::OperationMode::ACTIVE; - joint_opmodehandles_[j] = hardware_interface::OperationModeHandle( - joint_name, &joint_opmodes_[j]); - if (register_operation_mode_handle(&joint_opmodehandles_[j]) != - hardware_interface::return_type::OK) - { - RCLCPP_WARN_STREAM(nh_->get_logger(), "cant register opmode handle for joint" << joint_name); - } - } - - // since handles references may have changed due to underlying DynamicJointState msg - // vectors resizing and reallocating we need to get these handles again - // any handles not registered are skipped, such as the command handles if they arent - // involved in the control method - bindJointHandles(joint_pos_stateh_); - bindJointHandles(joint_vel_stateh_); - bindJointHandles(joint_eff_stateh_); - bindJointHandles(joint_pos_cmdh_); - bindJointHandles(joint_vel_cmdh_); - bindJointHandles(joint_eff_cmdh_); - - - // - // Complete initialization of limits, PID controllers, etc now that registered handles are bound - // - for (size_t j = 0; j < joint_names_.size(); j++) { - auto simjoint = sim_joints_[j]; - assert(simjoint); - - // setup joint limits - registerJointLimits( - j, - 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) { - try { - nh_->declare_parameter(transmissions[j].joints[0].name + ".p"); - nh_->declare_parameter(transmissions[j].joints[0].name + ".i"); - nh_->declare_parameter(transmissions[j].joints[0].name + ".d"); - nh_->declare_parameter(transmissions[j].joints[0].name + ".i_clamp_max"); - nh_->declare_parameter(transmissions[j].joints[0].name + ".i_clamp_min"); - nh_->declare_parameter(transmissions[j].joints[0].name + ".antiwindup", false); - - if (nh_->get_parameter(transmissions[j].joints[0].name + ".p").get_type() == - rclcpp::PARAMETER_DOUBLE && - nh_->get_parameter(transmissions[j].joints[0].name + ".i").get_type() == - rclcpp::PARAMETER_DOUBLE && - nh_->get_parameter(transmissions[j].joints[0].name + ".d").get_type() == - rclcpp::PARAMETER_DOUBLE) - { - pid_controllers_.push_back( - control_toolbox::PidROS(nh_, transmissions[j].joints[0].name)); - if (pid_controllers_[j].initPid()) { - switch (joint_control_methods_[j]) { - case POSITION: joint_control_methods_[j] = POSITION_PID; - RCLCPP_INFO( - nh_->get_logger(), "joint %s is configured in POSITION_PID mode", - transmissions[j].joints[0].name.c_str()); - break; - case VELOCITY: joint_control_methods_[j] = VELOCITY_PID; - RCLCPP_INFO( - nh_->get_logger(), "joint %s is configured in VELOCITY_PID mode", - transmissions[j].joints[0].name.c_str()); - break; - } - } - } - } catch (const std::exception & e) { - RCLCPP_ERROR(rclcpp::get_logger("gazebo_ros2_control"), "%s", e.what()); - } - simjoint->SetParam("fmax", 0, joint_effort_limits_[j]); - } - } - - // get the current state of the sim joint to initialize our ROS control joints - // @note(guru-florida): perhaps we dont need this if readSim() is called after init anyway - readSim(rclcpp::Time(), rclcpp::Duration(0, 0)); - - // Initialize the emergency stop code. - e_stop_active_ = false; - last_e_stop_active_ = false; - - return true; -} - -void DefaultRobotHWSim::readSim(rclcpp::Time, rclcpp::Duration) -{ - for (unsigned int j = 0; j < joint_names_.size(); j++) { - auto joint_handle = std::make_shared( - joint_names_[j], "position"); - if (get_joint_handle(*joint_handle) == hardware_interface::return_type::OK) { - double position = sim_joints_[j]->Position(0); - if (joint_types_[j] == urdf::Joint::PRISMATIC) { - joint_handle->set_value(position); - } else { - double prev_position = joint_handle->get_value(); - joint_handle->set_value( - prev_position + - angles::shortest_angular_distance(prev_position, position)); - } - } - joint_handle = std::make_shared( - joint_names_[j], "velocity"); - if (get_joint_handle(*joint_handle) == hardware_interface::return_type::OK) { - joint_handle->set_value(sim_joints_[j]->GetVelocity(0)); - } - - joint_handle = std::make_shared( - joint_names_[j], "effort"); - if (get_joint_handle(*joint_handle) == hardware_interface::return_type::OK) { - joint_handle->set_value(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_.clear(); - std::transform( - joint_pos_stateh_.begin(), joint_pos_stateh_.end(), - std::back_inserter(last_joint_position_command_), - [](const hardware_interface::JointHandle & ph) {return ph.get_value();}); - last_e_stop_active_ = true; - } - for (int i = 0; i < n_dof_; i++) { - joint_pos_cmdh_[i].set_value(last_joint_position_command_[i]); - } - } else { - last_e_stop_active_ = false; - } - for (auto & limit_handle : joint_limit_handles_) { - if (limit_handle) { - limit_handle->enforce_limits(period); - } - } - for (unsigned int j = 0; j < n_dof_; j++) { - switch (joint_control_methods_[j]) { - case EFFORT: - { - const double effort = e_stop_active_ ? 0 : joint_eff_cmdh_[j].get_value(); - sim_joints_[j]->SetForce(0, effort); - } - break; - - case POSITION: - sim_joints_[j]->SetPosition(0, joint_pos_cmdh_[j].get_value(), true); - break; - - case POSITION_PID: - { - double error; - switch (joint_types_[j]) { - case urdf::Joint::REVOLUTE: - angles::shortest_angular_distance_with_limits( - joint_pos_stateh_[j].get_value(), - joint_pos_cmdh_[j].get_value(), - joint_lower_limits_[j], - joint_upper_limits_[j], - error); - break; - case urdf::Joint::CONTINUOUS: - error = angles::shortest_angular_distance( - joint_pos_stateh_[j].get_value(), - joint_pos_cmdh_[j].get_value()); - break; - default: - error = joint_pos_cmdh_[j].get_value() - joint_pos_stateh_[j].get_value(); - } - - const double effort_limit = joint_effort_limits_[j]; - - const double effort = ignition::math::clamp( - pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); - - sim_joints_[j]->SetForce(0, effort); - sim_joints_[j]->SetParam("friction", 0, 0.0); - } - break; - - case VELOCITY: - if (usingODE) { - sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_vel_cmdh_[j].get_value()); - } else { - sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_vel_cmdh_[j].get_value()); - } - break; - - case VELOCITY_PID: - double error; - if (e_stop_active_) { - error = -joint_vel_stateh_[j].get_value(); - } else { - error = joint_vel_cmdh_[j].get_value() - joint_vel_stateh_[j].get_value(); - } - const double effort_limit = joint_effort_limits_[j]; - const double effort = ignition::math::clamp( - pid_controllers_[j].computeCommand(error, period), - -effort_limit, effort_limit); - sim_joints_[j]->SetForce(0, effort); - break; - } - } -} - -void DefaultRobotHWSim::eStopActive(const bool active) -{ - e_stop_active_ = active; -} - -// Register the limits of the joint specified by joint_name and joint_handle. The limits are -// retrieved from joint_limit_nh. If urdf_model is not NULL, limits are retrieved from it also. -// Return the joint's type, lower position limit, upper position limit, and effort limit. -void DefaultRobotHWSim::registerJointLimits( - size_t joint_nr, - 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) -{ - const std::string & joint_name = joint_names_[joint_nr]; - const ControlMethod ctrl_method = joint_control_methods_[joint_nr]; - - *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, nh_, limits)) { - has_limits = true; - } - if (joint_limits_interface::getSoftJointLimits(urdf_joint->name, nh_, soft_limits)) { - has_soft_limits = true; - } - - // @note (ddeng): these joint limits arent input into the node, so fetch them from the urdf - // TODO(guru-florida): These are now a part of the node as of 2020-09-03, - // so what one do we take? - 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; - - has_limits = true; - } - } - - if (joint_limits_interface::getJointLimits(joint_name, 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: - { - joint_limit_handles_[joint_nr] = - std::make_unique( - joint_pos_cmdh_[joint_nr], joint_vel_cmdh_[joint_nr], joint_eff_cmdh_[joint_nr], - limits); - } - break; - case POSITION: - { - joint_limit_handles_[joint_nr] = - std::make_unique( - joint_pos_stateh_[joint_nr], joint_pos_cmdh_[joint_nr], limits, soft_limits); - } - break; - case VELOCITY: - { - joint_limit_handles_[joint_nr] = - std::make_unique( - joint_vel_stateh_[joint_nr], joint_vel_cmdh_[joint_nr], limits); - } - break; - } - } else { - switch (ctrl_method) { - case EFFORT: - { - joint_limit_handles_[joint_nr] = - std::make_unique( - joint_pos_cmdh_[joint_nr], joint_vel_cmdh_[joint_nr], joint_eff_cmdh_[joint_nr], - limits); - } - break; - case POSITION: - { - joint_limit_handles_[joint_nr] = - std::make_unique( - joint_pos_stateh_[joint_nr], joint_pos_cmdh_[joint_nr], limits); - } - break; - case VELOCITY: - { - joint_limit_handles_[joint_nr] = - std::make_unique( - joint_vel_stateh_[joint_nr], joint_vel_cmdh_[joint_nr], limits); - } - break; - } - } -} - -void DefaultRobotHWSim::bindJointHandles( - std::vector & joint_iface_handles) -{ - for (auto & jh : joint_iface_handles) { - // some handles, especially command handles, may not be registered so skip these - if (jh.get_name().empty() || jh.get_interface_name().empty()) { - continue; - } - - // now retrieve the handle with the bound value reference (bound directly to the - // DynamicJointState msg in RobotHardware) - if (get_joint_handle(jh) != hardware_interface::return_type::OK) { - RCLCPP_ERROR_STREAM( - nh_->get_logger(), "state handle " << jh.get_interface_name() << " failure for joint " << - jh.get_name()); - continue; - } - - // verify handle references a target value - if (!jh) { - RCLCPP_ERROR_STREAM( - nh_->get_logger(), jh.get_interface_name() << " handle for joint " << jh.get_name() << - " is null"); - } - } -} -} // 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 1c27e8b9..91e84dbf 100644 --- a/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp +++ b/gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp @@ -34,11 +34,21 @@ #include #include +#include #include #include "gazebo_ros/node.hpp" #include "gazebo_ros2_control/gazebo_ros2_control_plugin.hpp" +#include "gazebo_ros2_control/gazebo_system.hpp" + +#include "pluginlib/class_loader.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include "hardware_interface/resource_manager.hpp" +#include "hardware_interface/component_parser.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "urdf/model.h" #include "yaml-cpp/yaml.h" @@ -51,6 +61,10 @@ namespace gazebo_ros2_control class GazeboRosControlPrivate { public: + GazeboRosControlPrivate() = default; + + virtual ~GazeboRosControlPrivate() = default; + // Called by the world update start event void Update(); @@ -60,55 +74,45 @@ class GazeboRosControlPrivate // Get the URDF XML from the parameter server std::string getURDF(std::string param_name) const; - void eStopCB(const std::shared_ptr e_stop_active); - // Node Handles - gazebo_ros::Node::SharedPtr model_nh_; // namespaces to robot name + gazebo_ros::Node::SharedPtr model_nh_; // 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(); + gazebo_ros2_control::GazeboSystemInterface>> robot_hw_sim_loader_; - // Strings - std::string robot_namespace_; + // String with the robot description std::string robot_description_; + + // String with the name of the node that contains the robot_description std::string robot_description_node_; - std::string param_file_; - // Transmissions in this plugin's scope - std::vector transmissions_; + // Name of the file with the controllers configuration + std::string param_file_; - // Robot simulator interface - std::string robot_hw_sim_type_str_; - std::shared_ptr robot_hw_sim_; + // Executor to spin the controller rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; - // executor_->spin causes lockups, us ethis alternative for now + + // Thread where the executor will sping std::thread thread_executor_spin_; // Controller manager std::shared_ptr controller_manager_; + + // Available controllers 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_; + rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); - // 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_; + // Last time the update method was called + rclcpp::Time last_update_sim_time_ros_; }; GazeboRosControlPlugin::GazeboRosControlPlugin() @@ -131,7 +135,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element // Save pointers to the model impl_->parent_model_ = parent; - impl_->sdf_ = sdf; // Get parameters/settings for controllers from ROS param server // Initialize ROS node @@ -161,82 +164,41 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } // Get robot_description ROS param name - if (impl_->sdf_->HasElement("robot_param")) { - impl_->robot_description_ = impl_->sdf_->GetElement("robot_param")->Get(); + if (sdf->HasElement("robot_param")) { + impl_->robot_description_ = sdf->GetElement("robot_param")->Get(); } else { impl_->robot_description_ = "robot_description"; // default } // Get robot_description ROS param name - if (impl_->sdf_->HasElement("robot_param_node")) { + if (sdf->HasElement("robot_param_node")) { impl_->robot_description_node_ = - impl_->sdf_->GetElement("robot_param_node")->Get(); + sdf->GetElement("robot_param_node")->Get(); } else { impl_->robot_description_node_ = "robot_state_publisher"; // default } - // Get the robot simulation interface type - if (impl_->sdf_->HasElement("robot_sim_type")) { - impl_->robot_hw_sim_type_str_ = impl_->sdf_->Get("robot_sim_type"); - } else { - impl_->robot_hw_sim_type_str_ = "gazebo_ros2_control/DefaultRobotHWSim"; - RCLCPP_DEBUG_STREAM( - impl_->model_nh_->get_logger(), - "Using default plugin for RobotHWSim (none specified in URDF/SDF)\"" << - impl_->robot_hw_sim_type_str_ << "\""); - } - - if (impl_->sdf_->HasElement("parameters")) { - impl_->param_file_ = impl_->sdf_->GetElement("parameters")->Get(); + if (sdf->HasElement("parameters")) { + impl_->param_file_ = sdf->GetElement("parameters")->Get(); } else { RCLCPP_ERROR( impl_->model_nh_->get_logger(), "No parameter file provided. Configuration might be wrong"); } // Get the Gazebo simulation period - rclcpp::Duration gazebo_period(impl_->parent_model_->GetWorld()->Physics()->GetMaxStepSize()); - - // Decide the plugin control period - if (impl_->sdf_->HasElement("control_period")) { - impl_->control_period_ = rclcpp::Duration(impl_->sdf_->Get("control_period")); - - // Check the period against the simulation period - if (impl_->control_period_ < gazebo_period) { - RCLCPP_ERROR_STREAM( - impl_->model_nh_->get_logger(), - "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( - impl_->model_nh_->get_logger(), - " 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( - impl_->model_nh_->get_logger(), - "Control period not found in URDF/SDF, defaulting to Gazebo period of " << - impl_->control_period_.seconds()); - } - - // Initialize the emergency stop code. - impl_->e_stop_active_ = false; - impl_->last_e_stop_active_ = false; - if (impl_->sdf_->HasElement("e_stop_topic")) { - const std::string e_stop_topic = impl_->sdf_->GetElement("e_stop_topic")->Get(); - impl_->e_stop_sub_ = impl_->model_nh_->create_subscription( - e_stop_topic, 1, - std::bind(&GazeboRosControlPrivate::eStopCB, impl_.get(), std::placeholders::_1)); - } + rclcpp::Duration gazebo_period( + std::chrono::duration_cast( + std::chrono::duration( + impl_->parent_model_->GetWorld()->Physics()->GetMaxStepSize()))); // Read urdf from ros parameter server then // setup actuators and mechanism control node. // This call will block if ROS is not properly initialized. std::string urdf_string; + std::vector control_hardware; try { urdf_string = impl_->getURDF(impl_->robot_description_); - impl_->transmissions_ = transmission_interface::parse_transmissions_from_urdf(urdf_string); + control_hardware = hardware_interface::parse_control_resources_from_urdf(urdf_string); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM( impl_->model_nh_->get_logger(), @@ -244,49 +206,64 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element return; } - // Load the RobotHWSim abstraction to interface the controllers with the gazebo model + std::unique_ptr resource_manager_ = + std::make_unique(); + try { impl_->robot_hw_sim_loader_.reset( - new pluginlib::ClassLoader( + 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; - - rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast(impl_->model_nh_); - if (!impl_->robot_hw_sim_->initSim( - impl_->model_nh_->get_namespace(), node_ros2, impl_->parent_model_, urdf_model_ptr, - impl_->transmissions_)) - { - RCLCPP_FATAL( - impl_->model_nh_->get_logger(), "Could not initialize robot simulation interface"); - return; + "gazebo_ros2_control::GazeboSystemInterface")); + + for (unsigned int i = 0; i < control_hardware.size(); i++) { + std::string robot_hw_sim_type_str_ = control_hardware[i].hardware_class_type; + auto gazeboSystem = std::unique_ptr( + impl_->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); + + rclcpp::Node::SharedPtr node_ros2 = std::dynamic_pointer_cast(impl_->model_nh_); + if (!gazeboSystem->initSim( + node_ros2, + impl_->parent_model_, + control_hardware[i], + sdf)) + { + RCLCPP_FATAL( + impl_->model_nh_->get_logger(), "Could not initialize robot simulation interface"); + return; + } + + resource_manager_->import_component(std::move(gazeboSystem)); } impl_->executor_ = std::make_shared(); + auto spin = [this]() + { + while (rclcpp::ok()) { + impl_->executor_->spin_once(); + } + }; + impl_->thread_executor_spin_ = std::thread(spin); + // Create the controller manager - RCLCPP_ERROR(impl_->model_nh_->get_logger(), "Loading controller_manager"); + RCLCPP_INFO(impl_->model_nh_->get_logger(), "Loading controller_manager"); impl_->controller_manager_.reset( new controller_manager::ControllerManager( - impl_->robot_hw_sim_, impl_->executor_, - "gazebo_controller_manager")); + std::move(resource_manager_), + impl_->executor_, + "controller_manager")); + impl_->executor_->add_node(impl_->controller_manager_); // TODO(anyone): 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, + auto load_params_from_yaml_node = [](rclcpp::Node::SharedPtr node, YAML::Node & yaml_node, const std::string & prefix) { std::function, const std::string &)> + std::shared_ptr, const std::string &)> feed_yaml_to_node_rec = [&](YAML::Node yaml_node, const std::string & key, - std::shared_ptr node, const std::string & prefix) + std::shared_ptr node, const std::string & prefix) { if (node->get_name() != prefix) { return; @@ -339,12 +316,6 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element 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) { @@ -358,12 +329,12 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element } } }; - if (lc_node->get_name() != prefix) { + if (node->get_name() != prefix) { return; } - feed_yaml_to_node_rec(yaml_node, prefix, lc_node, prefix); + feed_yaml_to_node_rec(yaml_node, prefix, node, prefix); }; - auto load_params_from_yaml = [&](rclcpp_lifecycle::LifecycleNode::SharedPtr lc_node, + auto load_params_from_yaml = [&](rclcpp::Node::SharedPtr node, const std::string & yaml_config_file, const std::string & prefix) { if (yaml_config_file.empty()) { @@ -372,9 +343,9 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element YAML::Node root_node = YAML::LoadFile(yaml_config_file); for (auto yaml : root_node) { auto nodename = yaml.first.as(); - RCLCPP_ERROR(impl_->model_nh_->get_logger(), "nodename: %s", nodename.c_str()); + RCLCPP_DEBUG(impl_->model_nh_->get_logger(), "nodename: %s", nodename.c_str()); if (nodename == prefix) { - load_params_from_yaml_node(lc_node, yaml.second, prefix); + load_params_from_yaml_node(node, yaml.second, prefix); } } }; @@ -385,65 +356,81 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element 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); - if (controller_name != "joint_state_controller") { - controller->get_lifecycle_node()->configure(); + auto controller_manager_node_name = yaml.first.as(); + if (controller_manager_node_name == "controller_manager") { + for (auto yaml_node_it : yaml.second) { // ros__parameters + for (auto controller_manager_params_it : yaml_node_it.second) { + auto controller_name = controller_manager_params_it.first.as(); + + if (controller_name == "update_rate") { + float udpate_rate = controller_manager_params_it.second.as(); + // Decide the plugin control period + impl_->control_period_ = rclcpp::Duration( + std::chrono::duration_cast( + std::chrono::duration(1.0 / udpate_rate))); + + // Check the period against the simulation period + if (impl_->control_period_ < gazebo_period) { + RCLCPP_ERROR_STREAM( + impl_->model_nh_->get_logger(), + "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( + impl_->model_nh_->get_logger(), + " Desired controller update period (" << impl_->control_period_.seconds() << + " s) is slower than the gazebo simulation period (" << + gazebo_period.seconds() << " s)."); + } + } else { + for (auto controller_type_it : controller_manager_params_it.second) { + auto controller_type = controller_type_it.second.as(); + auto controller = impl_->controller_manager_->load_controller( + controller_name, + controller_type); + impl_->controllers_.push_back(controller); + load_params_from_yaml( + controller->get_node(), + impl_->param_file_, + controller_name); + controller->configure(); + start_controllers.push_back(controller_name); + } } - start_controllers.push_back(controller_name); } } + break; } } + auto switch_future = std::async( std::launch::async, - &controller_manager::ControllerManager::switch_controller, impl_->controller_manager_, + &controller_manager::ControllerManager::switch_controller, + impl_->controller_manager_, start_controllers, stop_controllers, 2, true, rclcpp::Duration(0, 0)); // STRICT_: 2 while (std::future_status::timeout == switch_future.wait_for(std::chrono::milliseconds(100))) { impl_->controller_manager_->update(); } - switch_future.get(); - - auto spin = [this]() - { - while (rclcpp::ok()) { - impl_->executor_->spin_once(); - } - }; - impl_->thread_executor_spin_ = std::thread(spin); - - // 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())); + controller_interface::return_type result = switch_future.get(); + if (result != controller_interface::return_type::SUCCESS) { + RCLCPP_ERROR(impl_->model_nh_->get_logger(), "Error initializing the joint_state_controller"); + } } catch (pluginlib::LibraryLoadException & ex) { RCLCPP_ERROR( impl_->model_nh_->get_logger(), "Failed to create robot simulation interface loader: %s ", ex.what()); } - RCLCPP_ERROR(impl_->model_nh_->get_logger(), "Loaded gazebo_ros2_control."); -} + // 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())); -void -spin(std::shared_ptr exe) -{ - exe->spin(); + RCLCPP_INFO(impl_->model_nh_->get_logger(), "Loaded gazebo_ros2_control."); } // Called by the world update start event @@ -454,37 +441,12 @@ void GazeboRosControlPrivate::Update() 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_->read(); controller_manager_->update(); + controller_manager_->write(); } - - // 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 @@ -492,7 +454,6 @@ 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 @@ -515,13 +476,13 @@ std::string GazeboRosControlPrivate::getURDF(std::string param_name) const robot_description_node_.c_str()); } - RCLCPP_ERROR( + RCLCPP_INFO( model_nh_->get_logger(), "connected to service!! %s", robot_description_node_.c_str()); // search and wait for robot_description on param server while (urdf_string.empty()) { std::string search_param_name; - RCLCPP_ERROR(model_nh_->get_logger(), "param_name %s", param_name.c_str()); + RCLCPP_DEBUG(model_nh_->get_logger(), "param_name %s", param_name.c_str()); try { auto f = parameters_client->get_parameters({param_name}); @@ -541,18 +502,12 @@ std::string GazeboRosControlPrivate::getURDF(std::string param_name) const } usleep(100000); } - RCLCPP_ERROR( + RCLCPP_INFO( model_nh_->get_logger(), "Recieved urdf from param server, parsing..."); return urdf_string; } -// 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); +GZ_REGISTER_MODEL_PLUGIN(GazeboRosControlPlugin) } // namespace gazebo_ros2_control diff --git a/gazebo_ros2_control/src/gazebo_system.cpp b/gazebo_ros2_control/src/gazebo_system.cpp new file mode 100644 index 00000000..1602af1f --- /dev/null +++ b/gazebo_ros2_control/src/gazebo_system.cpp @@ -0,0 +1,311 @@ +// Copyright 2021 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "gazebo_ros2_control/gazebo_system.hpp" + +class gazebo_ros2_control::GazeboSystemPrivate +{ +public: + GazeboSystemPrivate() = default; + + ~GazeboSystemPrivate() = default; + /// \brief Degrees od freedom. + size_t n_dof_; + + /// \brief Gazebo Model Ptr. + gazebo::physics::ModelPtr parent_model_; + + /// \brief last time the write method was called. + rclcpp::Time last_update_sim_time_ros_; + + /// \brief vector with the joint's names. + std::vector joint_names_; + + /// \brief vector with the control method defined in the URDF for each joint. + std::vector joint_control_methods_; + + /// \brief handles to the joints from within Gazebo + std::vector sim_joints_; + + /// \brief vector with the current joint position + std::vector joint_position_; + + /// \brief vector with the current joint velocity + std::vector joint_velocity_; + + /// \brief vector with the current joint effort + std::vector joint_effort_; + + /// \brief vector with the current cmd joint position + std::vector joint_position_cmd_; + + /// \brief vector with the current cmd joint velocity + std::vector joint_velocity_cmd_; + + /// \brief vector with the current cmd joint effort + std::vector joint_effort_cmd_; + + /// \brief The current positions of the joints + std::vector> joint_pos_state_; + + /// \brief The current velocities of the joints + std::vector> joint_vel_state_; + + /// \brief The current effort forces applied to the joints + std::vector> joint_eff_state_; + + /// \brief The position command interfaces of the joints + std::vector> joint_pos_cmd_; + + /// \brief The velocity command interfaces of the joints + std::vector> joint_vel_cmd_; + + /// \brief The effort command interfaces of the joints + std::vector> joint_eff_cmd_; +}; + +namespace gazebo_ros2_control +{ + +bool GazeboSystem::initSim( + rclcpp::Node::SharedPtr & model_nh, + gazebo::physics::ModelPtr parent_model, + const hardware_interface::HardwareInfo & hardware_info, + sdf::ElementPtr sdf) +{ + this->dataPtr = std::make_unique(); + this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); + + this->nh_ = model_nh; + this->dataPtr->parent_model_ = parent_model; + this->dataPtr->n_dof_ = hardware_info.joints.size(); + + this->dataPtr->joint_names_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_control_methods_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_position_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_velocity_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_effort_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_pos_state_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_vel_state_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_eff_state_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_position_cmd_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_velocity_cmd_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_effort_cmd_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_pos_cmd_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_vel_cmd_.resize(this->dataPtr->n_dof_); + this->dataPtr->joint_eff_cmd_.resize(this->dataPtr->n_dof_); + + gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics(); + + std::string physics_type_ = physics->GetType(); + if (physics_type_.empty()) { + RCLCPP_ERROR(this->nh_->get_logger(), "No physics engine configured in Gazebo."); + return false; + } + + if (this->dataPtr->n_dof_ == 0) { + RCLCPP_WARN_STREAM(this->nh_->get_logger(), "There is not joint available "); + return false; + } + + for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) { + std::string joint_name = this->dataPtr->joint_names_[j] = hardware_info.joints[j].name; + + gazebo::physics::JointPtr simjoint = parent_model->GetJoint(joint_name); + if (!simjoint) { + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name << + "' which is not in the gazebo model."); + continue; + } + this->dataPtr->sim_joints_.push_back(simjoint); + + // Accept this joint and continue configuration + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name); + + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:"); + + // register the command handles + for (unsigned int i = 0; i < hardware_info.joints[j].command_interfaces.size(); i++) { + if (hardware_info.joints[j].command_interfaces[i].name == "position") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); + this->dataPtr->joint_control_methods_[j] |= POSITION; + this->dataPtr->joint_pos_cmd_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joint_position_cmd_[j]); + } + if (hardware_info.joints[j].command_interfaces[i].name == "velocity") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); + this->dataPtr->joint_control_methods_[j] |= VELOCITY; + this->dataPtr->joint_vel_cmd_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joint_velocity_cmd_[j]); + } + if (hardware_info.joints[j].command_interfaces[i].name == "effort") { + this->dataPtr->joint_control_methods_[j] |= EFFORT; + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); + this->dataPtr->joint_eff_cmd_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joint_effort_cmd_[j]); + } + } + + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "\tState:"); + // register the state handles + for (unsigned int i = 0; i < hardware_info.joints[j].state_interfaces.size(); i++) { + if (hardware_info.joints[j].state_interfaces[i].name == "position") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); + this->dataPtr->joint_pos_cmd_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joint_position_cmd_[j]); + } + if (hardware_info.joints[j].state_interfaces[i].name == "velocity") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); + this->dataPtr->joint_vel_cmd_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joint_velocity_cmd_[j]); + } + if (hardware_info.joints[j].state_interfaces[i].name == "effort") { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); + this->dataPtr->joint_eff_cmd_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joint_effort_cmd_[j]); + } + } + } + return true; +} + +hardware_interface::return_type +GazeboSystem::configure(const hardware_interface::HardwareInfo & actuator_info) +{ + if (configure_default(actuator_info) != hardware_interface::return_type::OK) { + return hardware_interface::return_type::ERROR; + } + return hardware_interface::return_type::OK; +} + +std::vector +GazeboSystem::export_state_interfaces() +{ + std::vector state_interfaces; + + for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + this->dataPtr->joint_names_[i], + hardware_interface::HW_IF_POSITION, + &this->dataPtr->joint_position_[i])); + } + for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + this->dataPtr->joint_names_[i], + hardware_interface::HW_IF_VELOCITY, + &this->dataPtr->joint_velocity_[i])); + } + for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + this->dataPtr->joint_names_[i], + hardware_interface::HW_IF_EFFORT, + &this->dataPtr->joint_effort_[i])); + } + return state_interfaces; +} + +std::vector +GazeboSystem::export_command_interfaces() +{ + std::vector command_interfaces; + + for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + this->dataPtr->joint_names_[i], + hardware_interface::HW_IF_POSITION, + &this->dataPtr->joint_position_cmd_[i])); + } + for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + this->dataPtr->joint_names_[i], + hardware_interface::HW_IF_VELOCITY, + &this->dataPtr->joint_velocity_cmd_[i])); + } + for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); i++) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + this->dataPtr->joint_names_[i], + hardware_interface::HW_IF_EFFORT, + &this->dataPtr->joint_effort_cmd_[i])); + } + return command_interfaces; +} + +hardware_interface::return_type GazeboSystem::start() +{ + status_ = hardware_interface::status::STARTED; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type GazeboSystem::stop() +{ + status_ = hardware_interface::status::STOPPED; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type GazeboSystem::read() +{ + for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { + this->dataPtr->joint_position_[j] = this->dataPtr->sim_joints_[j]->Position(0); + this->dataPtr->joint_velocity_[j] = this->dataPtr->sim_joints_[j]->GetVelocity(0); + this->dataPtr->joint_effort_[j] = this->dataPtr->sim_joints_[j]->GetForce(0u); + } + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type GazeboSystem::write() +{ + // Get the simulation time and period + gazebo::common::Time gz_time_now = this->dataPtr->parent_model_->GetWorld()->SimTime(); + rclcpp::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec); + rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; + + for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { + if (this->dataPtr->joint_control_methods_[j] & POSITION) { + this->dataPtr->sim_joints_[j]->SetPosition( + 0, this->dataPtr->joint_position_cmd_[j], + true); + } + if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { + this->dataPtr->sim_joints_[j]->SetVelocity( + 0, + this->dataPtr->joint_velocity_cmd_[j]); + } + if (this->dataPtr->joint_control_methods_[j] & EFFORT) { + const double effort = + this->dataPtr->joint_effort_cmd_[j]; + this->dataPtr->sim_joints_[j]->SetForce(0, effort); + } + } + + this->dataPtr->last_update_sim_time_ros_ = sim_time_ros; + + return hardware_interface::return_type::OK; +} +} // namespace gazebo_ros2_control + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS( + gazebo_ros2_control::GazeboSystem, gazebo_ros2_control::GazeboSystemInterface) diff --git a/gazebo_ros2_control_demos/CMakeLists.txt b/gazebo_ros2_control_demos/CMakeLists.txt index 27a49626..67b47332 100644 --- a/gazebo_ros2_control_demos/CMakeLists.txt +++ b/gazebo_ros2_control_demos/CMakeLists.txt @@ -40,6 +40,12 @@ ament_target_dependencies(example_velocity std_msgs ) +add_executable(example_effort examples/example_effort.cpp) +ament_target_dependencies(example_effort + rclcpp + std_msgs +) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_lint_auto REQUIRED) @@ -48,7 +54,7 @@ if(BUILD_TESTING) endif() ## Install -install(TARGETS example_position example_velocity +install(TARGETS example_position example_velocity example_effort DESTINATION lib/${PROJECT_NAME} ) diff --git a/gazebo_ros2_control_demos/config/cartpole_controller.yaml b/gazebo_ros2_control_demos/config/cartpole_controller.yaml index 1f39ba1e..c55ae298 100644 --- a/gazebo_ros2_control_demos/config/cartpole_controller.yaml +++ b/gazebo_ros2_control_demos/config/cartpole_controller.yaml @@ -1,17 +1,15 @@ -cart_pole_controller: +controller_manager: ros__parameters: - type: joint_trajectory_controller/JointTrajectoryController - joints: - - slider_to_cart - write_op_modes: - - slider_to_cart - state_publish_rate: 25 - action_monitor_rate: 20 - constraints: - stopped_velocity_tolerance: 0.05 - goal_time: 5 + update_rate: 100 # Hz + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController -joint_state_controller: + joint_state_controller: + type: joint_state_controller/JointStateController + +joint_trajectory_controller: ros__parameters: - type: joint_state_controller/JointStateController - publish_rate: 50 + joints: + - slider_to_cart + interface_name: position diff --git a/gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml b/gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml new file mode 100644 index 00000000..a52b1246 --- /dev/null +++ b/gazebo_ros2_control_demos/config/cartpole_controller_effort.yaml @@ -0,0 +1,15 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + effort_controllers: + type: effort_controllers/JointGroupEffortController + + joint_state_controller: + type: joint_state_controller/JointStateController + +effort_controllers: + ros__parameters: + joints: + - slider_to_cart + interface_name: position diff --git a/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml b/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml index 03f7ac24..c6cb3c59 100644 --- a/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml +++ b/gazebo_ros2_control_demos/config/cartpole_controller_velocity.yaml @@ -1,17 +1,15 @@ -cart_pole_controller: +controller_manager: ros__parameters: - type: velocity_controllers/JointGroupVelocityController - joints: - - slider_to_cart - write_op_modes: - - slider_to_cart - state_publish_rate: 25 - action_monitor_rate: 20 - constraints: - stopped_velocity_tolerance: 0.05 - goal_time: 5 + update_rate: 100 # Hz + + velocity_controller: + type: velocity_controllers/JointGroupVelocityController -joint_state_controller: + joint_state_controller: + type: joint_state_controller/JointStateController + +velocity_controller: ros__parameters: - type: joint_state_controller/JointStateController - publish_rate: 50 + joints: + - slider_to_cart + interface_name: position diff --git a/gazebo_ros2_control_demos/examples/example_effort.cpp b/gazebo_ros2_control_demos/examples/example_effort.cpp new file mode 100644 index 00000000..dd4bff8b --- /dev/null +++ b/gazebo_ros2_control_demos/examples/example_effort.cpp @@ -0,0 +1,58 @@ +// Copyright 2020 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "std_msgs/msg/float64_multi_array.hpp" + +std::shared_ptr node; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + node = std::make_shared("effort_test_node"); + + auto publisher = node->create_publisher( + "/effort_controllers/commands", 10); + + RCLCPP_INFO(node->get_logger(), "node created"); + + std_msgs::msg::Float64MultiArray commands; + + using namespace std::chrono_literals; + + commands.data.push_back(0); + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 100; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = -200; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = 0; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + rclcpp::shutdown(); + + return 0; +} diff --git a/gazebo_ros2_control_demos/examples/example_position.cpp b/gazebo_ros2_control_demos/examples/example_position.cpp index 2cafb297..160fd869 100644 --- a/gazebo_ros2_control_demos/examples/example_position.cpp +++ b/gazebo_ros2_control_demos/examples/example_position.cpp @@ -96,7 +96,7 @@ int main(int argc, char * argv[]) node->get_node_graph_interface(), node->get_node_logging_interface(), node->get_node_waitables_interface(), - "/cart_pole_controller/follow_joint_trajectory"); + "/joint_trajectory_controller/follow_joint_trajectory"); bool response = action_client->wait_for_action_server(std::chrono::seconds(1)); diff --git a/gazebo_ros2_control_demos/examples/example_velocity.cpp b/gazebo_ros2_control_demos/examples/example_velocity.cpp index 42f3e9f5..66076916 100644 --- a/gazebo_ros2_control_demos/examples/example_velocity.cpp +++ b/gazebo_ros2_control_demos/examples/example_velocity.cpp @@ -28,7 +28,8 @@ int main(int argc, char * argv[]) node = std::make_shared("velocity_test_node"); - auto publisher = node->create_publisher("/commands", 10); + auto publisher = node->create_publisher( + "/velocity_controller/commands", 10); RCLCPP_INFO(node->get_logger(), "node created"); diff --git a/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py b/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py similarity index 96% rename from gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py rename to gazebo_ros2_control_demos/launch/cart_example_effort.launch.py index df94e05d..89df1466 100644 --- a/gazebo_ros2_control_demos/launch/cart_example_position_pid.launch.py +++ b/gazebo_ros2_control_demos/launch/cart_example_effort.launch.py @@ -37,7 +37,7 @@ def generate_launch_description(): xacro_file = os.path.join(gazebo_ros2_control_demos_path, 'urdf', - 'test_cart_position_pid.xacro.urdf') + 'test_cart_effort.xacro.urdf') doc = xacro.parse(open(xacro_file)) xacro.process_doc(doc) diff --git a/gazebo_ros2_control_demos/package.xml b/gazebo_ros2_control_demos/package.xml index 49a0e780..ec9f27e3 100644 --- a/gazebo_ros2_control_demos/package.xml +++ b/gazebo_ros2_control_demos/package.xml @@ -33,7 +33,6 @@ robot_state_publisher rclcpp std_msgs - transmission_interface velocity_controllers xacro diff --git a/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_effort.xacro.urdf similarity index 64% rename from gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf rename to gazebo_ros2_control_demos/urdf/test_cart_effort.xacro.urdf index 2fed7bf0..a1aa22f9 100644 --- a/gazebo_ros2_control_demos/urdf/test_cart_position_pid.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_cart_effort.xacro.urdf @@ -48,29 +48,25 @@ - - transmission_interface/SimpleTransmission + + + + gazebo_ros2_control/GazeboSystem + - hardware_interface/PositionJointInterface + + -1000 + 1000 + + + + - - hardware_interface/PositionJointInterface - 1 - - + + - - / - 50.0 - 10.0 - 15.0 - 3.0 - -3.0 - false - - gazebo_ros2_control/DefaultRobotHWSim - $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml + $(find gazebo_ros2_control_demos)/config/cartpole_controller_effort.yaml diff --git a/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf index 2f0d8f82..187ca60e 100644 --- a/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -48,19 +48,24 @@ - - transmission_interface/SimpleTransmission + + + + gazebo_ros2_control/GazeboSystem + - hardware_interface/PositionJointInterface + + -15 + 15 + + + + - - hardware_interface/PositionJointInterface - 1 - - + + - gazebo_ros2_control/DefaultRobotHWSim $(find gazebo_ros2_control_demos)/config/cartpole_controller.yaml diff --git a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index 854cf6ba..f94ba88f 100644 --- a/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/gazebo_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -48,19 +48,22 @@ - - transmission_interface/SimpleTransmission + + + gazebo_ros2_control/GazeboSystem + - hardware_interface/VelocityJointInterface + + -10 + 10 + + + + - - hardware_interface/VelocityJointInterface - 1 - - + - gazebo_ros2_control/DefaultRobotHWSim $(find gazebo_ros2_control_demos)/config/cartpole_controller_velocity.yaml diff --git a/img/gazebo_ros2_control_position_pid.gif b/img/gazebo_ros2_control_position.gif similarity index 100% rename from img/gazebo_ros2_control_position_pid.gif rename to img/gazebo_ros2_control_position.gif diff --git a/img/moveit2.gif b/img/moveit2.gif new file mode 100644 index 00000000..00ad27af Binary files /dev/null and b/img/moveit2.gif differ