From a51236735017aca3c2643c87ceab20a431509eff Mon Sep 17 00:00:00 2001 From: ahcorde Date: Sat, 13 Mar 2021 23:57:56 +0100 Subject: [PATCH 01/50] Added ignition_ros2_control Signed-off-by: ahcorde --- ignition_ros2_control/CMakeLists.txt | 98 +++++ ignition_ros2_control/LICENSE | 15 + .../ignition_hardware_plugins.xml | 10 + .../ignition_ros2_control_plugin.hpp | 65 +++ .../ignition_ros2_control/ignition_system.hpp | 75 ++++ .../ignition_system_interface.hpp | 90 ++++ ignition_ros2_control/package.xml | 28 ++ .../src/ignition_ros2_control_plugin.cpp | 402 +++++++++++++++++ ignition_ros2_control/src/ignition_system.cpp | 409 ++++++++++++++++++ 9 files changed, 1192 insertions(+) create mode 100644 ignition_ros2_control/CMakeLists.txt create mode 100644 ignition_ros2_control/LICENSE create mode 100644 ignition_ros2_control/ignition_hardware_plugins.xml create mode 100644 ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp create mode 100644 ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp create mode 100644 ignition_ros2_control/include/ignition_ros2_control/ignition_system_interface.hpp create mode 100644 ignition_ros2_control/package.xml create mode 100644 ignition_ros2_control/src/ignition_ros2_control_plugin.cpp create mode 100644 ignition_ros2_control/src/ignition_system.cpp diff --git a/ignition_ros2_control/CMakeLists.txt b/ignition_ros2_control/CMakeLists.txt new file mode 100644 index 00000000..efc3fba6 --- /dev/null +++ b/ignition_ros2_control/CMakeLists.txt @@ -0,0 +1,98 @@ +cmake_minimum_required(VERSION 3.5) +project(ignition_ros2_control) + +# Default to C11 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 11) +endif() +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# Compiler options +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(angles REQUIRED) +find_package(controller_manager REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(urdf REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) + +find_package(ignition-gazebo5 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) + +find_package(ignition-plugin1 REQUIRED) +set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) + +find_package(ignition-physics4 REQUIRED) +set(IGN_PHYSICS_VER ${ignition-physics4_VERSION_MAJOR}) + +include_directories(include) + +add_library(${PROJECT_NAME}-system SHARED + src/ignition_ros2_control_plugin.cpp +) + +target_link_libraries(${PROJECT_NAME}-system + ignition-gazebo${IGN_GAZEBO_VER}::core + ignition-physics${IGN_PHYSICS_VER}::core + ignition-plugin${IGN_PLUGIN_VER}::register +) +ament_target_dependencies(${PROJECT_NAME}-system + ament_index_cpp + controller_manager + hardware_interface + pluginlib + rclcpp + urdf + yaml_cpp_vendor +) + +######### + +add_library(ignition_hardware_plugins SHARED + src/ignition_system.cpp +) +ament_target_dependencies(ignition_hardware_plugins + angles + hardware_interface + rclcpp +) +target_link_libraries(ignition_hardware_plugins + ignition-gazebo${IGN_GAZEBO_VER}::core +) + +## Install +install(TARGETS + ignition_hardware_plugins + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Testing and linting +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME} ignition_hardware_plugins) + +# Install directories +install(TARGETS ${PROJECT_NAME}-system + DESTINATION lib +) + +pluginlib_export_plugin_description_file(ignition_ros2_control ignition_hardware_plugins.xml) + +# Setup the project +ament_package() diff --git a/ignition_ros2_control/LICENSE b/ignition_ros2_control/LICENSE new file mode 100644 index 00000000..1dee6304 --- /dev/null +++ b/ignition_ros2_control/LICENSE @@ -0,0 +1,15 @@ +Software License Agreement (Apache License) + +Copyright 2021 Open Source Robotics Foundation + +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. diff --git a/ignition_ros2_control/ignition_hardware_plugins.xml b/ignition_ros2_control/ignition_hardware_plugins.xml new file mode 100644 index 00000000..949be43d --- /dev/null +++ b/ignition_ros2_control/ignition_hardware_plugins.xml @@ -0,0 +1,10 @@ + + + + GazeboPositionJoint + + + diff --git a/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp b/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp new file mode 100644 index 00000000..2dc2dd39 --- /dev/null +++ b/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp @@ -0,0 +1,65 @@ +// 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 IGNITION_ROS2_CONTROL__IGNITION_ROS2_CONTROL_PLUGIN_HPP_ +#define IGNITION_ROS2_CONTROL__IGNITION_ROS2_CONTROL_PLUGIN_HPP_ + +#include + +#include "ignition/gazebo/System.hh" + +namespace ignition_ros2_control +{ +// Forward declarations. +class IgnitionROS2ControlPluginPrivate; + +class IgnitionROS2ControlPlugin + : public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemUpdate +{ + /// \brief Constructor + +public: + IgnitionROS2ControlPlugin(); + + /// \brief Destructor + +public: + ~IgnitionROS2ControlPlugin() override = default; + + // Documentation inherited + +public: + void Configure( + const ignition::gazebo::Entity & _entity, + const std::shared_ptr & _sdf, + ignition::gazebo::EntityComponentManager & _ecm, + ignition::gazebo::EventManager & _eventMgr) override; + + // Documentation inherited + +public: + void Update( + const ignition::gazebo::UpdateInfo & _info, + ignition::gazebo::EntityComponentManager & _ecm) override; + + /// \brief Private data pointer. + +private: + std::unique_ptr dataPtr; +}; +} // namespace ignition_ros2_control + +#endif // IGNITION_ROS2_CONTROL__IGNITION_ROS2_CONTROL_PLUGIN_HPP_ diff --git a/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp b/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp new file mode 100644 index 00000000..2a585a7d --- /dev/null +++ b/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp @@ -0,0 +1,75 @@ +// 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 IGNITION_ROS2_CONTROL__IGNITION_SYSTEM_HPP_ +#define IGNITION_ROS2_CONTROL__IGNITION_SYSTEM_HPP_ + +#include +#include +#include +#include + +#include "angles/angles.h" + +#include "ignition_ros2_control/ignition_system_interface.hpp" + +namespace ignition_ros2_control +{ +// Forward declaration +class IgnitionSystemPrivate; + +// These class must inherit `ignition_ros2_control::IgnitionSystemInterface` which implements a +// simulated `ros2_control` `hardware_interface::SystemInterface`. + +class IgnitionSystem : public IgnitionSystemInterface +{ +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, + std::map & joints, + const hardware_interface::HardwareInfo & hardware_info, + ignition::gazebo::EntityComponentManager & _ecm) override; + +private: + /// \brief Private data class + std::unique_ptr dataPtr; +}; + +} // namespace ignition_ros2_control + +#endif // IGNITION_ROS2_CONTROL__IGNITION_SYSTEM_HPP_ diff --git a/ignition_ros2_control/include/ignition_ros2_control/ignition_system_interface.hpp b/ignition_ros2_control/include/ignition_ros2_control/ignition_system_interface.hpp new file mode 100644 index 00000000..eac99bb0 --- /dev/null +++ b/ignition_ros2_control/include/ignition_ros2_control/ignition_system_interface.hpp @@ -0,0 +1,90 @@ +// 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 IGNITION_ROS2_CONTROL__IGNITION_SYSTEM_INTERFACE_HPP_ +#define IGNITION_ROS2_CONTROL__IGNITION_SYSTEM_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include "ignition/gazebo/System.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" + +namespace ignition_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 IgnitionSystemInterface + : public hardware_interface::BaseInterface +{ +public: + /// \brief Initilize the system interface + /// param[in] model_nh + /// param[in] joints + /// param[in] hardware_info + /// param[in] _ecm + virtual bool initSim( + rclcpp::Node::SharedPtr & model_nh, + std::map & joints, + const hardware_interface::HardwareInfo & hardware_info, + ignition::gazebo::EntityComponentManager & _ecm) = 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 ignition_ros2_control + +#endif // IGNITION_ROS2_CONTROL__IGNITION_SYSTEM_INTERFACE_HPP_ diff --git a/ignition_ros2_control/package.xml b/ignition_ros2_control/package.xml new file mode 100644 index 00000000..f1b2a3a1 --- /dev/null +++ b/ignition_ros2_control/package.xml @@ -0,0 +1,28 @@ + + + ignition_ros2_control + 0.0.0 + Lol + Alejandro Hernández + Alejandro Hernández + Apache 2 + + ament_cmake + + angles + ament_index_cpp + pluginlib + rclcpp + yaml_cpp_vendor + urdf + hardware_interface + controller_manager + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp new file mode 100644 index 00000000..7cd86c87 --- /dev/null +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -0,0 +1,402 @@ +// 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 +#include + +#include "controller_manager/controller_manager.hpp" + +#include "hardware_interface/resource_manager.hpp" +#include "hardware_interface/component_parser.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + + +#include "ignition_ros2_control/ignition_ros2_control_plugin.hpp" +#include "ignition_ros2_control/ignition_system.hpp" + +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointType.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Physics.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Model.hh" + +#include "ignition/plugin/Register.hh" + +#include "pluginlib/class_loader.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace ignition_ros2_control +{ +////////////////////////////////////////////////// +class IgnitionROS2ControlPluginPrivate +{ + /// \brief Entity ID for sensor within Gazebo. + +public: + ignition::gazebo::Entity entity_; + +public: + std::shared_ptr node; + + // Thread where the executor will sping + std::thread thread_executor_spin_; + + // Executor to spin the controller + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + + // Timing + rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); + + // Interface loader + boost::shared_ptr> robot_hw_sim_loader_; + + // Controller manager + std::shared_ptr controller_manager_; + + // String with the robot description + // TODO(ahcorde): Add param in plugin tag + std::string robot_description_ = "robot_description"; + // String with the name of the node that contains the robot_description + // TODO(ahcorde): Add param in plugin tag + std::string robot_description_node_ = "robot_state_publisher"; + + // Last time the update method was called + rclcpp::Time last_update_sim_time_ros_; + + std::string getURDF(std::string param_name) const + { + std::string urdf_string; + + using namespace std::chrono_literals; + auto parameters_client = std::make_shared( + node, robot_description_node_); + while (!parameters_client->wait_for_service(0.5s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR( + node->get_logger(), "Interrupted while waiting for %s service. Exiting.", + robot_description_node_.c_str()); + return 0; + } + RCLCPP_ERROR( + node->get_logger(), "%s service not available, waiting again...", + robot_description_node_.c_str()); + } + + RCLCPP_INFO( + node->get_logger(), "connected to service!! %s asking for %s", + robot_description_node_.c_str(), + param_name.c_str()); + + // search and wait for robot_description on param server + while (urdf_string.empty()) { + std::string search_param_name; + RCLCPP_DEBUG(node->get_logger(), "param_name %s", param_name.c_str()); + + try { + auto f = parameters_client->get_parameters({param_name}); + f.wait(); + std::vector values = f.get(); + urdf_string = values[0].as_string(); + } catch (const std::exception & e) { + RCLCPP_ERROR(node->get_logger(), "%s", e.what()); + } + + if (!urdf_string.empty()) { + break; + } else { + RCLCPP_ERROR( + node->get_logger(), "ignition_ros2_control plugin is waiting for model" + " URDF in parameter [%s] on the ROS param server.", search_param_name.c_str()); + } + usleep(100000); + } + RCLCPP_INFO( + node->get_logger(), "Recieved urdf from param server, parsing..."); + + return urdf_string; + } + + ////////////////////////////////////////////////// + +public: + std::map GetEnabledJoints( + const ignition::gazebo::Entity & _entity, + const std::shared_ptr & _sdf, + ignition::gazebo::EntityComponentManager & _ecm) const + { + std::map output; + + std::vector enabledJoints; + + // Get all available joints + std::vector jointEntities; + jointEntities = _ecm.ChildrenByComponents(_entity, ignition::gazebo::components::Joint()); + + // Iterate over all joints and verify whether they can be enabled or not + for (const auto & jointEntity : jointEntities) { + const auto jointName = _ecm.Component( + jointEntity)->Data(); + + // Make sure the joint type is supported, i.e. it has exactly one + // actuated axis + const auto * jointType = _ecm.Component(jointEntity); + switch (jointType->Data()) { + case sdf::JointType::PRISMATIC: + case sdf::JointType::REVOLUTE: + case sdf::JointType::CONTINUOUS: + case sdf::JointType::GEARBOX: + { + // Supported joint type + break; + } + case sdf::JointType::FIXED: + { + igndbg << "[ignition_ros2_control] Fixed joint [" << jointName << + "(Entity=" << jointEntity << ")] is skipped.\n"; + continue; + } + case sdf::JointType::REVOLUTE2: + case sdf::JointType::SCREW: + case sdf::JointType::BALL: + case sdf::JointType::UNIVERSAL: + { + ignwarn << "[ignition_ros2_control] Joint [" << jointName << + "(Entity=" << jointEntity << + ")] is of unsupported type. Only joints with a single axis" + " are supported.\n"; + continue; + } + default: + { + ignwarn << "[ignition_ros2_control] Joint [" << jointName << + "(Entity=" << jointEntity << ")] is of unknown type.\n"; + continue; + } + } + output[jointName] = jointEntity; + } + + return output; + } +}; + +////////////////////////////////////////////////// +IgnitionROS2ControlPlugin::IgnitionROS2ControlPlugin() +: dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void IgnitionROS2ControlPlugin::Configure( + const ignition::gazebo::Entity & _entity, + const std::shared_ptr & _sdf, + ignition::gazebo::EntityComponentManager & _ecm, + ignition::gazebo::EventManager &) +{ + // Make sure the controller is attached to a valid model + const auto model = ignition::gazebo::Model(_entity); + if (!model.Valid(_ecm)) { + ignerr << "[Ignition ROS 2 Control] Failed to initialize because [" << + model.Name(_ecm) << "(Entity=" << _entity << + ")] is not a model. Please make sure that" + " Ignition ROS 2 Control is attached to a valid model.\n"; + return; + } + + // Get params from SDF + std::string paramFileName = _sdf->Get("parameters"); + + if (paramFileName.empty()) { + ignerr << "Ignition ros2 controle found an empty parameters file. " << + "Failed to initialize."; + return; + } + + std::vector arguments = {"--ros-args", "--params-file", paramFileName}; + + std::vector argv; + for (const auto & arg : arguments) { + argv.push_back(reinterpret_cast(arg.data())); + } + + if (!rclcpp::ok()) { + rclcpp::init(static_cast(argv.size()), argv.data()); + this->dataPtr->node = rclcpp::Node::make_shared("ignition_ros2_control"); + } + this->dataPtr->executor_ = std::make_shared(); + this->dataPtr->executor_->add_node(this->dataPtr->node); + auto spin = [this]() + { + while (rclcpp::ok()) { + this->dataPtr->executor_->spin_once(); + } + }; + this->dataPtr->thread_executor_spin_ = std::thread(spin); + + RCLCPP_ERROR_STREAM( + this->dataPtr->node->get_logger(), "[Ignition ROS 2 Control] Setting up controller for [" << + "(Entity=" << _entity << ")].\n"); + + ignmsg << "[Ignition ROS 2 Control] Setting up controller for [" << + model.Name(_ecm) << "(Entity=" << _entity << ")].\n"; + + // Get list of enabled joints + auto enabledJoints = this->dataPtr->GetEnabledJoints( + _entity, + _sdf, + _ecm); + + // 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 = this->dataPtr->getURDF(this->dataPtr->robot_description_); + control_hardware = hardware_interface::parse_control_resources_from_urdf(urdf_string); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM( + this->dataPtr->node->get_logger(), + "Error parsing URDF in ignition_ros2_control plugin, plugin not active : " << ex.what()); + return; + } + + RCLCPP_ERROR_STREAM( + this->dataPtr->node->get_logger(), "getURFD\n"); + + std::unique_ptr resource_manager_ = + std::make_unique(); + + try { + this->dataPtr->robot_hw_sim_loader_.reset( + new pluginlib::ClassLoader( + "ignition_ros2_control", + "ignition_ros2_control::IgnitionSystemInterface")); + } catch (pluginlib::LibraryLoadException & ex) { + RCLCPP_ERROR( + this->dataPtr->node->get_logger(), "Failed to create robot simulation interface loader: %s ", + ex.what()); + } + + RCLCPP_ERROR_STREAM( + this->dataPtr->node->get_logger(), "pluginlib\n"); + + 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( + this->dataPtr->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); + + if (!gazeboSystem->initSim( + this->dataPtr->node, + enabledJoints, + control_hardware[i], + _ecm)) + { + RCLCPP_FATAL( + this->dataPtr->node->get_logger(), "Could not initialize robot simulation interface"); + return; + } + + resource_manager_->import_component(std::move(gazeboSystem)); + } + RCLCPP_ERROR_STREAM( + this->dataPtr->node->get_logger(), "initSim\n"); + + // Create the controller manager + RCLCPP_INFO(this->dataPtr->node->get_logger(), "Loading controller_manager"); + this->dataPtr->controller_manager_.reset( + new controller_manager::ControllerManager( + std::move(resource_manager_), + this->dataPtr->executor_, + "controller_manager")); + this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_); + + if (!this->dataPtr->controller_manager_->has_parameter("update_rate")) { + RCLCPP_ERROR_STREAM( + this->dataPtr->node->get_logger(), + "controller manager doesn't have an update_rate parameter"); + return; + } + + auto worldEntity = + _ecm.EntityByComponents(ignition::gazebo::components::World()); + + auto physicsComp = + _ecm.Component(worldEntity); + const auto & physicsParams = physicsComp->Data(); + const auto newStepSize = + std::chrono::duration(physicsParams.MaxStepSize()); + + // Get the Gazebo simulation period + rclcpp::Duration gazebo_period( + std::chrono::duration_cast( + std::chrono::duration(newStepSize))); + + auto cm_update_rate = this->dataPtr->controller_manager_->get_parameter("update_rate").as_int(); + this->dataPtr->control_period_ = rclcpp::Duration( + std::chrono::duration_cast( + std::chrono::duration(1.0 / static_cast(cm_update_rate)))); + // Check the period against the simulation period + if (this->dataPtr->control_period_ < gazebo_period) { + RCLCPP_ERROR_STREAM( + this->dataPtr->node->get_logger(), + "Desired controller update period (" << this->dataPtr->control_period_.seconds() << + " s) is faster than the gazebo simulation period (" << + gazebo_period.seconds() << " s)."); + } else if (this->dataPtr->control_period_ > gazebo_period) { + RCLCPP_WARN_STREAM( + this->dataPtr->node->get_logger(), + " Desired controller update period (" << this->dataPtr->control_period_.seconds() << + " s) is slower than the gazebo simulation period (" << + gazebo_period.seconds() << " s)."); + } + + this->dataPtr->entity_ = _entity; +} + +////////////////////////////////////////////////// +void IgnitionROS2ControlPlugin::Update( + const ignition::gazebo::UpdateInfo & _info, + ignition::gazebo::EntityComponentManager & _ecm) +{ + // Get the simulation time and period + rclcpp::Time sim_time_ros(std::chrono::duration_cast( + _info.simTime).count()); + rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; + + if (sim_period >= this->dataPtr->control_period_) { + this->dataPtr->last_update_sim_time_ros_ = sim_time_ros; + this->dataPtr->controller_manager_->read(); + this->dataPtr->controller_manager_->update(); + this->dataPtr->controller_manager_->write(); + } +} +} // namespace ignition_ros2_control + +IGNITION_ADD_PLUGIN( + ignition_ros2_control::IgnitionROS2ControlPlugin, + ignition::gazebo::System, + ignition_ros2_control::IgnitionROS2ControlPlugin::ISystemConfigure, + ignition_ros2_control::IgnitionROS2ControlPlugin::ISystemUpdate) + +IGNITION_ADD_PLUGIN_ALIAS( + ignition_ros2_control::IgnitionROS2ControlPlugin, + "ignition::gazebo::systems::IgnitionROS2ControlPlugin") diff --git a/ignition_ros2_control/src/ignition_system.cpp b/ignition_ros2_control/src/ignition_system.cpp new file mode 100644 index 00000000..3b5e0c42 --- /dev/null +++ b/ignition_ros2_control/src/ignition_system.cpp @@ -0,0 +1,409 @@ +// 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 +#include + +#include "ignition_ros2_control/ignition_system.hpp" + +#include "ignition/gazebo/components/JointForce.hh" +#include "ignition/gazebo/components/JointForceCmd.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointPositionReset.hh" +#include "ignition/gazebo/components/JointVelocity.hh" +#include "ignition/gazebo/components/JointVelocityCmd.hh" + +class ignition_ros2_control::IgnitionSystemPrivate +{ +public: + IgnitionSystemPrivate() = default; + + ~IgnitionSystemPrivate() = default; + /// \brief Degrees od freedom. + size_t n_dof_; + + /// \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_; + + ignition::gazebo::EntityComponentManager * ecm; +}; + +namespace ignition_ros2_control +{ + +bool IgnitionSystem::initSim( + rclcpp::Node::SharedPtr & model_nh, + std::map & enableJoints, + const hardware_interface::HardwareInfo & hardware_info, + ignition::gazebo::EntityComponentManager & _ecm) +{ + this->dataPtr = std::make_unique(); + this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); + + this->nh_ = model_nh; + this->dataPtr->ecm = &_ecm; + this->dataPtr->n_dof_ = hardware_info.joints.size(); + + RCLCPP_ERROR(this->nh_->get_logger(), "n_dof_ %d", this->dataPtr->n_dof_); + + 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_); + + // ignition::physics::PhysicsEnginePtr physics = ignition::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 Ignition."); + // 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; + + ignition::gazebo::Entity simjoint = enableJoints[joint_name]; + if (!simjoint) { + RCLCPP_WARN_STREAM( + this->nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name << + "' which is not in the ignition model."); + continue; + } + this->dataPtr->sim_joints_.push_back(simjoint); + + // Create joint position component if one doesn't exist + if (!_ecm.EntityHasComponentType( + simjoint, + ignition::gazebo::components::JointPosition().TypeId())) + { + _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointPosition()); + } + + // Create joint velocity component if one doesn't exist + if (!_ecm.EntityHasComponentType( + simjoint, + ignition::gazebo::components::JointVelocity().TypeId())) + { + _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointVelocity()); + } + + // Create joint force component if one doesn't exist + if (!_ecm.EntityHasComponentType( + simjoint, + ignition::gazebo::components::JointForce().TypeId())) + { + _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointForce()); + } + + // 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_state_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joint_position_[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_state_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joint_velocity_[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_state_[j] = std::make_shared( + joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joint_effort_[j]); + } + } + } + return true; +} + +hardware_interface::return_type +IgnitionSystem::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 +IgnitionSystem::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 +IgnitionSystem::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 IgnitionSystem::start() +{ + status_ = hardware_interface::status::STARTED; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type IgnitionSystem::stop() +{ + status_ = hardware_interface::status::STOPPED; + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type IgnitionSystem::read() +{ + for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { + // Get the joint velocity + const auto * jointVelocity = + this->dataPtr->ecm->Component( + this->dataPtr->sim_joints_[j]); + + // Get the joint force + const auto * jointForce = + this->dataPtr->ecm->Component( + this->dataPtr->sim_joints_[j]); + + // Get the joint position + const auto * jointPositions = + this->dataPtr->ecm->Component( + this->dataPtr->sim_joints_[j]); + + this->dataPtr->joint_position_[j] = jointPositions->Data()[0]; + this->dataPtr->joint_velocity_[j] = jointVelocity->Data()[0]; + // this->dataPtr->joint_effort_[j] = jointForce->Data()[0]; + } + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type IgnitionSystem::write() +{ + for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { + if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { + if (!this->dataPtr->ecm->Component( + this->dataPtr->sim_joints_[j])) + { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->sim_joints_[j], + ignition::gazebo::components::JointVelocityCmd({0})); + } else { + const auto jointVelCmd = + this->dataPtr->ecm->Component( + this->dataPtr->sim_joints_[j]); + *jointVelCmd = ignition::gazebo::components::JointVelocityCmd( + {this->dataPtr->joint_velocity_cmd_[j]}); + } + } + + if (this->dataPtr->joint_control_methods_[j] & POSITION) { + if (!this->dataPtr->ecm->Component( + this->dataPtr->sim_joints_[j])) + { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->sim_joints_[j], + ignition::gazebo::components::JointPositionReset({this->dataPtr->joint_position_[j]})); + const auto jointPosCmd = + this->dataPtr->ecm->Component( + this->dataPtr->sim_joints_[j]); + *jointPosCmd = ignition::gazebo::components::JointPositionReset( + {this->dataPtr->joint_position_cmd_[j]}); + } + } + + if (this->dataPtr->joint_control_methods_[j] & EFFORT) { + if (!this->dataPtr->ecm->Component( + this->dataPtr->sim_joints_[j])) + { + this->dataPtr->ecm->CreateComponent( + this->dataPtr->sim_joints_[j], + ignition::gazebo::components::JointForceCmd({0})); + } else { + const auto jointEffortCmd = + this->dataPtr->ecm->Component( + this->dataPtr->sim_joints_[j]); + *jointEffortCmd = ignition::gazebo::components::JointForceCmd( + {this->dataPtr->joint_effort_cmd_[j]}); + } + } + } + + // // Get the simulation time and period + // ignition::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 ignition_ros2_control + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS( + ignition_ros2_control::IgnitionSystem, ignition_ros2_control::IgnitionSystemInterface) From 2b68d6f3cf07c24ba4f4dfebdd99edb22caf6bf3 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Sat, 13 Mar 2021 23:58:02 +0100 Subject: [PATCH 02/50] Added ignition_ros2_control demos Signed-off-by: ahcorde --- ignition_ros2_control_demos/CMakeLists.txt | 61 ++++++ .../config/cartpole_controller_effort.yaml | 14 ++ .../config/cartpole_controller_position.yaml | 14 ++ .../config/cartpole_controller_velocity.yaml | 14 ++ .../examples/example_effort.cpp | 58 ++++++ .../examples/example_position.cpp | 179 ++++++++++++++++++ .../examples/example_velocity.cpp | 58 ++++++ .../launch/cart_example_effort.launch.py | 99 ++++++++++ .../launch/cart_example_position.launch.py | 99 ++++++++++ .../launch/cart_example_velocity.launch.py | 99 ++++++++++ ignition_ros2_control_demos/package.xml | 45 +++++ .../urdf/test_cart_effort.xacro.urdf | 74 ++++++++ .../urdf/test_cart_position.xacro.urdf | 77 ++++++++ .../urdf/test_cart_velocity.xacro.urdf | 74 ++++++++ 14 files changed, 965 insertions(+) create mode 100644 ignition_ros2_control_demos/CMakeLists.txt create mode 100644 ignition_ros2_control_demos/config/cartpole_controller_effort.yaml create mode 100644 ignition_ros2_control_demos/config/cartpole_controller_position.yaml create mode 100644 ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml create mode 100644 ignition_ros2_control_demos/examples/example_effort.cpp create mode 100644 ignition_ros2_control_demos/examples/example_position.cpp create mode 100644 ignition_ros2_control_demos/examples/example_velocity.cpp create mode 100644 ignition_ros2_control_demos/launch/cart_example_effort.launch.py create mode 100644 ignition_ros2_control_demos/launch/cart_example_position.launch.py create mode 100644 ignition_ros2_control_demos/launch/cart_example_velocity.launch.py create mode 100644 ignition_ros2_control_demos/package.xml create mode 100644 ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf create mode 100644 ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf create mode 100644 ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf diff --git a/ignition_ros2_control_demos/CMakeLists.txt b/ignition_ros2_control_demos/CMakeLists.txt new file mode 100644 index 00000000..19577c5a --- /dev/null +++ b/ignition_ros2_control_demos/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.5.0) +project(ignition_ros2_control_demos) + +# Default to C11 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 11) +endif() +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(NOT WIN32) + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(control_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) +find_package(std_msgs REQUIRED) + +install(DIRECTORY + launch + config + urdf + DESTINATION share/${PROJECT_NAME}/ +) + +add_executable(example_position examples/example_position.cpp) +ament_target_dependencies(example_position + rclcpp + rclcpp_action + control_msgs +) + +add_executable(example_velocity examples/example_velocity.cpp) +ament_target_dependencies(example_velocity + rclcpp + 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) + + ament_lint_auto_find_test_dependencies() +endif() + +## Install +install(TARGETS example_position example_velocity example_effort + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/ignition_ros2_control_demos/config/cartpole_controller_effort.yaml b/ignition_ros2_control_demos/config/cartpole_controller_effort.yaml new file mode 100644 index 00000000..11c26f19 --- /dev/null +++ b/ignition_ros2_control_demos/config/cartpole_controller_effort.yaml @@ -0,0 +1,14 @@ +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + + effort_controllers: + type: effort_controllers/JointGroupEffortController + + joint_state_controller: + type: joint_state_controller/JointStateController + +effort_controllers: + ros__parameters: + joints: + - slider_to_cart diff --git a/ignition_ros2_control_demos/config/cartpole_controller_position.yaml b/ignition_ros2_control_demos/config/cartpole_controller_position.yaml new file mode 100644 index 00000000..935af69e --- /dev/null +++ b/ignition_ros2_control_demos/config/cartpole_controller_position.yaml @@ -0,0 +1,14 @@ +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + joint_trajectory_controller: + type: joint_trajectory_controller/JointTrajectoryController + + joint_state_controller: + type: joint_state_controller/JointStateController + +joint_trajectory_controller: + ros__parameters: + joints: + - slider_to_cart diff --git a/ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml b/ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml new file mode 100644 index 00000000..ad259e5e --- /dev/null +++ b/ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml @@ -0,0 +1,14 @@ +controller_manager: + ros__parameters: + update_rate: 1000 # Hz + + velocity_controller: + type: velocity_controllers/JointGroupVelocityController + + joint_state_controller: + type: joint_state_controller/JointStateController + +velocity_controller: + ros__parameters: + joints: + - slider_to_cart diff --git a/ignition_ros2_control_demos/examples/example_effort.cpp b/ignition_ros2_control_demos/examples/example_effort.cpp new file mode 100644 index 00000000..dd4bff8b --- /dev/null +++ b/ignition_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/ignition_ros2_control_demos/examples/example_position.cpp b/ignition_ros2_control_demos/examples/example_position.cpp new file mode 100644 index 00000000..160fd869 --- /dev/null +++ b/ignition_ros2_control_demos/examples/example_position.cpp @@ -0,0 +1,179 @@ +// 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 "rclcpp_action/rclcpp_action.hpp" + +#include "control_msgs/action/follow_joint_trajectory.hpp" + +std::shared_ptr node; +bool common_goal_accepted = false; +rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN; +int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; + +void common_goal_response( + std::shared_future::SharedPtr> future) +{ + RCLCPP_DEBUG( + node->get_logger(), "common_goal_response time: %f", + rclcpp::Clock().now().seconds()); + auto goal_handle = future.get(); + if (!goal_handle) { + common_goal_accepted = false; + printf("Goal rejected\n"); + } else { + common_goal_accepted = true; + printf("Goal accepted\n"); + } +} + +void common_result_response( + const rclcpp_action::ClientGoalHandle + ::WrappedResult & result) +{ + printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); + common_resultcode = result.code; + common_action_result_code = result.result->error_code; + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + printf("SUCCEEDED result code\n"); + break; + case rclcpp_action::ResultCode::ABORTED: + printf("Goal was aborted\n"); + return; + case rclcpp_action::ResultCode::CANCELED: + printf("Goal was canceled\n"); + return; + default: + printf("Unknown result code\n"); + return; + } +} + +void common_feedback( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback) +{ + std::cout << "feedback->desired.positions :"; + for (auto & x : feedback->desired.positions) { + std::cout << x << "\t"; + } + std::cout << std::endl; + std::cout << "feedback->desired.velocities :"; + for (auto & x : feedback->desired.velocities) { + std::cout << x << "\t"; + } + std::cout << std::endl; +} + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + node = std::make_shared("trajectory_test_node"); + + std::cout << "node created" << std::endl; + + rclcpp_action::Client::SharedPtr action_client; + action_client = rclcpp_action::create_client( + node->get_node_base_interface(), + node->get_node_graph_interface(), + node->get_node_logging_interface(), + node->get_node_waitables_interface(), + "/joint_trajectory_controller/follow_joint_trajectory"); + + bool response = + action_client->wait_for_action_server(std::chrono::seconds(1)); + if (!response) { + throw std::runtime_error("could not get action server"); + } + std::cout << "Created action server" << std::endl; + + std::vector joint_names = {"slider_to_cart"}; + + std::vector points; + trajectory_msgs::msg::JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap + point.positions.resize(joint_names.size()); + + point.positions[0] = 0.0; + + trajectory_msgs::msg::JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(1.0); + point2.positions.resize(joint_names.size()); + point2.positions[0] = -1.0; + + trajectory_msgs::msg::JointTrajectoryPoint point3; + point3.time_from_start = rclcpp::Duration::from_seconds(2.0); + point3.positions.resize(joint_names.size()); + point3.positions[0] = 1.0; + + trajectory_msgs::msg::JointTrajectoryPoint point4; + point4.time_from_start = rclcpp::Duration::from_seconds(3.0); + point4.positions.resize(joint_names.size()); + point4.positions[0] = 0.0; + + points.push_back(point); + points.push_back(point2); + points.push_back(point3); + points.push_back(point4); + + rclcpp_action::Client::SendGoalOptions opt; + opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1); + opt.result_callback = std::bind(common_result_response, std::placeholders::_1); + opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2); + + control_msgs::action::FollowJointTrajectory_Goal goal_msg; + goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); + goal_msg.trajectory.joint_names = joint_names; + goal_msg.trajectory.points = points; + + auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); + + if (rclcpp::spin_until_future_complete(node, goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); + return 1; + } + RCLCPP_ERROR(node->get_logger(), "send goal call ok :)"); + + rclcpp_action::ClientGoalHandle::SharedPtr + goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); + return 1; + } + RCLCPP_ERROR(node->get_logger(), "Goal was accepted by server"); + + // Wait for the server to be done with the goal + auto result_future = action_client->async_get_result(goal_handle); + RCLCPP_INFO(node->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node->get_logger(), "get result call failed :("); + return 1; + } + + std::cout << "async_send_goal" << std::endl; + rclcpp::shutdown(); + + return 0; +} diff --git a/ignition_ros2_control_demos/examples/example_velocity.cpp b/ignition_ros2_control_demos/examples/example_velocity.cpp new file mode 100644 index 00000000..66076916 --- /dev/null +++ b/ignition_ros2_control_demos/examples/example_velocity.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("velocity_test_node"); + + auto publisher = node->create_publisher( + "/velocity_controller/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] = 1; + publisher->publish(commands); + std::this_thread::sleep_for(1s); + + commands.data[0] = -1; + 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/ignition_ros2_control_demos/launch/cart_example_effort.launch.py b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py new file mode 100644 index 00000000..abb01136 --- /dev/null +++ b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py @@ -0,0 +1,99 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + # Launch Arguments + use_sim_time = LaunchConfiguration('use_sim_time', default=False) + + ignition_ros2_control_demos_path = os.path.join( + get_package_share_directory('ignition_ros2_control_demos')) + + xacro_file = os.path.join(ignition_ros2_control_demos_path, + 'urdf', + 'test_cart_effort.xacro.urdf') + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + ignition_spawn_entity = Node( + package='ros_ign_gazebo', + executable='create', + output='screen', + arguments=['-topic', 'robot_description', + '-name', 'cartpole', + '-allow_renaming', 'true'], + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], + output='screen' + ) + + load_joint_trajectory_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'effort_controllers'], + output='screen' + ) + + return LaunchDescription([ + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [os.path.join(get_package_share_directory('ros_ign_gazebo'), + 'launch', 'ign_gazebo.launch.py')]), + launch_arguments=[('ign_args', [' -r -v 3'])]), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=ignition_spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_joint_trajectory_controller], + ) + ), + node_robot_state_publisher, + ignition_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) diff --git a/ignition_ros2_control_demos/launch/cart_example_position.launch.py b/ignition_ros2_control_demos/launch/cart_example_position.launch.py new file mode 100644 index 00000000..5e596de6 --- /dev/null +++ b/ignition_ros2_control_demos/launch/cart_example_position.launch.py @@ -0,0 +1,99 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + # Launch Arguments + use_sim_time = LaunchConfiguration('use_sim_time', default=False) + + ignition_ros2_control_demos_path = os.path.join( + get_package_share_directory('ignition_ros2_control_demos')) + + xacro_file = os.path.join(ignition_ros2_control_demos_path, + 'urdf', + 'test_cart_position.xacro.urdf') + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + ignition_spawn_entity = Node( + package='ros_ign_gazebo', + executable='create', + output='screen', + arguments=['-topic', 'robot_description', + '-name', 'cartpole', + '-allow_renaming', 'true'], + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], + output='screen' + ) + + load_joint_trajectory_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'], + output='screen' + ) + + return LaunchDescription([ + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [os.path.join(get_package_share_directory('ros_ign_gazebo'), + 'launch', 'ign_gazebo.launch.py')]), + launch_arguments=[('ign_args', [' -r -v 3'])]), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=ignition_spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_joint_trajectory_controller], + ) + ), + node_robot_state_publisher, + ignition_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) diff --git a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py new file mode 100644 index 00000000..09846af9 --- /dev/null +++ b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -0,0 +1,99 @@ +# 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. + +import os + +from ament_index_python.packages import get_package_share_directory + + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration + +from launch_ros.actions import Node + +import xacro + + +def generate_launch_description(): + # Launch Arguments + use_sim_time = LaunchConfiguration('use_sim_time', default=False) + + ignition_ros2_control_demos_path = os.path.join( + get_package_share_directory('ignition_ros2_control_demos')) + + xacro_file = os.path.join(ignition_ros2_control_demos_path, + 'urdf', + 'test_cart_velocity.xacro.urdf') + + doc = xacro.parse(open(xacro_file)) + xacro.process_doc(doc) + params = {'robot_description': doc.toxml()} + + node_robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[params] + ) + + ignition_spawn_entity = Node( + package='ros_ign_gazebo', + executable='create', + output='screen', + arguments=['-topic', 'robot_description', + '-name', 'cartpole', + '-allow_renaming', 'true'], + ) + + load_joint_state_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], + output='screen' + ) + + load_joint_trajectory_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_start_controller', 'velocity_controller'], + output='screen' + ) + + return LaunchDescription([ + # Launch gazebo environment + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [os.path.join(get_package_share_directory('ros_ign_gazebo'), + 'launch', 'ign_gazebo.launch.py')]), + launch_arguments=[('ign_args', [' -r -v 3'])]), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=ignition_spawn_entity, + on_exit=[load_joint_state_controller], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_controller, + on_exit=[load_joint_trajectory_controller], + ) + ), + node_robot_state_publisher, + ignition_spawn_entity, + # Launch Arguments + DeclareLaunchArgument( + 'use_sim_time', + default_value=use_sim_time, + description='If true, use simulated clock'), + ]) diff --git a/ignition_ros2_control_demos/package.xml b/ignition_ros2_control_demos/package.xml new file mode 100644 index 00000000..12dc4684 --- /dev/null +++ b/ignition_ros2_control_demos/package.xml @@ -0,0 +1,45 @@ + + + ignition_ros2_control_demos + 0.0.0 + ignition_ros2_control_demos + + Alejandro Hernandez + + Apache License 2.0 + + http://ros.org/wiki/gazebo_ros_control + https://github.com/ros-simulation/gazebo_ros2_control/issues + https://github.com/ros-simulation/gazebo_ros2_control + + Alejandro Hernández + + ament_cmake + + control_msgs + rclcpp + rclcpp_action + std_msgs + + ament_index_python + control_msgs + hardware_interface + ignition_ros2_control + joint_state_controller + joint_trajectory_controller + launch + launch_ros + rclcpp + robot_state_publisher + std_msgs + velocity_controllers + xacro + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf new file mode 100644 index 00000000..5cef6feb --- /dev/null +++ b/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ignition_ros2_control/IgnitionSystem + + + + -15 + 15 + + + + + + + + + + + $(find ignition_ros2_control_demos)/config/cartpole_controller_effort.yaml + + + + diff --git a/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf new file mode 100644 index 00000000..e888edae --- /dev/null +++ b/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -0,0 +1,77 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ignition_ros2_control/IgnitionSystem + + + + -15 + 15 + + + + + + + + + + + $(find ignition_ros2_control_demos)/config/cartpole_controller_position.yaml + + + + diff --git a/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf new file mode 100644 index 00000000..25f9eb63 --- /dev/null +++ b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ignition_ros2_control/IgnitionSystem + + + + -15 + 15 + + + + + + + + + + + $(find ignition_ros2_control_demos)/config/cartpole_controller_velocity.yaml + + + + From c85f448bce74aa3f92acbc9c3267938c608fce63 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Sun, 14 Mar 2021 00:06:17 +0100 Subject: [PATCH 03/50] Added github actions Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 43 +++++++++++++++++++++++++++++++ ignition_ros2_control/package.xml | 3 +++ 2 files changed, 46 insertions(+) create mode 100644 .github/workflows/ci.yaml diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml new file mode 100644 index 00000000..a934c5a1 --- /dev/null +++ b/.github/workflows/ci.yaml @@ -0,0 +1,43 @@ +name: Ignition ros2 control CI + +on: + pull_request: + push: + branches: [ master ] + +jobs: + build: + runs-on: ubuntu-latest + container: + image: osrf/ros:foxy-desktop + steps: + - uses: actions/checkout@v2 + - name: Setup colcon workspace + id: configure + run: | + cd .. + mkdir -p /home/ros2_ws/src + cp -r ignition_ros2_control /home/ros2_ws/src/ + 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 + cd /home/ros2_ws/src/ + rosdep update + rosdep install --from-paths ./ -i -y --rosdistro foxy \ + --ignore-src + - name: Build project + id: build + run: | + cd /home/ros2_ws/ + . /opt/ros/foxy/local_setup.sh + colcon build --packages-up-to ignition_ros2_control_demos + - name: Run tests + id: test + run: | + cd /home/ros2_ws/ + . /opt/ros/foxy/local_setup.sh + colcon test --event-handlers console_direct+ --packages-select ignition_ros2_control ignition_ros2_control_demos + colcon test-result diff --git a/ignition_ros2_control/package.xml b/ignition_ros2_control/package.xml index f1b2a3a1..4d8eac89 100644 --- a/ignition_ros2_control/package.xml +++ b/ignition_ros2_control/package.xml @@ -11,6 +11,9 @@ angles ament_index_cpp + ignition-gazebo5 + ignition-physics4 + ignition-plugin1 pluginlib rclcpp yaml_cpp_vendor From d7d44ece14cbf7d5b8e079adc5b066b614b8b2fc Mon Sep 17 00:00:00 2001 From: ahcorde Date: Sun, 14 Mar 2021 00:10:10 +0100 Subject: [PATCH 04/50] udpate github actions Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index a934c5a1..cec8881e 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -17,7 +17,7 @@ jobs: run: | cd .. mkdir -p /home/ros2_ws/src - cp -r ignition_ros2_control /home/ros2_ws/src/ + cp -r ign_ros2_control /home/ros2_ws/src/ apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -q -y --no-install-recommends \ dirmngr \ From 483a97348576b4f136d416a4df05a9377f118bb4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sun, 14 Mar 2021 08:22:26 +0100 Subject: [PATCH 05/50] update github actions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index cec8881e..d43486b1 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -25,6 +25,7 @@ jobs: lsb-release \ python3-colcon-ros cd /home/ros2_ws/src/ + sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' rosdep update rosdep install --from-paths ./ -i -y --rosdistro foxy \ --ignore-src From 33a888b5f351595247f91ae70f4a69f70dc4e65c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sun, 14 Mar 2021 08:25:40 +0100 Subject: [PATCH 06/50] update github actions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index d43486b1..0063bfbd 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -18,6 +18,7 @@ jobs: cd .. mkdir -p /home/ros2_ws/src cp -r ign_ros2_control /home/ros2_ws/src/ + sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -q -y --no-install-recommends \ dirmngr \ @@ -25,7 +26,6 @@ jobs: lsb-release \ python3-colcon-ros cd /home/ros2_ws/src/ - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' rosdep update rosdep install --from-paths ./ -i -y --rosdistro foxy \ --ignore-src From 9f47fe7d23d1a56c172a4375e27ce4437f444d4e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sun, 14 Mar 2021 08:30:28 +0100 Subject: [PATCH 07/50] update github actions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 0063bfbd..40de8015 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -19,12 +19,15 @@ jobs: mkdir -p /home/ros2_ws/src cp -r ign_ros2_control /home/ros2_ws/src/ sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' + wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - + IGN_DEPS="libignition-gazebo5-dev libignition-plugin1-dev libignition-physics4-dev" 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 + python3-colcon-ros \ + $IGN_DEPS cd /home/ros2_ws/src/ rosdep update rosdep install --from-paths ./ -i -y --rosdistro foxy \ From 622614e6c21f1d4ba6cd520df4dd4af74d42ad2f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sun, 14 Mar 2021 08:32:46 +0100 Subject: [PATCH 08/50] update github actions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 40de8015..b2dadae3 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -15,6 +15,8 @@ jobs: - name: Setup colcon workspace id: configure run: | + apt update -qq + apt install -qq -y lsb-release wget cd .. mkdir -p /home/ros2_ws/src cp -r ign_ros2_control /home/ros2_ws/src/ From 5df52f97a5d18f8e25b9954a1a98bf524f9be95e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sun, 14 Mar 2021 08:37:34 +0100 Subject: [PATCH 09/50] update github actions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index b2dadae3..6ac76c08 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -21,6 +21,7 @@ jobs: mkdir -p /home/ros2_ws/src cp -r ign_ros2_control /home/ros2_ws/src/ sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' + sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - IGN_DEPS="libignition-gazebo5-dev libignition-plugin1-dev libignition-physics4-dev" apt-get update && apt-get upgrade -q -y From 9121427d8dbfc176c245b30810a364a4f8aea159 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sun, 14 Mar 2021 08:42:38 +0100 Subject: [PATCH 10/50] update github actions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 6ac76c08..06458beb 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -23,7 +23,7 @@ jobs: sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - - IGN_DEPS="libignition-gazebo5-dev libignition-plugin1-dev libignition-physics4-dev" + IGN_DEPS="libignition-gazebo5-dev libignition-plugin-dev libignition-physics4-dev" apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -q -y --no-install-recommends \ dirmngr \ From fa457b6f65f7be3774d796df6e7b209a7161d0d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sun, 14 Mar 2021 08:46:47 +0100 Subject: [PATCH 11/50] update github actions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 06458beb..e8ac1c8b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -20,12 +20,11 @@ jobs: cd .. mkdir -p /home/ros2_ws/src cp -r ign_ros2_control /home/ros2_ws/src/ - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - IGN_DEPS="libignition-gazebo5-dev libignition-plugin-dev libignition-physics4-dev" apt-get update && apt-get upgrade -q -y - apt-get update && apt-get install -q -y --no-install-recommends \ + apt-get update && apt-get install -qq -y \ dirmngr \ gnupg2 \ lsb-release \ From 4b4c1882515c323a11d6f771b468b50e2f6c216a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sun, 14 Mar 2021 08:55:22 +0100 Subject: [PATCH 12/50] update github actions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 7 +- README.md | 198 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 203 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index e8ac1c8b..70140112 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -9,18 +9,20 @@ jobs: build: runs-on: ubuntu-latest container: - image: osrf/ros:foxy-desktop + image: ubuntu:20.04 steps: - uses: actions/checkout@v2 - name: Setup colcon workspace id: configure run: | apt update -qq - apt install -qq -y lsb-release wget + apt install -qq -y lsb-release wget curl cd .. mkdir -p /home/ros2_ws/src cp -r ign_ros2_control /home/ros2_ws/src/ sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' + sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu focal main" > /etc/apt/sources.list.d/ros2-latest.list' + curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - IGN_DEPS="libignition-gazebo5-dev libignition-plugin-dev libignition-physics4-dev" apt-get update && apt-get upgrade -q -y @@ -31,6 +33,7 @@ jobs: python3-colcon-ros \ $IGN_DEPS cd /home/ros2_ws/src/ + rosdep init rosdep update rosdep install --from-paths ./ -i -y --rosdistro foxy \ --ignore-src diff --git a/README.md b/README.md index 58ec7650..ac233453 100644 --- a/README.md +++ b/README.md @@ -1 +1,199 @@ # ign_ros2_control + +This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Ignition Robotics Gazebo](http://ignitionrobotics.org/) simulator. + +This package provides a Ignition Gazebo system plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model. + +# Usage + +This repository contains the contents for testing ign_ros2_control + +It is running Gazebo and some other ROS 2 nodes. + +## Video + Pictures + +# TODO ADD picture + +## Running + +### Modifying or building your own + +```bash +cd Docker +docker build -t ignition_ros2_control . +``` + +### To run the demo + +#### Using Docker + +Docker allows us to run the demo without GUI if we don't configure it properly. The following command runs the demo without GUI: + +```bash +docker run -it --rm --name ignition_ros2_control_demo --net host ignition_ros2_control ros2 launch ignition_ros2_control_demos cart_example_position.launch.py gui:=false +``` + +The in your local machine you can run the Gazebo client: + +```bash +gzclient +``` + +#### Using Rocker + +To run the demo with GUI we are going to use [rocker](https://github.com/osrf/rocker/) which is a tool to run docker +images with customized local support injected for things like nvidia support. And user id specific files for cleaner +mounting file permissions. You can install this tool with the following [instructions](https://github.com/osrf/rocker/#installation). + +The following command will launch Gazebo: + +```bash +rocker --x11 --nvidia --name ignition_ros2_control_demo ignition_ros2_control:latest +``` + +The following commands allow to move the cart in the rail: + +```bash +docker exec -it ignition_ros2_control_demo bash +source /home/ros2_ws/install/setup.bash +ros2 run ignition_ros2_control_demos example_position +``` + + +## Add ros2_control tag to a URDF + +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 + + - a specific `` for our robot + - `` tag including the robot controllers: commands and states. + +```xml + + + ignition_ros2_control/GazeboSystem + + + + -1000 + 1000 + + + + + + +``` + +## Add the ignition_ros2_control plugin + +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 `ignition_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. + +```xml + + + robot_description + robot_state_publisher + $(find ignition_ros2_control_demos)/config/cartpole_controller.yaml + + +``` + +The `ignition_ros2_control` `` tag also has the following optional child elements: + + - ``: 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` + - ``: YAML file with the configuration of the controllers + +#### Default ignition_ros2_control Behavior + +By default, without a `` tag, `ignition_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: + + - hardware_interface::JointStateInterface + - hardware_interface::EffortJointInterface + - hardware_interface::VelocityJointInterface + +#### Advanced: custom ignition_ros2_control Simulation Plugins + +The `ignition_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 `ignition_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 + + + ignition_ros2_control/GazeboSystem + + ... + + + + ... + + +``` + +#### Set up controllers + +Use the tag `` inside `` to set the YAML file with the controller configuration. + +```xml + + + $(find ignition_ros2_control_demos)/config/cartpole_controller.yaml + + +``` + +This controller publishes the state of all resources registered to a +`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 +``` + +This controller creates an action called `/cart_pole_controller/follow_joint_trajectory` of type `control_msgs::action::FollowJointTrajectory`. + +```yaml +cart_pole_controller: + ros__parameters: + type: joint_trajectory_controller/JointTrajectoryController + joints: + - slider_to_cart + write_op_modes: + - slider_to_cart +``` +#### Executing the examples + +There are some examples in the `ignition_ros2_control_demos` package. These examples allow to launch a cart in a 30 meter rail. + +You can run some of the configuration running the following commands: + +```bash +ros2 launch ignition_ros2_control_demos cart_example_position.launch.py +ros2 launch ignition_ros2_control_demos cart_example_velocity.launch.py +ros2 launch ignition_ros2_control_demos cart_example_effort.launch.py +``` + +Send example commands: + +When the Gazebo world is launched you can run some of the following commads to move the cart. + +```bash +ros2 run ignition_ros2_control_demos example_position +ros2 run ignition_ros2_control_demos example_velocity +ros2 run ignition_ros2_control_demos example_effort +``` From cb42397f35e1afcdf837a4166ec614ac63008bcd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sun, 14 Mar 2021 08:57:23 +0100 Subject: [PATCH 13/50] update github actions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 70140112..75daa1c1 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -16,7 +16,7 @@ jobs: id: configure run: | apt update -qq - apt install -qq -y lsb-release wget curl + apt install -qq -y lsb-release wget curl gnupg2 cd .. mkdir -p /home/ros2_ws/src cp -r ign_ros2_control /home/ros2_ws/src/ From 52c5a08c7a3650e4b84993f6f6009e5d605dcd93 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sun, 14 Mar 2021 22:45:31 +0100 Subject: [PATCH 14/50] update github actions MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 75daa1c1..49a4bb21 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -20,10 +20,12 @@ jobs: cd .. mkdir -p /home/ros2_ws/src cp -r ign_ros2_control /home/ros2_ws/src/ + sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' + sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-prerelease `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-prerelease.list' sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' - sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu focal main" > /etc/apt/sources.list.d/ros2-latest.list' - curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - + sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' + curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - IGN_DEPS="libignition-gazebo5-dev libignition-plugin-dev libignition-physics4-dev" apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -qq -y \ From a1d1da883619613071e0c206f16db10a0a633bc4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sat, 20 Mar 2021 23:10:52 +0100 Subject: [PATCH 15/50] Fixed warnings MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- ignition_ros2_control/src/ignition_ros2_control_plugin.cpp | 4 +--- ignition_ros2_control/src/ignition_system.cpp | 7 ++++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index 7cd86c87..d1acd27d 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -139,7 +139,6 @@ class IgnitionROS2ControlPluginPrivate public: std::map GetEnabledJoints( const ignition::gazebo::Entity & _entity, - const std::shared_ptr & _sdf, ignition::gazebo::EntityComponentManager & _ecm) const { std::map output; @@ -261,7 +260,6 @@ void IgnitionROS2ControlPlugin::Configure( // Get list of enabled joints auto enabledJoints = this->dataPtr->GetEnabledJoints( _entity, - _sdf, _ecm); // Read urdf from ros parameter server then @@ -375,7 +373,7 @@ void IgnitionROS2ControlPlugin::Configure( ////////////////////////////////////////////////// void IgnitionROS2ControlPlugin::Update( const ignition::gazebo::UpdateInfo & _info, - ignition::gazebo::EntityComponentManager & _ecm) + ignition::gazebo::EntityComponentManager & /*_ecm*/) { // Get the simulation time and period rclcpp::Time sim_time_ros(std::chrono::duration_cast( diff --git a/ignition_ros2_control/src/ignition_system.cpp b/ignition_ros2_control/src/ignition_system.cpp index 3b5e0c42..52a6d921 100644 --- a/ignition_ros2_control/src/ignition_system.cpp +++ b/ignition_ros2_control/src/ignition_system.cpp @@ -307,10 +307,11 @@ hardware_interface::return_type IgnitionSystem::read() this->dataPtr->ecm->Component( this->dataPtr->sim_joints_[j]); + // TODO(ahcorde): Revisit this part // Get the joint force - const auto * jointForce = - this->dataPtr->ecm->Component( - this->dataPtr->sim_joints_[j]); + // const auto * jointForce = + // this->dataPtr->ecm->Component( + // this->dataPtr->sim_joints_[j]); // Get the joint position const auto * jointPositions = From 59baef11c859b77d1cd13c0a8ed2526125e578e9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sat, 20 Mar 2021 23:11:12 +0100 Subject: [PATCH 16/50] Update CI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 49a4bb21..e94540f0 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -26,19 +26,22 @@ jobs: wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - - IGN_DEPS="libignition-gazebo5-dev libignition-plugin-dev libignition-physics4-dev" + IGN_DEPS="libignition-gazebo5-dev libignition-plugin-dev libignition-physics4-dev libignition-utils1-cli-dev" apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -qq -y \ dirmngr \ gnupg2 \ lsb-release \ python3-colcon-ros \ + python3-colcon-common-extensions \ + python3-rosdep \ + build-essential \ $IGN_DEPS cd /home/ros2_ws/src/ rosdep init rosdep update rosdep install --from-paths ./ -i -y --rosdistro foxy \ - --ignore-src + --ignore-src --skip-keys "ignition-gazebo5 ignition-physics4 ignition-plugin1" - name: Build project id: build run: | From 3cd82f998a3b00888b789c5c12180e2c0a813b5d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Herna=CC=81ndez?= Date: Sun, 21 Mar 2021 09:43:20 +0100 Subject: [PATCH 17/50] Update CI MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- .github/workflows/ci.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index e94540f0..2794e53b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -15,6 +15,7 @@ jobs: - name: Setup colcon workspace id: configure run: | + export DEBIAN_FRONTEND=noninteractive apt update -qq apt install -qq -y lsb-release wget curl gnupg2 cd .. From 55dd07661ee396dd70136af7d3ad020d83902c43 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Sun, 21 Mar 2021 23:42:21 +0100 Subject: [PATCH 18/50] update README.md Signed-off-by: ahcorde --- README.md | 10 ++++++---- .../launch/cart_example_position.launch.py | 2 +- .../urdf/test_cart_position.xacro.urdf | 1 + img/ign_ros2_control.gif | Bin 0 -> 83704 bytes 4 files changed, 8 insertions(+), 5 deletions(-) create mode 100644 img/ign_ros2_control.gif diff --git a/README.md b/README.md index ac233453..119aff37 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,11 @@ This is a ROS 2 package for integrating the `ros2_control` controller architectu This package provides a Ignition Gazebo system plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model. +Tested on: + + - ROS 2 Foxy + - Ignition Edifice + # Usage This repository contains the contents for testing ign_ros2_control @@ -12,7 +17,7 @@ It is running Gazebo and some other ROS 2 nodes. ## Video + Pictures -# TODO ADD picture +![](img/ign_ros2_control.gif) ## Running @@ -59,7 +64,6 @@ source /home/ros2_ws/install/setup.bash ros2 run ignition_ros2_control_demos example_position ``` - ## Add ros2_control tag to a URDF To use `ros2_control` with your robot, you need to add some additional elements to your URDF. @@ -106,8 +110,6 @@ robot hardware interfaces between `ros2_control` and Gazebo. The `ignition_ros2_control` `` tag also has the following optional child elements: - - ``: 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` - ``: YAML file with the configuration of the controllers #### Default ignition_ros2_control Behavior diff --git a/ignition_ros2_control_demos/launch/cart_example_position.launch.py b/ignition_ros2_control_demos/launch/cart_example_position.launch.py index 5e596de6..13e2d62f 100644 --- a/ignition_ros2_control_demos/launch/cart_example_position.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_position.launch.py @@ -76,7 +76,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( [os.path.join(get_package_share_directory('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')]), - launch_arguments=[('ign_args', [' -r -v 3'])]), + launch_arguments=[('ign_args', [' -r -v 3 empty.sdf'])]), RegisterEventHandler( event_handler=OnProcessExit( target_action=ignition_spawn_entity, diff --git a/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf index e888edae..f9967a18 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -16,6 +16,7 @@ + diff --git a/img/ign_ros2_control.gif b/img/ign_ros2_control.gif new file mode 100644 index 0000000000000000000000000000000000000000..65664bde015eaa50ab537f501ef638620f1deb9b GIT binary patch literal 83704 zcmeF2_d6Tj`~O2oB8ix-tyXJmirV_J_iELqMHN+56phu!j4jk&rM9B>Y&CX75Nc~R z_9luN9lm}49pBI6I@fj1&*wVN`?~MveIH$Y9R)?FO`tsR6mbgxpaKA%002nF+ocsZt{RLbCs3JS4`WC6?PN<`|XlWpztM?!e{Gf0V zsFV|oK?NZff(WT(U}R=^Ph#kqV|1-x964a21+&OnUD3~E<5FZp^|4E+al!<+SXl(< zK|(HdH+Y1E>3BqSy~JWmBp@6Tft5EUC6HEWa>`nA_8|(_SQQyyiqS-ssbdX&M=fmw zE$c|F)&Xr3FKx$%I!5<(b@cV@FnUgJ^z{w&>%JO3D>3~3%Sb`P*yx$Dt*@!6soBcq zU1^@X2Kx7SVD}81?)enm3$4HZkEqWUR@N525tiCEwzjr*_Vy3{QM>%m)aqd>!SNqX zPEG<)CqvzTyR*^_=Y-Bjj~=;X54#k7dHndXtE;QKtILzf%qNw7Pdz-IKY#8cM)$(R z*so$L;Dt}X%a_~E80pY+RrY!dmV$p#Ky+OB__tl$0x+cB_t#yCdMWvCMG2%CC5di3~r>R zrlvdUW@Kb!N!uO3u_t=XfoQuk8-^BzK zdp{^EEiLbpn{HSZf58XB9L8pl?eLd=@~(bCe| z+S(Rj^Ktp$)2B~e7;BuLSWiz+zq>(y|3LpRX`ly(A0iCBaUAL>8zK-1kxIjI48z0` z;_e}dM4FhG__DG)S?)LGq&zh>J)LbjQ}TFbf;dm;SXfy2`t{qlzPI1zXTE>Ld|z4p z{^OY398CVUK;GM1US3{bU*B}NwYj;uHIlRSkNtz~&;Hwk2|I=yJG;AkE2De6-}m;m z_x`c}KMp4I4pur3_LnL9bBEumfBZQ3aeREVi$6L#I@#$up-i3}Z=IZ+TwGlIvJ(7b zdF}7tzn4}5{Fg8j?fd4M>So%~Qb^!GF#!0_X=s4}1VG~78Sp=uVE#`e|0k0FpG5+> z1X#^->D3i>M?hG_?D2I)y%<%z@BU`K8>f*i}fZxoYaYb)&~O zLi78X58%ph9>bRExkmRdRSt&CDbOnzi8$jp1bemR@ccMeAOq=*XYd@9*Ql*-sW(Qq zOSMRV($@~9(kn1(|FQaJY1MP~V|!$lr@%q8e)ova@|Ozp_YX#tb{1NXj5Pab#vAqs zH;TsDQ*;UrdYc^XKluiU%v@PWpj6V3ASP0fCJg@S@09yWqQZLH zoABi8Q4AuijF`7~R>|JF;vdd@G@yh}#j!$W2KVr($HXHdoy6~yRl1w3rw<^=l5PWj zSr_!M?~-gcmzY6=_l-oQ0nv>-zv*md7qw4cqO(;vSr;Ch;~HVKWh70q+M4Xr4b^?w z6z+&{kbZK(4VPOMU8XSVqdJ4spa_oIB@GBLS z@1w={+g1fytRG$3qHIh zG!lNAF&hoRGg_PSaPajyKmi$ghY33wJBNdOn2(cB#rkx{h4uS^C0JVZ_~&;(noDqb zvYL#9x>zy=6o5RM#*ZbcOz7SCGF`u?#?t8M!(wFc(PvOVSMp@?ey3Kxf9Rda4O38# zV?_8-TQ!3(+M-ev_cNwLv<;L55YsqN*<0(KbBAR$>ts5dEQXw3?!D9lXe45p^v4BF zxmekkXH`0u#BD>4Ke|!jEORK-Z9hro2>O!lZv&7%gv-&4m^mX|K+e45kMGs3`xiTO zkLj5t48~s=vs^W+AG(FBW)|pDj9*HSeTA>1#;uYVdxbx)E`OAGa`5h**rZlt+||BV z%W!zOLNAbO8 z#x#1Egdz%ZJ}$qldU2SH?h|}Qt&i8>OpTpaED>NE!hfc(NWmtQ2o0?b=-d8C&5AU+ zE;BS_5|oO}{aqrWt8Y+S@jk7>s8p;qyuqU5M|z!+fyg8MVcYozp~%issh2~;sifLW z9J~yf-1vfWATvY(NUX)X4h>r-YfV zi<1xNX7QUK!`U9rpLH>ko_rzQvja*$>#>Q|F2aU$1lyktStHf1^22kZK|dP{>1*6| z4d*AqhTN z32JxN!#-LUEl*rR1ZVY(Lh565jh2~nQGKad(Q#J9<*S`%{m7*H1UI7<&V{o9h0*$? zm&6s`UuT1Ahz2ZX_tA5sV%7pjx73tsm+Kz@aI>U_v~r_0F}rhu^=LyzD{)OK_)8v@V@{PINbE%$Xss%XMa75+`FC^EQk&lokR<(>9|EPKg_Lew~k@5lw|)<4p~& z3sQ_p(>u12O&#TnacojkiSXX0zh5zH1x{93X5@+SBWHLaqParXc>7-N#bkv^bCuP| zwq@tVR9#YYwVUyd?ZU-$+h}v`%aNT2zjCJ2)*El&)OVe^e$C=dS{gD(c3qW!&5@E? zn#zs$JnVkW&yM~}%t!XT9xd=LLPYq8NRUj{udkaX^6lL?S}91|H%F1yjt%33poL$H zXQQoMry~cie*O9mU}(dEO(@}9zsV5OwjQ=oZ~vFQTnxzyeZo?Qapu358BQY={!>&*0DeP!I!_t44*I01~1R2*%nxC8-2c*8TMlCE10A@o4d151!+9FhDBb4x=PZ_}> z6i5LYnp_9^#s;ZygpsbmyBuMqjF9hOFvcvh&Josxh89pD0q97wE?uB^gfl9T1A^dk z4|mUs_GpXtoQ?K6i$27Ji$YLu%>rG?;V3-qOly!g&N=`YR)K{D)L|Z-1-2la+p%!L z0c*hwc>5(fCQ>ixDGsE$Nmq@7br;6OC`8O z<^>)KjzN9N#K6u_?^{82jB)LZ@k?yrR5NJ38Ejn-UU5At3KBF(X4v0k_~OWLu*qO8 zpL{`vA8o=*btCJrut9I<8?LaBx}eg+SpG72kzQOW9#()3^~A!et=$HZ*ssELL*DS$ zo9K?L*qp*pPexb~d+LxGOwKx}*ZTEx9i3(&jRBG-w-B)lNR(E5BWv?UZYyl!QURmG zm=LAw7sE&A?Twkmqa5pk3iwj1$x+`1!F6izLB3Q04tR{(t3WiEBRgpCEa^}o`Q$9= zG%LAu1NH@gb*Y3`BV)=3p`(S)iKvtkymdqrR7EkUlrbeh2VMmR7nr3+4MMwd*cdi2 z86SGg8drhLs%iyQ0MhwwU}Y~;k2atlxYV>t8f7xla< z?9mmO>HBTz2eaw>XXytO$t4@G8oi7gpg7c%pi(m2cOYXFjUBvHgPUx^BB!vGU|4{S z+ZSYRVJj%X0vZuWhslhq$H9C^p~+;}5Dpes7aE4k5*UKq$PVkTS{#@NW}D>Ar5znGV5{ z0SVDo&_Ob+8dEay0p6EzV9F#RX5WDs28y`d-R~S=8BVR{1bU7Qng{1#VdX-F|=jbTP2ztFy2p6#pLL|1ht^C0)g-*GTnJL zMr}UOn-Ltl8Pgk=^?ndc(oRhgp^HIRFS>z3kTo;%HDR_v^?VL3jPQvm6}GuZBX$Qb z+k9uGjPFVXcXMj*DwURI!m2vxnNG96@D;2Ml}7YKD=|>(jsgNDQ)lk|c{?Z!04tzW zy8*zZjBuXHPz(+>#0XC#hgKB6r_O=7)qz5AkTh>P?%N()h7p{HeRY5ZzpR622hs;zGSZ!v)9X4yH*l}^3u&WCjhRYKM>Cag=Mg)OrQ8sP z2}W3x8Vu!BbvqJaeb!W4*Kjt|;8WLZmkVlRe0?*G?e;Mu$P9E;hnOyerv$y;Lc-rW zf?raaPjugpo))POq6h}{X4$X`Z-{3dDv_-^TerMF2NtYXUg-w=qhFjMT&GG11#*DH z(BLq}cobh-j&RB7RuXB8VPY$3QZZ;+-;GdMTb5h9s}y9x>BgYz$>>(QVp9;64okKR zx)A&DXvmpAAZ1*g@)Jh|z}mE}urE1NyBJ}q=$<6`pzfz_(N=B2v$=dvg8~2!sf^W6agm#J z?dt)cu%Mcf@^&pkyUupH9<;;2prf3tzfwG?lv4W*=R+pZ*Zs=)6kJg8vU6q&-aXVG zRT}DpP72C}_qGQmGeTn7;RV><1^IX4yE!AjAf?`sIWIrSHPK!3u2{&ekjGJFQ9kXT zBG7!aDVK5G_sJmj1;h!7P76mLZS-0xx9bk9i7VlAUf!!k(QqRe&a4xDV2EpS#HD+_ z-T~0-;^@+$!9T1JCX94$!@TKG295-HuEcA1LwYh2#;MXeBA@vBWlv%&2+`TYFG@F6 z{;u#eC8-S~=ZR{r`xs*wl!>;*iwC+Apjn-Ph=l3~wtd5j$+?EfhTQqay9`C*LB+~$ zrCj}G%Kb&UFtIW>G9Q9Wre8pQU@#nrih@3@c5_l0XfU*%FYifs0`v6@it$cK+AKYJ zG9W?7Vjlvf71GUo3=AP6ati@a3RT$GLT(Cxp^mPz`V|EUQ!5-k^oBY!LY;9{KQ752 z7s{t?Z+dqejX58vO~?biNn5gF%(m1?^0-b%|7RIPt7h=@NIs6CxP2` zBUDtsCh~RfmDArR!-{Y;CWW9`5@H4bG9uBs;Xrwelh3hgwqzh%E~pm=$~GHvl!WbM z7VTmYxCB_U%IoVYs8p-axI$u)H59r@EEoh25n*Y3bZ>a(-l)#KoqemL5)>bpljT+{ z>lyUa8_aAr|FSU6odBM}fRWZgdc^dt-S(ZYqeZ8qzE{V7By^OP$5iL~+e!@7FIZ!4 zlYVq`($SH+;)BA_1(d*YmbvLt(#LnLp#C_h??+G$4rVnpFo+!(x-2YvRzsJGeLHYI z;zk<3Mn3`d2EquRG#w{y{Gqd6c;&E1h*1HP)M#Yi(_1(~_6iY19L*CF^gV^vRITwq z4dSLo`<4Q2VC>$?MRbsXud%RVZ%~fJ)ZKje0)TMIh5Ijm%A#}XtQB$cr|ZTm8h3AS zJ97Ft5;0K+wcvx4C%{~OftLB`ivTn=09dUfeK~+O?JMGtM5iJ3z8p&IkRbNi6EVi2 zj*eyi3Gbkgpj1ZqV$JM3_WH8&wzlj*-67cevp|$1urzdC4h>dk0|#Q?rVN7z1B2$o zYQJ(iEOI(@GuD!B*S@oJYs+7_XB(7L5#H7X3tb|4NYLkAZ4 z0y)dcTO@eiwEO_qjD>am4YQ&3u+b1*o*BY|OyDqR zc})4&Wi>`3rC``-~bBslUG3>E*(mAp&6^tesouDIIpwh)dEx70*C6&GVnm6 z7wIPfRz3)JwcQFqL$XojIinC#KXl?xU@{+F2;X2T8Aiat%0;#t{{~jJx!~HqD(*t! zN5(;fSG>=5w*OFRlNRG_Cpvk*AC0_-r+M;v$%1cPT>ZvCORIAM;1GSjr2^pj9IB7+*1yvO63bQn)NdXBM^fEZt^4 z%kdt@w;n4m^8HEKLYFno1hrb8w$<<(wQoISw0MT3`en_H1libB_9GFicYpuFNN6hY)S%Yp`Jf$$6Wt%5& z_)I?KHjW<=;{H$_l$GEpbl?)gt_A@C_yU)lP@ZiOG!M`i&pipYMO?Ie2UEh6k_N6t zYbH4ZCYYsOI8&n*ZQ}fzDr3}v)5^j=zN-eBCS$ku=|9+U_O=Oj_wy z=Q19;iY2!6KChy-ced}3jA%&%`#R}(%SN=lsEFXO)eE2VJp#=X3bHDM_DJtHWRT(S zZ|<}Wupz`~%J!p?{!PU%j=h=O_EKONa#ACak@>Mm3F{Qy*pNuKo7HK_s5-YUpgZT?#%Il_Y}*O=yZ5CAWKh2i|$UA zXmZTDPby8`o$zmaTsAFI9H83q;3sH2Mz@LUYntA~jd#bfn7bH-ADs_Z`46; zH3J}l%Snpws$FYnZxVtm&t?(ykLolzN7F$-Jsd~~t-&4LiVwZ*UQhpyBx`-SR1DiI z4x)X^CeEfqPw}zT`si4X8n6+Z$dJ<=_J8P+so3D!5#SbBr|l!Gu`tI&lPw(IJMwpP zL560Tue+^}3KI;H>l4Ufq0>^xUiez9dMbNVvf+1d{_yUTvXZDkzu@btTLRP6C3^5> zy^{d3$~-@dv>R|wtYaLX#z{{#PEcEoyTig<7n`O+WBrT`{J`%@TM%!pe6#s2J3VLj z-s^M^_S9%aAVF=~ugoIZcj(>vsIOeKVgExh!K@vx8l++yFl8xl@i&lR;Uk)x*wx30 z*)QBm8DIRydlqwzJCE^d-lLuIhDhcUbK4!YX<^}U)*(7$K9_70f4^pXyAh+C1NGS97D=mS_WgP;K0IDSM(xrDaFLt_A>N@w$aoHlY53!PR z6|@hLs+sjbS_17#FpXXn4>~@=P>t5EB4xSr+NbykHA-X{2fHaHa&<@Y(;J#c%Bos* zw3ZYaEymBZO@&9AoH$2}k6E;&_h*Gb>c^!2e#`U-YV`kyue^}JjorL#l%fa? z!}v;%%4DzVYVV!n-v1(*Z+?wE-hug>VS0A>=y8!sHkt~4eY%l-dL@D`inoP5RkK!* zDl~n@t9>TNeWo=1`Ak~*d|GmJnjU6A1QKv-7DRZ{XBF9RZKR?2QSiK?H>$vo(}lR4!&^t;1-$m++{GpZ}4U0zj8yjKy5InX)u`Pw!bIBC}!~0@!)G3 zJX$3puqoE5Z!kg&ANi3r)CHl&h{r_YW76=(Liik{-epuDK4B97)=}?%EAfCm9{)yb=*=$PoqH(5ZzwZzNU9niZ#0zCq;F$Clr=e&zp9@ip`Y_{sF0N)PaGKY%=iv(Vh05+%P$OA^#{l-os$IjABt5t}5 zO=G|MOo@3TzgNfpejM3{8yW*hHNQ>xuu;#j(Pn4VqIl4;y`jJR*m)X>u9O7b8@@RH zSICj*L(BiHk{D?3_H&ayu$q04Fmpk-9E6B2u|_!IXsqy{P&M#F>*$|a;0M+qj!;@t z96Trn>{xNvJs#ski8RIE4aP4Gi4~95bwXY!8y#lM_~D5 z6biYzL<|#&3BQI-4C12^Wd~onvw)_tIid;?A}C_pHUoTP`4|X~WlVS%7 z;i81EPiowroJg3Eu$a^iH5Pw7sp~MwbYdX2Mqr7O3sIn*RvR0Wz}zI8J5*c=#beC% zU=DoLtW%h=YV+VWum+0Sr4C~q3RXfz-25}4**|4JWzlAIU+-khOBcb06 z46d7*X-C^d$$3s;@OQ038KcQDiJoUh!4=UOXJOF}QwiS;P}a0cWGWN(xiWuS%{@J% z7?X@r+aQa%s{Z+5Kbx9J8+6T;9M#$5k2d!b$3L6_)kJ_fd*(c9(H1p{ic^G8h1uxP z`Hp5gk*j_i6s~75jm&^N;*(ZcZ8G@=WE8VC=HO+enx|EXc+T?599G zXFb(O4l}}mo}SVAdDD6VFdp8(`cDgAw7;#X5W9H3PUlbH{J+i85A>|f^iI*v%^ECK zPeoq0bAN%kyosN~JBjkEg|FCKgjRs>xr>TaMBhY;@S~k(w__|`_?rHx)DQt3xAh(noO@V`7u#UHEaIg zBh}$qV&?6eaIr#IE?pUY$k0-33UOk|SQ?5Xb1W{>xh`=}FF_wWa{Y7)uU@*A$`v3n%nvZ6d{t5C3_y1sH!^+*M@sy1MyngPF;5>9)5|*6UImD+=BbD)2b^x)@{+(D_DK-bB(cP^)8iZ9D(W@CC496kAdcy_>1+C zp@)8=_YC3BthtOU9i2zRgmJX~G28<9=yO=Y2Q0PD^jhKS+C!ZUztJ(r{rIaB{7?1F zZkVxtD}3As0DB|gD`&K2h9EDT+h+*!dBw()T59#IXm>r^Y24gnvtCQ4O(%>`Fwmd%BVH<2k$O=FPT@r+a&P5WH4a!`G6fQq2AddpRbzi*J7B0jW;YXt7-=vmeyx%2yHL(i% zxrO=$j!OLrIGl4#1n|s60aG7C^WJAsR4@FR9&ARhN(dKz4N@hX>M`PosjGxuJ}-!G z+hj3d`T%vMo^9<&% zS0zmno@U=OH{*Y=_HQr=`O?udz#8_fCGEu-w12vmaT&^Irhc6Vvv{@YWmcqlKI~fE zfn985pxItPVOjth?0Uw3W?~g>BKGSQtf38tVgv~zBfSb9i&N)b^T2p}M_4_>xbR|{ zNl-104aS$iTlna$0sM=BeU9?O(ywV(HK;3jqHa>CboAFd)zX^SVd_4}A0Vr!i`WZS zut!p4zLbv=n%d+Iw3b38Ugs-Lu3Xds^$gJZZPExhx|cN2(+b0++0rikvH#+>XeIB> zE5dFbe-B)2-Bgd`!=aYd@zEErx9d`!?ml@kv}F}ywKWP;knJ>XC za$x?&V|iRy|6%O?)*Z|5KLSMZ_~eW0(O3F!y%^vJI()Y;QwCkS;Z<%tS5R&h7$L~@~H;Z=MQ zYjWqHsSQ?{vi2f|yG&qFukheh%I6-Q>cQ*q_0UDDsa+e#s%xl4dA*B&u4xYkM$;xr z0T*F`w30skt>$!ABtRQ+5muTBt^5gNVhA5>kM^LA;a1FJ_n1K})@h)SJ+h?0@GQsf=Aj@SE1;Vy@#KdPAx+Gx!E-Bhwd+N->qNE9ml$DhrI$Z;w~mBK zg&G)UNsnWZi4ucM>1W%=qLUT(%J0>9oQO?V8>xQ!-R9+h_us~G-hY|&NIB38p1(6Y z!rAiT;aL7{1uqAswv~a(1-TI}0VLCKK7#Ul>Ybt8o@oz zRsH+>;`C^c-X7&G&MMCnM$3uf3#Sun?zt7`|#=F;^~LFMJ0*) ze$L?C>UI!j2#r(vCDAnQEGp43Wg9HnGFMhvZHz_$-dHKs=wgn zebt-Y(nM{p(558m@~u&}=LxZ+;ss3&Mw#bu!Q0Xg-T{q7OPf+h@D_$EL&EnjL*(YC zL4WAR^*aB^ef=VuSY~U6{!^+tAHv^vg=QR>!oVru6nE#$??*4|UC1p|=80cwM6`-` zs@z(-Ze9bS8o({Tksq8i_0?@%kjS6{>G962oiiR&ImXGr7?nS3#;~riOVO$xS40Kh z=|Dah6VfX5QatPr=9D#gEB;yKNcGE>-L?`@ib zZJ8C97s0aysRv6HhZgBMLh~|pq!*Y9;8wxV_Fhr_>X6RO6)Q8>N&>6a0 z3TEsyUv=+LpWot-k%)>3xemHAhrhI$khXm+#G+Dti{2}ejQ214q4Z_0+&i@~ ze_z6!^pRG|f1uA{xblX5rGTxgcW^Ey19? zD7mJ0vG8{VIFa#0-^swMf84brXmHH%V z@}lZX{{DKA?4K>uXM`^T-I~e#`WRky*~wDr^m2T>s9ABx-J%XHo^Nf#x_#q`9gz^; z_h-G*M(?J)u<6wsYJ(y)huPSVmzoSN)~33IyHVk?&9wo~wtg3iO??tbtCCbKWSs#r zIOuut{-PV?+?+=+3-qe;-CY}0bB+1tpe6LoAELKfHQVXi(y0DJ^r2?1+2`<8=1jh! z+g*H$$=rXm~ z{|fEt8W}P_Q&@sIo0%$4+=}PLw@8{D(cHK<$>gsk=<(KjKpW51e6PZ}&Fo90xENQG z*l&KekA=mu_J6ioUTugL^*)qxlZu1XyB!!>B0-;)-;Kzk?^u8L2p8;scPyP!BxucH9DD zgtvqg?Wk*4L@AoQ_q?mo-}^k{mJ+v%fQ;)QL?#JMcOR?N#W>evqkI^t-l-$dlP`mS zzC%1Co)BBQZW|!Kt~-2T?npbkT^I3(NmVd{BjUvf_x3G_c&0XYk({=QiMm<@eyT0u z0Y)V!0I{v(EWw&`NiFXjytSLbz|jbl_lhD%(CF~1TJR21eJADniE0AK(TqF88gvMWK z$gL;P$ft$z9Byg_ANDt8jrerD`W@79J_6 zTQF)#qu#^4AZ&3nr2&J%wJ&9IUyP_*(laX|S8(e<&NBHV*7`vw~8Yrxm~s1Oh9d_G6TCz)h_Twc72Pw28|g{IVQ+=$s8~_G z5}vxnfy|Kbc_wU@ZeGsIC~B4vqv}{~K5cuCKj=Y5n#&Wi!yj${i2p030866H9s=ne z)80m1_Ih*lDK^=^j0Cy5gWRkm+^TsLJb{j$(a)-Z53P$7zD}dV`aIZaWQ=>SBxTB* z={!{EMV|9<%UxlF^7C-gGyTv(vG<|U1_Swcve$HY2a5Q5{#EBi{4GSDhQZtJ-3fvC zYX|`$o#prv4s^mK|AS>lP^@QNv{FKm&?^BE6X$gz&4S}QQ2~1Gur#p&=Ih&W-}40| z2!>*{0+O6#=KTUvymuw{mlf75r2YzY69sM}R>oKbki07&AcD7~iig+*rSBBqzEv#i znXMoF=*|tHbmF_GLh4uj^*w1_%1Oi<93|R8 z`rk!MbRHOpB_SBA!&!_>GKradl<=Dp5vqI^Ga<&KX&ND+LlON}P<_qGk3u~kHAK^l zXGsa{j z)T#hegi+WPGDj9|l1HqN`TCnn?b1g-^u_KJh$fi)Dlzd1q&tTs-{B_R7zZYeTvDmx z3U+B2q{(*LEhhFoYn%7Gxo3eF2}CM?(OMO zjH+cm>kw@YldC*tPq#I-oL$e6@Hd|bTvNITUX37^;|RAlPFp1co8?b z3T^Qzl3!W!9ec}tVg&NOt9J*B25)&N!8pGy;_AmRC3Nrcll2gN7gvJ~Q5Pn!dzsGC z#v&F#o>eu@V^||`2Ncx>YjqwClqh2{rP5Ju~T%J^_o8|mMZ?27q+{p`PMZzh1kx%E< zg}$hqx$Xf(uRaE9+g4E2n1X}!G26fq6``~)79)$wB00Q<1@D!7>NB%atak5Bc*Px@ zW|)DnzwNk$_q=RL#y2wXXIRACIIw03d)uXoA0Wo>$4~|}J!pC>V-_Jx4p*(-VqF8u z&ZfxtMex&m+(c}2a*Nd|;qxGjXJ|Tqjvf`Y3|5gs3gvF;{A1w~hCBE|HJ22X%`n6| z;L2`-oKV;mM`lD!f`jE|$*9KF{s^SrJp?-SdH9prgrut>MtI7l$A=8}A2i}!RU-N9 z=2^2qkBf1OEui{xqIh?&9HaTIz}{{THN@Vc!`mHPqSHx(Z8Ob zQM-K1+)me|p&l3#eAP27>pk&yhtT=PYvW-2I!l(RYMRbzgCZ|aq#j_aPH zkL279ZGmWk`LexKg~xL(lKvHvUk4-~Jgz~jlrAhjGtH~{x-a?N7Bjgl*|q&lZi1*>ms2Ir;?~<`%l*&kz05A^slP$r!Y@5nMt4_&`W*kLEgRRa z6-eP~fQoHu9NdnEdRrRp+bF%}}Y2lL+nxixaDuuL4LRsRG zSGJ`-cg?A-aE-o6W-63 zeZ^x}Zhu!POA4SKnbNI|oWC&c&SwOiI`ewS!v zS#_wa6V8ACSI={=O%5rAc}06J8!b+GiABYd3{B#t?G^k&fBFMYxjL5jT8fi6m^V=6 zqxaS7F{r(d>SQ;q-hlF?I_N7=$_y=92UJ3b)4GHyReSBU^l#6j|0NW3E>uP?d!N=R z-v$oUi8rOIuRR|vP!O?Ce1&i2DfE7lVbA^m`rj?VWEzzOAECT{_#1RhEsono=awUS zEfLA_!k42lLPa(V;?lc~2=nx;he-4azVYG76!}B;;XSC6Vyv^!Q2+aQMocR;)}4mQ z^lg_#cH=X>SRr+4J3YZ`FM7f>=x)kDChNE#`YNjc;a_@{T%?8Tdf9U|>^yxyd_b;i zWOtVa{z6~5ti|PJq(o;8FT>TdxR!O-k>crxj2aJc4N+b=3o}KJ<|_ zeR2B{KsZ5Dr?nSeuGTiC26?HagzlmBqvlwk&}VMy7t3xEe3d_g`OnbpDiwVa=( z4j=H&Ow-Dy-G91V2uRPbEu!i76L=FYh!O_PS>;`Y*aDT#iL!hArv@fRva=(uOmER-osh1Vp?8$6mo z8GKhr7KVoHpp=51CI7dV47@y-hn`~vpQUh}-xdf=y_DFMR@}U!nEw9&WxrhLp1oplgddd)uos-qc4_u$fhDb$XK@KKt;mI zY$8MNxcQscqL<5gE{2XKXv~?mUsxTlEDY?L#DDx}g%f!JH`pi3!ah zPGQmmGgyI();fLOI<8Zxu9FE>){d`}E0`!C z_?TEIIcISRkoibznfAJQT03K6|9iHhsK#gly=#JL)w?EuNvMVmf@Okh>Onl>O|lNo zbY>Bq@p*L0^zb04KBz^xyZg0swdck=I?j8&-+R4-Nw*VI!Q&~o+rz|}2=-V{`>d%I z=>n@lI;w){HNYjrN4!Z-JjHwCwvP#~duPYPW7sea6uj!QsgN7YK{W9Gr@ z0@5-1%Rdu}&wPs3yqMTSAc?Ce*$q_&Jaw7E$VetConR;|J-TN@@0d@u>*pT;g4H*D zy^f+hL4DL)x}l!}uFBDG=4n=LL0kZ-kCl67W(_p_^E3cM){xH>L`Joo;x9hDH-2^B3G1K+HXKti zMJ)f!gEw4N#1<38lBqGJgEyp*ek$haqrSwe{(XjdoQw=Q{6a#Qe(-nf@Dsn|ql<*t z5id2rI7K=1Z?^Tnx%+0nV;cYVPh+5azwUv5_`|s!%RKqVqWPacF~<4&8@T&Z`S{0w z(6#^k?_&MiKf&QYKL4sW2 z30z5&CsC$Uxsqi|moH()lsS`TO`A7y=G3{9XHTC$fd&;il;}-Q8j&Vdx|C^Cr%$0q zm1>b=NUI-57S+0y|7%yTU%`eIJCt_S}x6KVo{^ns;yCzkvrQ`nq;;~F`9jDQiyC7En8I3bISQp)X)^e7f3oy=0p zEh~d^$}hpBtx8q`Va*lNF`Em|)?a}&5LaD^)e}))gH2Z1^M)NIJt2(4q4=vIQH1&+CUyz z<&{e+`Q(@PtoT8eX{Om_m~rMT<(heZ_~x8}K5XZniQf0;pplMi=%Sf^_voadW^3uD zspj_SsIfj&=8$Hs+H0q?4*NJ>o%`DCZm$+wZKRGp+wE!7UR&-JTO@7ZxA9gr?z#Dn zXzIKHzjW`u31{f{R+Z8B1Bl@|3l- zGhr z5#99WH^CWBagNiQcsV9AcLh4Ecw(8(bmu$mNKJXplb-eT<2Lb`Pkru_oc;9YKLHw0 zfSwbbhSDTF5t`63+LNITb?7wr8BvK&)RzLa=tVK=Pl6VdPzP1$M?nh3hmMq_B^4z_ zQJPYGW|XBZ{pUtGnkSEjl%_QuqDgU@Q=Rrvr9JiOFkKqdmclfqZmOV7|B;$hC+?J~ zP2K5Fp&C_H3YDraEvo4-r&O(ORftaY>Q^5sRk8ZCs%7;iQLh@NnR1n_AqDGO;VRFu z&K0F*4JumG`X$u0m9K1#>t6wTOS%qLqIR_?Uh_(^y0PL)eRZr~0~=Y%hLW(A&8K1e ziP*#zX|0d-EKMgHTG2YvvZc)?X1}S~%?_!vpLMNUM;lw&>d~~dwWeKFyILHz_O-nQ z>1=@;Tr=91xWN=^Zgs07-ujlgzZLFtWm{b7_A;Z$P42oDJCf#Zm#@(E?r5hQUPT1d zy4T&%a=Dw{kA9cElNE1x$5~$UW(ZM_ux~rnn_sQAm%o60?{v>P|KHoG(<8dM?}6>B zUj-wzzYTute~T+$0WV0I2(~bO7o1^*I+(*=g|KBMJmCb@_rfK{u!d3W&JM4bsUIF| zh(*jF5}TODDYo%DS)5~>y4a^MjhnHQXWs#$3I>efyu2%U|XW=EfXnGMkyscue!1<+^5H zw%N_v39_64U1vep5zmE|rk)kS=RRWxb%1uXg$EsJFdCZD)#S0F7mXYlKAO{zmh`6= zT4_<6^1qnQv~m92X;l*$)U7V!s9{}7>6-e~D1-7yR-J24|GS#kua5PvDO*}v*LpUo z*7dP_o$Q+eo7s~b?68ULpIb}$*wx;(vU}ZZZAVh7(2h1}jBRakvl`o=-uAfz+2e1+ zZ`$K_cZtibY;@22o=jf%%fkKccgI`a^!7Jx^Xct->kP^U*7w2T&F^ggo8fBAroacT zmsoB>zvm-pSRC{-XJJ{Na#(UGSOXa^kySH>286u)3H8es7GDu zqNaM)KbKu}p`qH0%L#uCnzqNk*<%fOucmDl)hF|;-5x)7ikN!xj zpPKA%e?#66fAhz+s_T(@CxHl4tb6a#V>vC z@DEij5A`tP_|W<$4iFLX(FBpsG_DYp&khsu4i}LT{ml`94-zBs4=0flx6Kl54-+#H z5I2z%f6WtD4-`WY5l4{}E6o%^&jC|07ISSC%gpO?@Rc%f>|}8lJB=1=5zk!F@=%c% zL(vz1G0TFH@!XCWnQ_&O@fZQm6Qpq(|EaMWt??SMF&njU8@aI?z405tF&xEl9Lcd9 z&G8)3F&))$9nCTCn(-Y2@fojd#XJEOZZT3&VHn|27UdBd*~=3&;T|DH6ZCN(d9feo z@w)(0ARmPw2hboPvKJB3%ARW>pW+vOfg#<)A;m5tJyO>uG9hEjBAEgk#NYumKn$=! zBiDl?>ju|8GA5ldB#mseNOCDO-~n{<0a}0_Sdu$jvPEE0CW%raX|l*tFCd#DCwEc< z?BOT3BPioU7mKng1MMhlGOlh?DVS0xof0|Joo0vLP)G(Jcw{D&g|RkWVQaLK76TA?9HdG(ZD3 zp&ep_9Q2YeAHpvIQ!pR0Fg^1J57WkSYAzFkF(JYp>|ql&0W(3vGWRkf>ftW~GXT}{ zJE-zAaTEGN6ER7OG!eoyAL1M`VF-pG6S(0a@?jVt0TDg{7oM^#M^SBCSU_j00bz&6!f7O|3Xv+Fn|;00Yyc00z?2A^g$lHfCK~rA3z~S^8pk9;6*K9 z4t(++K=ef?Uca(;lD!1#EyGXfy@(K_37$1F``gK*37)fktDrMJ=EV#8Vsek}?g# z9>`$@vfw+P0R(Jd9PCpd`0^P{U{$4b2-;yDXmtqe!7sbiG9%Sb`|M1ug6Y=uSZAfcAyCI^jeGH26jLp+@T2ob06lRP)%VU#8n`6v_vZvM>7By zX!KRLGe?mDP@6O##5FdXR3BWEH3woIpkW1o)KWoVOk;C1WwjmLp$2TgA8z$2<-uF= zRaM0kAL>8^vO!qgj95cL1Ce!OK~q^#44Rr%TBB83sWk#3z*_V420j1+w)IF|AYt_( zTra>xi@{LewM)NKTpQL#Ep=kE;Zl(STx-@8CUrLR6(LeJ3o4afDS#6OHZbKu4w7^O zTvcIfU<-oO3sQg{o?!w)lxji1N-g%wFxD|R_G~-WV>1kw;Its*G$D4tWFz1MH1q>H z00LN6A*9p>{{})__hD#nbW?GmPtTQTwbmzbG)C1mXzgKWX%tF1Xkn37RVy`In>IF~ zv>xh!0t8_it^r{c)&-2zN&j*jXjKh(!538b7iN>#$`(n|6m4bK;?#CoOQ~%M;%!Uz z159>h@fL4e79oUmOmo&y0XJrM0Rs#{Ap9~$n>QQw0Us(sN8wc;8n;Ao6kYN4GW9iP z`}JQX)=KTOTZ5H+_hDgsRW=tjQa$cXfp;Mu z6$6rB2_|4>^`Tq^qFXN@6QThWEFcT$fnF^D3Y;Mtd=_Yh7EvEH15{xeNR?NS76f8I z6{5is|6G+Gq;~*h;U2tzgT)~nn!pCm_iD3tYjJdfEBFl(qS58nJ-auU*AI? z|KPzIr~!H-xicsEnrXL^HLQ_}SRv}NDb?W@_E(C5R~6==D|ymA?LipQpcP)h4^CJi z#`Yql!4_w9V6FOjJIbaifk{-~xC(-j1P#{T1KnyC@Es6mIHXuYNAW3cDdjtCZ1{!{Q5DPlZ zrXN9}b$X|9nxPHTq3d@DF?k}sStmUJmjwbG)WAUdG-f@(E&7 zVp^spdGC7K(Y_kc!kVYa`k=2FsD~JhA{rrF;hQD1Qs1LP>0uMTxdmJSDr;k=|KBjI z<&3Z6%&-5t#LBv?DH*M|xs28tA?864{y-fZLLB};5at1|^V$!u(4cQRvpEg0$BeV> zYp}^W77ZK45?dp-nz9w6hqLbqJ$uS9d$Tngwq<+BK%1u_aI_Vyv{73_^x6ks`_Wq4 zwPPE&ha0RryS8;2?r=N3bbGfkq_<_zw#6*Cg^aqZTh*+a#$9X)$d)Qbr9J@6f!P2|BJG{s@yu?L3 z#N&I!Nqo+z=*SNm#lc9$SNt3Vp#vD80UCe-Iv^9^VH|pV%*njW&HT*KJk1rnhc)&A zhn&cje92u~%CFnW1KZB0JI<@Dim-r=q}-s<+R7{38+Kp;7+}k}oXd9r8`Qkf9sSWE zJ<=r|z%ktX-u%Fj{K=WT$*23sKOE27d(|%ezJhy-`n--#U5x%b)mKr_Cwv{^_Rtf3 z)^}jWC;irOJ=b-8*Au*lVNm}v{mwyMxb-JN(n%z1`1mh@$R9@;4 zeb%Es8Q8(r3xD-lzxCI=t8)$Pc^>k`p79%>_TL-#EiCaBAMGvv+Unlu;~tEPUidQ~ z?vFo;J|FZsVDI_9^#8sC#9_e0G7tD*6P6(?U4a|Up&g_F6_#Nf(xE)>Ko!K?_0d25 zdHwL=j`w*#rx$;||2+2NU+u}>{yE+3jNjQS--`eOi=%-A3mQCFP$0sE3>_(K2yx)U zh!7`Iyx7o_rgj`Vdi)47WIHtrA}lzGk^xFJ<3yqZ|1|-IiVL{Xfy#hup1NWbIISxN zV#5g#6Kr4;i8QIwrA(VTeF` z#o{)uS-5iJ(yhz(F5b0z_oju5C9vSZgbUN{>-Q|;#E%v;cAOaUUdd=HVzj)_ux7)T z4O;A+5H!Niq8*k#Ncy10tR0m>D0$Lk?2^Ccu?tDg#Sn4r#GQ+G6WO|*7&y%Af85$Ntah)M$TI%^7`$T2b;*|(uNeJY^^G^X5B!#WWBKOg)^~f~ zZ06IR`G-dRo_qeK1{#5#8F(3jj;!X~AG4tp|C<{5v=mY~HN=2SN5Ex(%s4ys#13+t zjKdBzCS+kWx?Bv zcHW6+o_ekrql{sVb)$_$M&_fCg`x+fT8aK8UX+IEcxa>QIk}{Q7+FdYrV$C`C})%o zNTq=cHaE^Du$`pR0-M;Dlsf2~f&vDS*o0F%A94~RbGZ$%&Yr&h3T&{#UdNT6f7TeN zq&jvgSfh)^ha{qrDr=};%no*5w1|ev|E-mkhI*}FsCA3bsLf6D+5!wT(11$GEHxZE z7TRDF1*WjWrg0*+lv^1T4hwL=0uS6=pT>FxXtLnJr|n$fPW#=n=|LP`!wcgTF2fiL zTGqGTdJHnC{AC;%L#dg|8aU~)+inEbC>72o&QJpl3O49da}26PvrA9o>?`LAyb?@w z(MBIlY<35;lCZ*r{wVQ!4quBS#}-FSamCeMJ@vy_pLLq1o01#y$W&IDpra^frA`Y7 zaPU9}aJMjFQZ-M2H-_xQGYbd+05CyM^E{KBoG$o_bmEFHzNf(l>w&D(_C;NF*4C2D zsO6Jcy>;i(UX6KL1)eQ7W@VdA|2FCct&NpBNF4Bh1QA5QK?~|El}g2vV zJM5^V4md3hXFTJ|FV8%6jvM<)gl9p(HC*dPC*!m%8ew zmu3;`SFzJW2_Xy^Y^2*D`&Ngg=O7^j70>_zG*E#VsDpn3Y@rKZ=)7VrjTJ_a-T^0O z!3s)nf;nu@1H;!t2vX2K5tNJh29z<=t?z>#^pE?j0>2OppaK;*KniaM!z^lXi&Y_@ zjACKK8!Av^JN#M@@l(XN|77fp)MMlJ+;~AHee5C`yi^C9C>nu0u`2AqgZ!vifdNp0 zgl(8b7|G0K5?bQ<&m7ffvO4DRj-lEkRo3B+ z9)zF(6)1oT35m#9%Cd!u)D9yLI3(*$(t*3|-X?JfwI5b)h(aW0ThO*dCVtW(C(0~%Gn_>xLpcIHnHHs6R>1?Mj9s1C7+GU;J>t*-iIXZZX z42elBR|umsk9Pc1|DYr-DX<9om0KQkjAOKZ zc`3r+)1*W#DgaT6FoVVvp_HTO8<%<`rbczD5yYuf*CJ16`c$JlEsaL>0oJgJb*yA9 zt69&A*0id1t!!wEBbyZ{HNSN$|Xhki0Sg6v+ zsW8Q=9e-+1^l3FgPSk{6FN@jCYId`n?W|`%3);@|wL1z6EYzr4J;v5_weB=*VqFWw z%W02}kgct4C(DqphIY5S?X7Qr3*6ue7p;7y(MnMpQ<>USqE0ofViDUw!J=`Z;p!=6 zZ@bKk=ytQe|Mxd>HmdXFhtiF7ow z`L*tKpJ`dmVnYlM(0~}QQQibAc)<*AFk8>-SNFnJ!pqI+eP3%m_)@sS7sjxKwe}qi zf~&gy_3vXlvt7(;-~lXtKnv*M;26tz#x(XVgoVUZ=T2CM^gSqkRNLAe6A`)PtmjXG z8q|)S7`GtY?222g0ejSV%2ckhmEn40XGMv}LN08Vr`ujGpOdlmEwhh_j2Dv+f%YcOYfzK&ir z*Vb6=Loe6O}czI-@)Z;4L$r$=-|j!X<;2$|qUTk|oDJRrgq{Tv1)^6&>d zgB2aa2*e>WQI9r=z$>m8M?R!6ibLE17x4ybJXXPnl<&hH`Pjt;OcB<1i~MWnLrA-0poHs zVFE+JfFZz&Z+kqWOe&Z_+r#j9qjsTq;@$fFH7(FeZW(S*zA;~7Fzz5oBeR;L=XC?8|8->N zYOjWSv{!pZf>Vv<7HJo8JB3lgrw^2%fg0Fr8>oS1wF5=4d?Q!{H?RX}B@g*W56~A@ z?Z9~TW_T&^eC|hY_O=htU;^n!5BN|A9FiM0;A}XU1@_l$$YBWdU=QDy0xJlBP3VMR zC4jeQa93A$AVxvgW`($ydttYL8&-wOLRd2gcAuXD1AqVgL5#0yTWXqwhI98cpHE> zB9IL;_=K65dQpgNfR!u`cz{`mBtQm=1!#b#D1lShXah)M6=;FwhJk&?|8{LSJ0DPY zAJ731Fo$S0FJdKq7@!8pAPsHc0rU`qfmnlJb%U}3R*8rULg;MdU^DyBGq^wxyMO_L z;0xR63(G)=nh1_i$BC)dfTd`AQpkI-)+q&-jsVw=45*5$m?TQlW}f7Vz}I9CHj8D& zB_B|Wx(JYTXjb26iO6V)g;#^$M*{K2g9llN==Xf{7J2jHbjM)xG4)_>S`0fi2AJr$EoCT}qrpstX^#tnlf$=4c|Bq9O;YOfDJHqeuj_^&R}m~1!Dn<|9Q|y58~i-D_EcRIh+r= zXT?PpW)S_m(r4wK+2(^XoX`)hS+(A-`Q3# zkZBq)4l9@m9UzAsunE~mqc5ge&;XG;+M{Q>Z=9GtP0ET1n4u^6rVVA1aGFUU+In*; zr&nX56nIA|idHMiVjh5fURl192!=T?RT;jDS(sM<=eA4zK6imts{sU50i(Yda>YOVl# zr23k#kQ9M*dPaQ8tfCRG_3E$>+jh8mFuLlkpE$1kx?@5bu*6ER1dk0!iuvw8?MK?vz!{ULt3mPJGBVt|F2*pV($7=8nF*yE4E`xwqf`cZ;`qtG9DIsmoGYI19Lv6S&{%LtFc_gG;XEnzbRC zu1dD2SF*Q}E4h$xVWvDSIDKI^oC>og4)llpZn zC3YHMyScMVyR~b(w~M>EtGj9YqlJM$tE;+%o2i<5x~QvATZ^&9E3-3uxT{+!+zGk6 zE4|Z8z13^I*E_r8D78GRPQdHE!rQaP`(E2?y5YOL!V0?odNJ>6hTAzLvWva(E5Gwg zzx7MKojX{@x@8XYy@|`TO^d$Go1ElZ|FZ(iy#^e*W+bJRWxoyVzz+<;5e&EZE0V~| zxS~6{0UW&GYf}uj!T$TX0?f6)r(Ihi!6%HuDXhZP+qohvc5=zV7@WbwOT6Z5wa~f0 zHhjY}T)rGe!j)#iD=frAOvFW;x5fFp{ku{GT*ErNwEvsF30%Mo#a}G85<0_cs>E7*p;zq0PaMM2Dp}3kXYcU+h z8v4Jbi?E*ywe5;xK}^Z3%*w4C|G9A~xQE-vhep7cY|DyV#+~fRSX;O-49ZhH!X}%_ zu1w6uY|L+)WfW@4x4g(A>&t1}!_X|phn&f~{9_{<&F31WmjTJg?9Ja?%-o8}pA5pw zj7x2r!_y4R)_lI`JI=hk&b^$@x-2?J#LcP<&iSm*l64c}o4VFf_n^=;quz1{h( z-`K6*N^RHa-QUO^|K8%g*TtP^GhN_%o#5qd-f$h?53bn7dEXOG;T2xr`;FoJP1v9F z-Kd?~oDJSl{o!Kt!e{K^g+1U4?qLoN;VmB5Vk{Q~PyjMc<1=pKGH&4&p5gj^06pFa z0j_(&P0s?}+QJRuM{coYZQvU|)}G7aE)L~DJ<-HVKUGdYIgaJ^O#nNN;l}+v9WLT1 zUgRSV*n!ztf;atAoW{%GPo#p^O78!g*bL%~o=%6J=%sGV zQ?AT?t`=5)|K*P!>5@L_+|B2}ZQ^@A>%uMSxt{9@1m@=5b);_U!OqI!tm&RkYp?$5 zu`cVJe$AI&=*;fxs&4GX{_I>f&?jCkz%K0D9?ADg?a|(C$R6p+zTKhz+8W(R=AQ2B zuI3!Qb=N*Y+Rp9s4#q7kMz;RJtbXG*zVA4m<+0x5+I`T2UdRGq@CT3Z36Jef4%vYI z=MIl;@V*}MPVW_O#4Fw1c#i9f!s`Am?)OdZ8ouzY#>9z|@F$P*2(R&}t?TMe;?9hs zRc-MzFVVcM9aLA_f<5vOUlt!v=OK>(0!{E{k@7`<@+&Xqfu8ga&-0H!@iY(hCp?bq z-tO*h|4jVu>i6yO{|@xOy>)4PY$5hxPxejU@^LTs>Y?^*Z}{_D*MkD&ey;ROf8svR^?J|NYti>cZ{pAn z?sFgNx-RO4fB2toz0>9xN^bT`KKYKn^Kk+Bk+1BDQTauW!~Y%lg8%ntpY}2j`nli0 z$-HovZ}lwC`J2BOH(urYZv6DU^%PF-JzfB>U-Un2ZKRL+nUCwiKRuqW``KT~ZeLKT z{(8x+_?b@qz%TsjQTN5~^Yqi>&X4i|APJ5T!`oZ_wEz3)i~ZV<|C_t$1nbN54-k(8 z{|+Qr(BMIYSP(8`*id0Zh!Gz>lvwfL!vYyKZsbV7BLM*+MUEs{G9&_&DOFlb$TA`p zm@#F}r09}iO`8>Q=6uPMs6xXSdArHwk#%3K_voJTaf14m~AJ@g~*ofK)ZJZ=Do|;prel_1qiB-2Z?74Eo5U<_a{>Iq0D4DL;kin<*#$T#C@5 z2@%Q+Jq$C{a6=9|1W%4*NH3Uy-eA_rljFu%F>V=+PiWBf0u18X!eK?SpuFh!h7 zT+l_mF7$9nB8xQgNDV;@ajX$p^YKUAD&+CWx?Z%BCmFK@&c1>OJTAE}GYZqjjc|Mp zLK3I!>q@$WMDk5IZE^`|2P%p+HJ^=-?&F|>cbW=_{O>!TxmLy9L_vlh_(mgMIsZzJ}j54S}Wh_*;|BuMD6~RUo ztE@3KNxaKVPlFY9SYo%bGsI6yeT~HyRejaT6rFU{rC4K}HPwc0)wY0K5pzh-DdBq( z*ka2y_gr(2T@TM?r@hu!cPC=C%6KcP7QlDmz3a;`y+lsYjTUXLTV3;AHCp}JA{SkT z8+Q0jbx|`y+3xDi(_+0!?A2Misx??zR0l#d(L|Rcvs>v>a>S-T7u&d@g&&rAW}3^3 z__SCkMib*TdG2|?mv;_XUyg}(X~1o3B)H&~EltS5m}{1LYO24w*(`SBW47tbhBo@y zwdR%7Ymmt<8#giqzIEx8e)cKqs^gY>Zl|smQRknw*4yV@&Gr*v|I4`Uy6BGsmk4RZ z0XBJYj@U*C%ADY)dveO-wwtW>0yzW`MHu->@Qe2L`{L0vMiKDH`0ktV!fhJ4R)0%o zl;D(Cwn_5JbJyMB%bE0KjWyU{1CBW2u)~f#_y|OE&OOgfbk+@5eRXVE?i2LeN;iFa zKf6Ra=>i>(lpx%9H~)NLduOFRJBo)xc;L@4!;CY47oK?IvEV~_<~whFd+oWcdSC;Z zgG|M&W<`&50W?T+1O}#UJ*Rx=qhJN&q&_?`LJ^2K#Pax%hvT^qeprwn{eoA#;~g(~ z{M(%W2*L;$!q900Bp?mHc0(MxMQqt)V7s^%6zn}>Wd9-}{{)o-!3kPWiA(Gd20alQ zf;fT^9K?bL^N_sd9ioGQ_+SdX=tU5U5QWHN;UTafLj^7{jRLgc|2}8HHL@{>XKSNC zUgwb{GEt9vB#-*gG9UUtM2Z~TVi(&Nhxf4|eqgBI8n7_DF=A13n`4CSfQY~#whDT2 zq+Y6mmbXpTQH`QpNFMokN>qMEeX3cJ%iwqr4RX+fUBp81gr~?eoPm+{gQW4um_Viwbl@`Pu`mPyZJJ~Nptk!Cgh|M|~4T@#z@(`IWjDX(sR6PSvS z;s(Ka&g0b*4#49j{!~~-bqdo&p{yY%JK0f?BD9;SLuSQ(*iV3_RHa98BE2}O!$2~m zp=PvV2oVXtHI%TC_lqP(->J!$&hvnnBx+D~QmKPJucRiG2ufG_)PJ^gnDIPVO#h-m zE!OmqiHzU;cG}B}1_BFO%qUfdx>T(hwVu!#sWUzL(Nbm#s&l0)Qq)JPsfN@(_Z(za zxoA#t_A(DxfM{6L2~J>^6>?^Ut5DC1R+iOehlA56Qz7Eix@MM%>PyyS5ST!ZqHd6g zAniII(bqzn^NWaF!x&5$Jg}mZtTIfhQXwm$|IgNznUy`ARd0*irpi^b!%eAFdC1$e z_T`=hL9JHB+11zbl(4dGq4NO9CMjw(pU4&3Z{OI~01pdqI1?g?G*iLNH5;BwXc07{^D@O z{biBBeFHFzXMAQfgP{v&9>W;UT;n(>{|^xg*6orR+gQi$>XS`o$as&t&UV(T zF}FL}7g{;BA;bb#>l@u(emTYc0YqR`GX56GwD~d}v>cm%Z#Yz`+iI7s24S^c+!UHYB~% z6sI>{+WSI>uHBH&t*_XnJ}$~{T@Z4M_>tq(|79*=n`2*6w|NaNKJ@iIwB#>e{^4mp$3g#m=i@)0eHAf` zaVo!03pI;dHOT@mMqnphldl{jo^%5v{-Lx3Tf4EMB<^!R%PKlc8;JOuKMo9y`cpmo zOFa-IJO&EC_;NdEJA@2;kCFN&(sH4OlBK#cF)ec>8^f=38omqcJoba04eUT5-`0 zCxVP5%)2yHLoj5+@M^!O`956By&2r9|0};=>!|^&Hv$yF5&Ri3Btt>OiVqYiD8f58 zd_xS(DDJi)-CC`d9fxJ#^`8m$+sG(|MRjS9p;9L1?PFCt8}>-a)P z^h2Uk#qNT`V-g!FTsvW-!~Fpvhl8kH!#q6{Hcv#vM+7cWBt>GZ8-c>63{#^aGdx#( z#b#v2XN1OQy1mwz#X7XZUehH=3qGGBw$HjaD;dUO9LJ<6L}#q3NbIpwTSr*bKX(j4 z3xvdU+%L(aL|)5A`?|d1`LaxOrwW9}_Yg;N9LRkzM`<(_Ic&sLWX0BdM-l|Zp13`0 zw8uosMl7f?2K*wst1g5nur35g1`$Yt97luf!M$0=bA(8eQ^}iS$7u9CRQx_EOfACl zr4)-hxqHAx|J%qh8?>EkwUG=;uP8~9a!Ei+#iBGymUP0GgvoXUNO42TVv8Z}o5{(` zGS-rxU$ZDIn7c8v4UrVeQ6$QiY)V6f3y@UEPh3PsRLhif%kM(UhA_e1<30HyAzq5D zxeKPC2uraPM6ztl`(vwle8{B~%)xX>!-UI&w7rBJ2)g{BTC&S4G(UZ`$uP^oe`K}4 z?98D&Mo~e%#$-&V>@75$wx@*3{8P+7T+GBgO1aF9T$@4##3{kb$^7z5&jiA*Tf<-+ z&8RF)pG-^AEKRmVOonX8zxlq|q($aYv{@>#FVfB3N-ZO{y5Q17IT38m1# z6HxIK&kK#mOPbHOJWS-oP~X&0lLSkkG*KKaiXk-44qeIbOwemXP#=X+@O)4BBu*Nw zQPwEY9PK$XRMH<^Fe5$EKN%?%P0=Go(j+a=A7#=e{W!9`(kxw37RAmd{a`Ak&! z|5Q;jRns<&96=q_-84u?ebhzWR8D0)J|npMAcI> zmC+d`)ghG&I6YNBD?~_DRi)(9K7~aW#nWCT(ioN1J~LHZ70M6X)mC*?Djm~EbyK4| zO)p$kXSG&r4H9Hc){(3~W^LAN?NK@fRb~2A5w%labx+B-Rc^Jg=KEGf1=nFE)*{8$ zVBOPnt=4(%%WZ8}as)bfeO7thS6Z#rlDpSvmDW)`SD5Kne=Ww|)K_v{SYwUVGM!b1 zmB;>s)P$W>j}5Me1<>mp(20${a4lJjz1X$n6LVEqjLldJaoCU@OVGTNGelFB|9#PX zC0UeRS#cd#{=^QGJz1M$*O}Fml496W!4*u9^)I^orNGm0ir`*v#Er&b?gBJ>8{E zT-vSOsJh+tNXBb@RK5k?)h$-neO;GDTzzX?>1|l#?N;SQ4bdE1Abnn-|NYtVC10}* zUg3qyWgM5b-Coiw-MejFrA1xVmEW8d+u`+H{e2PfWn1?hNcg4R!ll}`nA_hqU5cGv z`~_cOeNR0--v3Rd0M=RaEkMf+)d4o##1&u--rxl$UMzs%2)?lHWsg&Y-snZn{!Lx` zh2844UJs^VX%pcQ-lGY|TJ%j{3+~VQZQ&P&VHeI|6wX*0uHnrpVXQgf1dd@DevMSz zUjz2t1IAz>9^!Z`Vk1_fh|S^ht>BpCVJ<%3FlOPf<=_r3vMi=q(-Rd|8si==W3ffp zH}>K=9^NTlVhqLNG^U^%7ULAoURhmXDIR14zGFPT-w%%2JqAo9|Gt|)24qBTVjz;+ zC@y3}j$a@CVIcm=MdsQ@&L#ymV+5vRguLK0j^wEo*h*ey$nE5}{bb(>Wl45pNdA}= z)?o&2;O153xs?w+c4b(DWmdo|RL0?4*5ynF;Zwn7T6S7qrrgI>WMJ;V$<<_0wqhop z<=CxSDZb=Q4&o}t=3ky>%IRZdPUdhXm|+hYdXxGHG&6z1=}W^B%99u;Wa zRW6+EW^evxhK6W@wrHv`XP%8_I`^9b zFg|On&cD6p>vP#+w7%-8_GoBkX*(uaIcDa4M$W}%>~a~|$L45%Hfpm*?8uhvx)$1w zHtm2$W6pNcw*Kt5ChWp?>)77tY);6(&Tao~ZIBbVhj!_ghH21F?bR-B<4zM(&TN1# zRo?FH*WS+#_VZjZ}qele= zhNtcBXc8~+3le7L?P*r6?+*v@AMbDvzwYr4?HZ?PPR8+EeQx0XHXl!M{uc5g4{haM zZq?rA6>s1hS8}dnaw{+I)23enuX2fAa0Vap_C|2SMoXaBaxOO#OpS3FKhWQn*vSTS z{od~=m-4v|aS3hTH?O8Rhw++DZqfd7H79cg|Ic$OM?OFg^l3`f7=Q8g7V|@2bTdcv zJ?C>Erye(lbPv(hNtg5Lc=9IC^fE{FR8MeB51CH~bq-lpQXh48u5&+Db&CGzu%>lF z@AXG`byzRGfntR}YIRo6b&y_hO850$=k@7k_Cl@+U=MZ-@oYQ4b3D%uLx*R%M)T~w zXH?YPw(ePhP-Kc6id0o(BuGYtA}5I|AUS7|oJDeyEFvH|=bQzE0!e}d6%dr1b55dk z`R;x0dH3#f?(ObzKlB*gpKFZ&7&X^iYyQ?VpS6~driX%NjeW`a2|!;w=Y+Nd@o;0G z+56CI<5c4!&z0T#?U9?u^ohFnxaYKo%X<$?l8YA$uY5}`j!4@G-(M)&j<8GnU{2|B zPoLT4oSDAx2${BEllF-ixU7O|M0=mxTwF%woCVihk|aI57`%*+^y2#A<$Q3?IdBnR znwrFZHF29}l%jc+V0-?2!DZ0K8-C#>o$i})(cRqTn@Dn1@ZvC4!0+|IP9}+C9+Tf= zi6eH2iwN(_$gyk4s2k(-b>PKC&~#kso9j;;kBVQM7mhimu3b00@M}@>+h+8yUm$$F zfAVb1OL4&*toO6g_Q+k@r!B|7qRzYI#ZPpkf8&Dhn-{)mWB!5pK0RYu?cNvOB{w-^ z(jTNfjZDu*H%10E=Y^ZUbzS_N81SlzR5z49{G4z#toO3}Hoy`87wORESMQrJ+{@Y) z;p(wx9R~qcxBb(RzbdtI?jMFJqJB#W~Ml*Otvn`9<*)_BMC56!(Lir!^6j`b6m#3Id7HM!x#W~cLJt}#WV=+|j zx%0K`@q4ddTtb!lk#tY<)hoR2%r!eNpyPI^Y{%N~8T)X(OsbrZ4)`f?L(rPH*h4@- zADHIDrB{thO`Sq4DyE)(W0!+edRJ}PNsWz5s^hDfhebk-Xqf6h;22YkjLuEd5&sCIlf^HQL4js!>oduc9XJ<|Bh83 z<6KosFk8i|d{uk@s)qNn_YiTL#rN(s;o2Kgwk%8<9waQ)-*qnUY5utXuH^R)ug5x0 zKhJx)QO6oIL9abW{;-CUVS@(jqxy!kwABSTMNr(^cS##E!Ch^*6MV(HA9Pd7lrxsRZl2c9De zx9`{Moyr)h8mCP^r!@F$9#;rIl(jTQ&eFC>^5mNJGPI~)@&YMJ6*Y{6=bn~SHGkF9 zn`-(VO3p~u1sN{cZ>Kzi9nDf(c8HEA@Ui(Uy&>0V`N8&(QGbQYYyM#2)x{6i>1USN z?b{4XYI2-$${^3Jfc3qTU5W*%^l!G2#A$04;1?%L70Ss`y9I`4ZMDg-;sd5)4gxI~ zOL0w~f3LDc`|eYxR$VTSkau>jbncwJJKLD!_8MM&WqfaOSr51KVnRvu{`Ey@Raf}# z4pGgQz}_*L5$^T&x3K@oiTA&@Ic^Ue$;SVApMe z5ox0?m{Y$eE0~`P_X{%tZW9Xmnm-)wk#lNwDM>DI5E1>voWOskm^zs|f?e{--lm>3 zEQBvW&Im&Ce6x?ol~kLt@dsIgQaMzFFOq{SpDJOpk3A$w=@Q-dxm5qdSvZ$TnLvK% zr&eiXDoLn$=E~bc?5MjLc=1Vc*>p{|n7dB}qE)Zp3FF*JysB$=}__F zyKk5bSes3@hBV4?Us4rVV;PnqjJEHl1JG|64}Ewo?vB4P+g^d^J>j%hT}!zg{%&J) zc!p=|O!PLtfi2eVLi#)HL+|9wRlUZcQE%QoEe0(117&dyH#GlU^o^OsS5A$RR`P7n zV}h<&)EHv+B)i#zH5}D86v&I8pDMGC;*M$H$Y7_!HD;7LNgp>K9?63}wi5n9qxCYM zf~`Y-Q|agQ1T$NSZ9R-r1xP!PvfQY3oU*K#2OmnGJblv*n~{3WF>(1rs6=&bL^V|6 zOG)Z+PJRKKoS`bJF4LpTfI;Qq^Xt!^ps%`S#_ad#w+5pa9|tKGZ0gQQysCEFEV^lQ zfP7Z@+BQsC!(~-r_|uO(md-XtBd+|BafV(&<7V{|&+g;34|?xqMOa;kZY%T$t{;Ci z@e+qyL!WT&JRVRKX?%vgYd3f~(pdhwF7CdQM|Fif7z28 zYkX3Wugm@BS3N(YRYkqFCxrW0L|&i1IEcb~ zi>ei>=DwZei@y8bv`F~ge{>b((Y{J~hp)!}NBvY|;mBT9V2p%*sjg1fNgR*hk&o%m zWXktf*vwA!+w)KIy*j`VC88gF7O_mlI*I0;AH03N)TtrX$274T`SqPCnS)O+$1z{% z8@M@?yCGMTyDsL;-$Hk4ruftILk<~2OX!MP!7XcL^t113m(_1Nv;Ca~ZjE^snH(vj zgoybf;{pu0=F*}h&+P*8V9V4Rv>zpSk7WmVY!0%|)HKeI!>D~su?akTY&6`Kbt>O& zeA@Wn)L-~}es=?_m$o~E%`N^yxK2{qcZ_pSF!$DJL$HNr^8P%{OM-k`1!|q=@qt2F z+EJ?-KLy^!bP5%3q&4)5!M~Q92qP$#w)J}Hp!KO|EDq13o_wU6YS+a==Eo4>ukZSWtDf%d8MxC!->rMrydc^& ztI3#Im%6Bni<`0?5T6fa6Dq7@-m||q(M&3S-d3}C@ZzWc)B@vWOK;V!qgToGcZgH^Q-c0^CWobyruWoo>pZ0P!0c%@n;x;z23W? zGR7mXY1-Z(;>M1Rs&T1e%RP(amOW4;5#9L}zi;V3yJKqG{DF`7AOdM}bjj^fcTo)qzEzT13l75K%;>>b z=%lZPHst`1WJ=+wA9H<{!Rq&$CPbBHu2Pa?HaSK+VZloMvZ(`d}$W zxP10pc*v2q8S!s9X`x;YGc%ldgO5uYKZ57FKV%J0Fbn}R7d%acgl88!X}$2~$z_-N zS!W4~brO9;2lVs-M%x2G}ITdCEuLWQC4+u_Pg#4l&w&w?vNK*9SAKRX$iRuMO6q2h;y;sJ8fr=iDtR+_)9ZiIVD zB$jdgmZ&XEl+k%4jD@Y~tD{@5s|u|alPu(^=5;6EKItobAO%&QEfAWw;;=HgNtYwS zctwM{$qwbT*m49aSs}SWuQf)iWOQG1xFYf7m^eq>a1=02zcYcvUb&(xcjsS)K#Q5>!(3BfO4s1*v#ae!B zcJ0<4$?FDFJy?NY&*7|9h7v70IYGdh7qMNihM6GJnlxm?*?!8W+|svwRc*Uinsw0+ zpJL^!mCbgM4fo;T8+#MR%Z~y!5jSiR_$1a|!&J_jEM9hE+g8tcHw;DC?eYN}nXETQ;F#id ziaz7WPaClzHZ*CWX}l%kDEs3xTc!HUTb1%}Hz>Qr5Q=B`oKrB)?v+?^VfqNWy{j5 zS&4!|3aPkL(F+;G#g8&969Q98BMfviw#$mgv&y*!3r%17v84UpA}%h7w6(90s^{u#!P<7_In^IbsCX!xq_m zsmi+ZF@lq6ITg=!-L?kJmkO^rb(|f}5r?SB3ipuW>iz{zEbjJdyU2AJmX39;N%rE@ zoz4t%6)EoFV{ZRSr?6(HtZJlM#_|Z;E2`y6mU*k3ZHt?|!rm#azSQFAWZ4|Ly$Kvg zMnG{MXsrd!^r3#MD!tNtnWlUGNv5j#*O=wS>#Cxm%4xLOXbi!t_@YlLUM$2T0CqvGP>H3Qj@^6hGx#dcnx?q(?+ z@19NGvc#x$mu-ubvxYOf)@-Q;<_=k^I~(w4->V%rbKh3t>*4-Ia=03-% z{75{$>u^vPWnCdNKeJx_GXg0>_bdi}urezH3n~EWnZeQ==e-=B&K~@#PJOJy=TXFC zxjrRveQ-;q24G^yvEk`;U_UmmW^i)$oMHlQ$bfTXU#^?+^~F!rfu z&gkkhhbIymj!XLox4u+$(04HkfqaCHm6sCMC)T{}A+A61m^^Kqz`Z@3jZ2!&^PJr> z;DM(n63?&~mauDF%R)YU)lJ21aqf^o)0qM37lAS;5SShW1xxmyQh8;`6L|y`J{+=t=$$l5`01(D}A~4WH zwiK(xU;&r2@7zXB>2 zrn2gHaK7C-UzjPc5a2sem4npV2+fFy#L0X+uL0Rg(e$uhlhcP zYEeub9uHm!64ag|gk))AQFKUI4f+J8QUK*?&x0KynNuOuPZ_-N2Bwh{_WW%b#HmI) zt{aDdjZHBnwL7eXiARJaWAm*>eeyrqsoQF`Ct#G5pG z-;kWs#nJ^wElPOjnKgyCHR1w+0x^tt|^L({w&8ESM3<-Zv7kWWGxiJlnJmvTzDp+Ncw;bHz%eadu|~fGB6i0 z(p@3<&)tx208uwU9N~E+DeL`4P)1o6u^Oz-*FzZ4AUy_vQ#87SH^;vqI>387C@vmA zEok8Gp|s&uvoG>$ELTYP2+HeA^Z9_L*VE`deTXz^DEGRPSjd|O0AsBn2}$1ruuw*Y zyZf{|4XRnj$%jk}Xw7uat|sdZ_LTMTg}`KKHa%Ek0r#1%dAQr(KAMqTI9B*AR=6T! zAnGDm(PX07Q*>cm?08*#TY#flh5gb4anCKJ072?+>IeW4^)*&V%W9Rh9vNL!miX*` zxfk~4?rvgh)m-EId$OI}?_|P2gjzi)!4?~CbUnLY1N-Q=vK;$+;D?NM`&0B;H=Yyff%G{$wJecSP&m0zw7>ZG8i~nw^)?Hz z*jxA4i0jFQM5^}78smi*YR``)%IAE`ezn226)ay}vNmeih5ESX9lfz-Ns zCM~;*B@1(E(2;gvRu<8eL!)1`h3F1fc%VYSfn^*cJxWNPV0JYB1h`)y*XKNyHb6%59ey6Zz~E*nljE8?+9 z`cW>LoKrWf6mu;9IlZ{o=_S-kISHYXD&$LMt@@h#NwdclnazWALHp(}KJkw=vhI0T zAEu0JYvsk#aNc#G(4Lf2OXnr>;Lv(hrj>4BpFuI9T_7KhWBuhTw|1SuPz)u3Na=?# z=+|b?pQ(F>ZSH)NP5#qnMjie?wjV2#@|$)AVp9hyi&?6FczH`J>UBfoW;OJRWntC# zNr`5-U3Bt?S&GKbxbI)W_UhLZ7OIBBbqi*p7YvQYW196|MBkdAR>qW zB<%!iUFb_m7xn%P^YhsHtYpw1@Z`j%*0RY$L5MEE{-E@sw*t+(Gsn|Rg5B#*}}>^ccQ*S~y^ zxjbQ9?j?%QH6N;yGAQ44Og>mkp$Q{q0gz-S_0v7=(C)^f9N~P2!w_)DaS%pv9P_y` zWDgxF^oZy^^;?f*IgYhgcU6DtdI<1wZB42v!a8rrM)*Gn-Kq>Q2$+-hGPUhED_2%C zA5m zF&@ERE*P6B<CWRCE%5zPHHF8Zr;%~**QPT?1u6yScV|?JF0@>!^3A$fZ>p?w zjNkn5a*d^Wr^GtRujk|$H~Pp;?u)mn>%iHCND!Fu;`@WocfR}xqcjm4crsKFV-|`DUYl1OtL{B0Lv8?HWq*wz_S4s0RsU* z004poV8Fuq>j&`v?H~U7)K4HIm>WQZ`P40d0pby4^B_|mOwRRShJ%ETAD=-S%iSdp zMNTYIyLmF0uOQ8?Atoje5`$D$(+?!MMFO1+KKPVtOerzL2{!np7@=%pp!?hbT7i&{91iT< zHrtMskC%|muXE*$BeSQ&lQ%!x{z6Wb=+cKTLbzB+j1|gbE{{#vZD`Kib5&}WD;fC6 zsjt0hRWDeN;5if>v16bThegdRQ47@t>tg3ayWwfZ9v|-35%jvd?N5vZ{S0Bn= zgHnoz;4&L}xllZDiaOPP#1z%Ikt@q^Wvd;8hZ&YXgY*Az4E{F`=Won#vj30agei~r z(A0X{dge0N~ziYwcl%%;dTppZ4Ejj3Tu7zY( zikOk2_E=OQL-penB5L}zPEYu?StyB-&kL8BS!7C>-Ut~(%A&OrXb7@$lLxiGl0K*K z3!oSVz6_3I?t!VLEPhOj!_R{REA192nyX5vJ9?I)p~N=HM)@Gc=zDSS`PT&@syV6A zRjOzXe(mo_si17nN3}_rP(h?PIzz;cX@YsQ{#UDe#KXqG2Vy3pdK_!rPMFvB8Vaop z$GB|zhAbmV)RbMlvRQpBatjr+HHG-Tl~#evMvFq9H@f#K;<8{dGP`w0ZVsTl4r3pG z=g`gxi@xDtEVnL048)M`wDfnjvvJo+~&^G{;z7N_m%I2lY z$>j~zsPu5Vk;GNZXpq^HQAz;AtZ2I926gIYjtamu{Hk_OzeH)nd;Ch!MBD5Y3x&Ap z@yduPXJf@!*jn7gCJT~Io8DDDgL+SQ1{{vIhO$-61qUj*pAJ&)A&0`v28GjFuqZQ& zcZf$H)Qgz5vR+f@)cH6bNCgb2-u`~O{`6I$taW7cY@22ptq`7wCis54#)vXkr< zQ8Ws!q}yH<83pEE?wJXJXsjj4`nX_YJ+b+w`Z#vU+Ryt?mb**I|;f%OidE(%r_z&4|87E(2>(?jf zU%b_dyJG3hCtk%4#Ye#?<|`ZVQ=;HeK|hzDCC=ofpi=IW4JyVJeI!!}lS0zNk7U$J z!9E;!wc4J05OB)d`t2NaO1K(6#vFPkCH>DC006{+umQ!GDo_Ks5VKqD?qO8NW@uHm zJJgX4#-fy1MAt6Mg<-Q$8Kt>k%Oz0B#lSc*&yu`C-r=Q9ODYO|jny{J@wN12&tqdU z@!Vj}qOll8rmWMHT3cC^yru-{Hh@*-z6wKtLAg*KG@>rh;mp2~Ik$?bOky12#r2so zn-NbqS=X+z)NExqORc5)$2*^s?H?aoY8KuTIto0}R@GKcVPp_^L`Zd{jE=>hMRSX> ztoE9dm49*`C!0l53m5(iWMGsCd3A*jLHyxvG0N?G|CyZjF!1bk$K%tJ|v zFUh2dD>=w4l#}8ZUkjycBdK_}nX+##m90lYaNVfVR2tURL$DxXGvCOp)!zz);!qBr zSOBo32_)PK>$ep0w27qfNzJT-K-l$ktL-`{)jI5<>fI) zI0}*kSB&_;I81I!lr~4DJ-6fzfg;ME&?ZG#(#I+a+D_F*_Zm)2yp2+B5Dzt6r4{i4 zvG_5bkmiUvjXaXPZN<^>+6Yyctpi7@yRL|umr&yc5}h5rDws=`!uf4;`%3(-;nPcX zHkkjxlm000#Vm6in|Pi}1{)yoKeGh~NCT1ti2O@Haj=<<{sc4}pGh@SOdE5s0?cIJ z7c~JNh^1qCp5k+4IhcghmY6A>G*^k&HE0h|5eP!26GF%PW+3dnS>#V8j-yFS<*^`9 zR9`ls*1;vJ1UQh~X0QIAj{zU<@s z=8Qu~6W?IEkUA9AcCtz{R$KSb|GtJ&Z21equDRx|ey>$1#Qyevf0Qk@4#nSQ+@Baq z0EGS}hS>jzpLs>zE@s=4Ih_} zu3 zz)4)Ss5{AN=nkJv-@1k%3Y_e*W|KdV*QgJ0BZ9&xl5FogaUR3f#+6VCIw-x zZ+lGEcw+Xl*n(Bx&-;VP}rHWy$YfH|teBij2V=$BaR1RY0W|`z-qN zfv%*3*`aIcuKjc%9kFa$5E?oVNf*;%`XWjUtsKQste+J^$XE*Zc9i!~4&`$bWnzl- z2se$JIO-Yj!)YHZ@JiThT}f1CD_c!|AhNakN=uP*E#;A}{n~4No3gc3Bd@KsH)au> z>uFZ0_Uq|(m}e9+oSL`R-@5d3Ze)5)*>7a|te0(M2b^zh-t?T7G)g9;CJ2l-n+`F}XRF1oKgKQPM^&=uXyA9)t+LZ?7utg=RW8-opLZ=8;YK5}B?(OLE-4WgL* zXmDWxNVgoFxYb}B8mCiNQkZ_<#K5TN&h9ZPJS{j<@3yJzE@QlvhbfCt8lV@4hmZlE zh+WJDjiXQUyn%^5GZ&4A(CVT5%9Y80o|MRu`aS)&B+s_ z7wd%c2*}b&QO`rZ3MwZPthKx&*jVjC17vE;_c!I*DXcFAD<}$$3O2RayDBLexC&>Ib%;g@nR&dy*zvk*HKth@3fIt=nqWygMj;x*^D-r@G zpb`arZQ6fkq=V0F$1It^b6|~Q1+-lbNuW+t*)!KY`zqnmZ%%nJfi9*czj6lW%q z@mo$$sTWhh2#1Uz3uIQMDOICKQobMQkRhp%i=#S{c#g2~rdI?Y?ahy zlc3?sa+GHq!ngq>FOt>hj|fXXbUfd4eD*L5Jb2!8(77AU5^I1Ifkx3lJ~8J(lv>Hjd~4<6^Vvw{SMzM9JJ#;w4bUp!I=QC%V0}3uCe!j`?8_n& zD3utSO#A0*`PU8<>#y4Kuk{&>%_096@L*HRW-3GT`(nWK_tE-C@P0rt%r&!At6MoZ zuGUHN+xW@=uYPT|R}-~<_xt^AsdwDlE+ zd?1Nbe-1JS<@4ZS-uL^usFw=tm%!|k7r(s%=``QDJTFAPcK;E=K&w|YR_`e;ar{=# z%q%wH=J!{>R*Pk&m$V0KX{|2bhEoL{=SSKwt7Wd-OGfqf#;wn#4=YSY+lAE>qlmHH zNbSzIaM61h6unPCsKC0FWrS~$L^tXVT3OFIpkt_>zzAaE`eyEEGV zch>w{=&$eQ%LQMqF3DwRGv9NW3IfU*OgM_z_K)-0M~<<`2a=mg_OTPw3%*mU4z^_1 zIxL7MUW-QDYa0-DJ%%lF8d+?M)!Fvmvmzf59Vl4*Rw?8Bs^hAe%XGjdK{zo~PO3n& z#-v?ZM!qG>+}N&7B;>aA=*(S01HN3P=(E#5~yqy(ZOkfT@tN$)?kZ7dP6I0ySO&k>ce;f-E1C zAu)Gv#4#nj2>Tum1klj^uoSxlO{fLsmZ{?R_VWaY6aum=u~UgSAMs2zG10a?S8H1R zmM5Qw&Po@pGHwhs2ND0bs5!T5C{COs7h|W(*xP@?GLeD-!(LqI_K@x33?4-exPpxDQ?D4vE=xL2{+Mas+Rt^~!P228}U|Ip+9^oobn^k^C@@FCfS zQ|S=&aRS&~J@ebUJ8wR@L!McS9e}g;8EKR9e{?+gdnW#gocOD!HZL zG&U%6E@v$VFjo2w8ArE=8A6E|R7i+wa*(NF!`6ZsQ1Yk%^*YIf)ananD>*0H&xS<` zk(JMuBCLg4oK*9C)8mPD>;IcwkH2T%lLoM20Bv?SG~ zxW38|PG=C1#pv+=!Il55R`{M+&LCRM#(r_J;1%=3e}Xgr_&!x~X zlhKUYf@z?|M`9E?%i3hC`ws7Td&{%thEM{MphG(#s4WRGV0_hbvSARZ#_n8eWji+B z;CL(k{89$^qDK05R-;Su<31V+r<9)W_;~@Fz`bSc61@3&$v&J@CB^yk{0f^R7xi=K zoB?qQ6k7}d=X)o<*cISd3L|G#UJ9qN>3b9b^V(dBWRBoijzXr|Ek|<{moCTfG;c1y z)RtA-I+imT}I@?=K0qPmBQ>6;n$5%h@SXpMVG)vC~FmJ)PyO_CLYcklbn=1d-sxv8r8{R| z9_c+j`>JnSbv9+>y>~Wk7J27<#_G+}^KW(~Rp+x#Eqmv4E(3QizI#kRz4+m?QFSpN zaB;hLu@HpId$|}!?R>cu#a?~694oqixss^Fd$pRP=X|x6W?Ox=p6R`RwUHaid%aos z#`$`yw50layRv2fdZ%`P_vdcowDZrs){W|)`yChiKM%Tb`ECyTsGr>&4YJqV9FK?| z+?BBl&(`uD*Hp`)aeK=J)k(%fauThXZ`KH>cCj zZhu{F)ZG5Qxj4AJ1rX)}aFx43bcaEZ2e}~X!EPLh!(h_bTr76w9(;qt5So!(9MQoZ zV!y*sW5Ysh1NWnL;39GqGNR1~{p5Y&6kxA9=*> zAq6YPx4a1YdN7@xSfVU53p6;ikC@yGsx-KS#Xi$b=thQ5SBki@vl^?`~`3F`MmT-NaWJP{BG}^9EgRo6OIt)OPNP$?yuM2|~ z0ux}tw6vBkl*;3P__)LZ8rF&IN96zrXh;S5c^rakg`M0S2G>~K98gICkc0;T;kSzY zWIo9;biVS`EgXG{@DGWst#X3claTxHyX!My+OG@H~6~cJ6InXJN zCVAG^n=6&9r6iFL6*wy(?R8NKUNy(7K`L{ue2b)OCy!p)1?yy%rRd`b#j$mQS*VpokPl?lWjb|=LPQCPVN8MBQSv-gNnD<>;8K+O12i*{`=}vT zldo1B3QbIu1)g3Xl`c z9+Akch33Htnczz!5@O7MgS$OUak~HC!5zj6C3(3y(f1O6i`Dc9<`^~ zjsO%YsGJ%hllcHKf~c>4q?NC?yAvxyfCX;qnUaw(`x-h-Cr8+(_|0RXhGO9yxnNKyk)|=y zSHxv`Ci>xP?OaLMsYR!Kx}^Q`D35`DTFb_kM;@$uRJioJaiVj+ebK8B#O9HrsY>@5 z^_=Yu9yN=t@8Qv`Ly>(;p6cHf+Z{}#|7RHek4WSFiL~pAA_fM9>M8kHE z_`fP{2HUD1#yKvEK^ot2Ielvkw=`N|9?R=<$+0vO>g@;S$a#s9i^g+wt_u<`_AQY^ zylwZ7_clplq7rt6ZPT>LeAn|fAP5O@w?T(b(IvD9g8qqIuW@lRT+g984`!!;)jCt< z@4mS6HjXLY+5YKEUV|*}j#8gQ@cg|I{3pG&|Bbys|CGu9jlGWYWM3vo{1Gh+hhA|! z%)f!?U_?uP%i&DgSJoetSnRjI|6#8;BAg=VtA^5v0_9f{5D5VkHIHh6YL@D2#OG)^ zcU=s7ReS-PF`(M%FygwBL1($~M-et42s_n=g1_+(d)*^*sny&60Dv6-VK4Z%sOJRS zCY7y8h*;nyRn*5;jA%(fN%mo+&D|Pk!{pDXPRF;|V6RqTQgyffLqh(OkJ;*zo|tP_ z?RVylC%hECbc7N_Zu-Obmj+)YNLKtWMa$0>rej^5KQ9hfKIUk?@A~!glKL)&$rEpH z02yI_u%@m*7|1q=L84XY0G*k9>0%g-z#j%eL2$xhxB29E0O|2XM|BRQ3+yX5RA-8=J~}pH|g{* zT~k*}&p?Q=E;#JrKXuL1;LprL`uw5sl9}DUP#H<=xoKg3;5ngw8Q-+< zPf4>!ijp|@?5yw^M$){2X}_RCnjlRF@oFnzTlu2WxRu1-V)j@_eZ54i;XLQT-d^y4 zUbphs8W;Ezvzn29lxf-h%Hi2bDSp|$M{D@t4GQuveuaO zEZ4J)q;MNfdAKYFg@V`}MJ&o1kii9@XU>F+W>(q-w2p98ZG#y?F)oCTRVx?uI24F( zCmjE}qZeYHo=VYu?9|if$na=)a|-x9k-%N*hwpR&pv2?H`B_@Ses>QOkiMC_*=p&- zy$zg*|LEcVD=K_0z0V~<5tXp>`HyDSfD@Z8SWJms-@86S!_Nz5&_Q}1^#2h#F^Y%J z;2${+eODY=fNF3QC9#iChe<`o9C-9qe{XS5u$`H~d-VPCgYe;)ko0?3qRg{p@A}}| zw}HPlKR@1p!iUb98N3~9fstU})gBZ3E#HA~2CrR@ECv_5jRpUUI`&1ysNJc#XC>ij zKj>OrHyD`q_AoRNRjd1%mZ4cc!<%?rZA|Dd37FZ`?vJD7^OF>VuF7)j(_k!<{6V4d2<&RBZ^udM z$wJg7Nby%T7y+47WIMefNIQ(z9gajXo9h%|-Ghg#lIn~EYnH`yWIQedlszoNr=U$O zAFTkrTbF#PTiM<0{C{!^1c6tYdN)W5-nxxuJ|SlxV^MG)t$YF=z<`-Mt)XHc$KAJL z;n0EmC+Q^c31{jT&6Y~cOj6FAqQrgd)<%LwBzaVX$@Oc!9`5%-2t3d2m&GwJ81~t< zf>(LK|1jVEr5UaNm5TpvC-_JAI~1P@j!`1>KZH>eV0wt97X6V>Y2rc-GAor}`g_?{ zH1&m)JlxdHH#8`~P#TB?#=-2urk)_iK+4QNUh)9|;so7SNht+ms1*y-xt;j@8t9`T z9Oc0p3^HiRkF|H-nyO~SM&0!nDK1oU3eBf{qRZ3M#`LtjaR>0MUorlY_|#vZ+AJx8 z@do$v0_iRLPW|U?DhD+?aS1){zNy3Bf;(Q_t+GYa|Jh?os>$SA; zC>3!K$*zExkBKw?Xa9B^Km`*17t)sr1!2Md^l#9S$*ySVh$6ZQ$QLm0YEn2^H^H4p7CTL zC^tnlsQYuN7y(5q*Xu&rEY+tL3Y=!_diBH}*=Oy7l{3}Ip7t#KmI8nx`$F>5)lGZN zu0Sxsf9V(~Dd>95!2f5mW{vqG5pHIYqqO{H5?sTQ{2JM)m=RSGcbh*mlBVvHQ$OL3 zpc6*e^{{JN6?R(2lI?>CS_uv=^<7EQUOrt8<{}xmH|=`7Fm%OC?nX;{<*2aAGPfj` z>A$wVW%_py%PJ;L|BYzd8}gScV8XmF2AkfzlEbwi6w|r!ZJe2~EKowwr+%l&B}h#n z)N_`^BVYQE1cYVEKRF4~#KpFx?ifu+DO53Tta{bEcjVTg>6D7xVWxjhF2o{JaYa+Z`UR|M*C~uFC%G`bZ{2&gq%q@j(`m6HWjHEHOvH9dKe_=Xa zGM4hU{pyuC@ZAneBA5Qz$#WcL-`}47gDe%%1B$`19Pt0>A^dZ~Qh`uF%74*7|CxOM zu7L{VqvQW!9{9uIEP;)erI?)i!{I9Ama?&TjHmwM@Ej^V1E)grzc{>$LiG=a>!)Bi zys2EZm?B!L9>d|&1^VoNTm(DQ4R)V1I?|I$XIorWa7JkWG8!23z&0g~7d!-G9w>ND z!3k z>kNzKOI(}QJa`mLwjdI*ufIU+0vXItSU!T{(mNvC`sHcy_03<%I}Z%91Rt8O$ul3b zou}|G#A(_+N7P|3m{7-w3vVahwZ`BNB6gim+%HmIy2$ z<(DwhF{meE`p2*<8izeoOO~!e7~X=<<#bCLHVeA11HmVD>Xv1)!PDr)d+ zuLX}e+=#>%L}E+MqN;fFzmAGx`of6@GJXx+sd}ApE=X zbz#`3QkCZ#i9$d*yoWxwt>@yxpzEz@3RyXb&nZxare;SIk^cswL|BW5$PGcBii8LW zX(mqi|ZorWp?09t9aS?@wakUOw|8Zp{Th6GidCG?=*p!jLzK z-OE-4WV+p<@o1M2W=J0i8^j#jqG1D|h*8R1Hkp}@7lUpl%y1$H<#2y8qaB3dzTxx> zgx92W%7f_HujJg(miP);#qD8Y?op7K@GA=yYoUaPsqQSUl``dMOA zjE-Oqytj~}ma)^4x2VT&^6W@$gvj_i6#cp3J3`&{=X=(Asg0$bUq^dA>32u+JFk2S zE?Mp0W^5nKXnM7^_(zEO0zA0?8td}`tiOhMWW^NUDY)omH)Ec*QQqae*EJxfLvB%) zhs9kg@^EzqYKPuot@EAH@6q8LA{{Cc0I)vG&awnNIAY=IJc-ww&pKDl_T^y3A-Iv# zV-=pELgxqKdSgT9M{jsqYPx@Nz#cv=r2OX6YmVl`-RN(pBsn>654R}3H~T3SMW|?x zwt=RP4{5FYHUD)9{>i>Ds|&6?d-tZ*!eP3CxkArIpbPJ_D}4zJhf5{Cz4;JW)$ECp zTT!?viKHI6xgN`sh~7=MN9H#6T(Wh6ZA!-&n~U0co_;q9RjEx@88%KFkR*zCUkIypb^cm_=1(_`P3t9!AI9n-Yv3|_zM@H~( zAF`tA?;a5;lUOfXnq>+~LbF??znmu>r5w8$PMtAVSFwrIducmmk*UDv=#t#8yc_80 z1DBeRRh?GG7%a+Ym1dVK!pkeh6YL40^9hRON4|62e|lf3V6k;bWhaj*{MO>jCpe%% z9lPKhDSerkg>TX1d;LMfIXy7fI`F_Vk8z&epFZ3zLM{zn^#2yCcc*_k>Jo6`Mvixs7tsQoDosG9sz z-z}o+8uWKC_-F3}^p7Lr1xG}#n*y9GTyQu)3)hh@qA$qvNi20&HcmdvuTktAjpv91 z?(48!B~Px-hQUjp?6WJ1MZ^$0h?2+IX$C>kj@GP~U1<=1XoxM5vLC8xxEE8SrOM8O z7W+T`%)$EblzL%mdf58J$EDQy!4M&t%M&S~zjW9bWVv(hof5iwV>4}l zQ%pZO6(g(@Q1LM5Z*4r6oyzgu=cRzJ0pRJUf=KSnyQhPgB%~j z5CK(Ho3Q)fZ{VvFJK{grU1$(3Vwk315*2px%~-=#U^wXy&yHAIdME+^f?B0?=YJ}h z7Yj*!epZPkyLPsPKdM<3xBtXy%YZyx`rN!TnvR9$n5Co5j)g9~38SDdp76wm)1C7V z2l^4o_EEoNN1O zs`v@i!#{qh{!x#g1D~+JLQa_C4zqO!)}Li}WJPNRh&TR0{a&y@=u}!jwA`)~?^^e( zZoU~TtL@QkWeEJL*7Yiu>g3b*tEyNqWLt5mU#Z#Nb9?xBE^{I0RrSLT?s6=lVE{SB z?;2@ebO3(UE~#Xe|8rQlsNt?0F-qotX; zzKOR_H`{lZ>^KQNVLrQS9NqX;0OWM4lQ;;^e;#(@^;qMllE}5-_1P@tXyKa-EPJb7fO=j z84O(P3|lFpEvwwZuNg@~hYn}K87c9du5oC7S2$YNjJG3;982Ywcjhf;XAol#OeK5R z<`7W6&67l|iP?DymJS4X9KFr4GV7r0OQJ#&Ykrmv1o)2Ppexiw3-4GLetD54D!WK< zE*y_mDX~*7@zxxfNuenq#PKPJWL$Z&5yh<{ZSMTsxqLZtd9vTQ;XrQGhfki>ye0e< z%!*k0g*fz;-y*Bb3#+`~c~_;^vhWzE0?!n`TLl*r;-1QGDm^74bS_CT_GPnDQtcFDhXJmuTf)R zn~EtADX7cUSiw#4tmwf^^+_HDk$5whPzgy~SsAYvyIyCG$b{O#>W#4Zc5Nb;DVlpMX8L3R~aqd|n-Gx;remriBVC@9Uw z+zQiJ5xwDNM5gUsh9s2VgaX~r*(=km-1rOf!|REU=}!@cOctEViWBq@L$aa?r<4}V z@kV7kWGe%(t%8N$#1W>3l3J2TJ2JkI{D3qwDP#uLbPm~?mt!*d1teLrLs&5Z%`79B zIz&NypoOFdO^vTF=|5vj6d%NE@xB4wE89pAa?&jWDENxv36ZXoNGb8XA;}Lj00}aW* zM}Ll3rQXCRNL2rhOWf;Y_0lp(Kd`3i^{MV4&!zsAJypmAa$Zc8Ccj_j`ESTHj@Km|9eAcN0 z58KE|5tZ%}vLk%i6~3ZI0dCloHY;a^h$W@jz&b9Owxp_14=D4Fatui0(IVjnWs3!6fMbSjs zwJHM11S1(hq&0VC?SkuTs9}i*=t3B!bCFvo&9WhCMsF@ixba*(g2_1>e#R3lra$4I zCiv0iu+_=?iQA1o`W#8}9QrJ*Zm@B(^>4#b(wCO9p7sdJVgT3^nd@b&wYj*>)wh~Q zOkH}2HUb#=`j(2DJ#)fy+oRXoI(BTlUZBhFtDivd1Nc`J!!Ld+s;6)YRb=cbvFTPH zDe1uVJuRo~z69w9+PvPwB)&_obXq$|bVrW~I zLJ%}m25~g+Mkhuw$^8rk-OQI=>cI?VlaHzVjuF;T9QOhL^>yL<1I59=18trZ6n<~F zLI0Fxx~y`nsVR`CL>9aqknSBa(4&k_t;CKf%clPil;_Thm zu9#U~87Y>1khKAA94(>>-Mq|=m=R+kL0p}J!Pw~Ag|Nm5?}cX*)Fp?YC~s5{HmBn zwLVX(oHv*R&M@3}Ww)bcx?8(fj}5yY=WwO(qf?jjP+bBm23VC=iu4x))DJtneCD zBB+l?fjwT;AIS`P&DD!47n;m0?v514jf6k{EN3uw_P6cvWINX6{fBwSBsRqy&|h1= zD!b)3qauT7bn8#(fZDoJS+!ix4!Pf!m(lpQyBhdopX9<5{iI7;DCmhD;*-Hg;`NT9 zyUuTtu;P5uPmC^9ok5(ums~@MV}#E$PC&cDkA#jr87@5a<*es1-z1JSLNQuB3E^(= zV~`>9Tn6e->BJ5~9JR^#n9@i$JDm{F-*%Z?7bKRguQA6!yYxGBPM`v1dUlv4UcS*ENWovt?Vn*;D zkXE=Ngh_s?p8alqs3vveT@)+i_2XK%X=fYlYp;2ruev6B#(aiH8yCo!8KRof!W@!cRdY45RY-~T;^TqL z`;(tcqd&@*!p?|6jn+g39vf4-v8Hh^l{5KeoXILa40v38E5wLq!9N){$cxIM2<|a} ze0E>E##)=?Ro03X{I$A;Kcx=|ucRn0hMAJ}AU|iK-;p3 zDcX>;xj<#M3xv}DfCc}~XjQ2W*l`{MBsO)rb8qqXDrGK{nO1NnWz{3sz__>Yj34mW z1(64ahQV!Qs3|U-ukCk65vOKn7YtHG2k7vi#54?m40Ds_hd&p-nC@$?lVBU#8Q&ba zB(PJVyYLeo@k~58bm3rj$js%evxoUgsH_lAiv`x+w&k+ZAsz%zxWA-HD;XPsl5vQ< zq&jmpO7t@v8@936gR~oQ1qqLVUYkSU2_N!~fN-RTQ=l&YHH=RRHV}Lmc}d{bUgzxB zrNY098}a^eDj;sOU4=E(mb&hfv?Ere$w+DPzGS%qj6gPNi?uxdFh%i*j^z1kZZu6D zPanb|Y_U~_gm}4nK4k6M2(JY;6lo@$L4U#8y-$zPiz|NM5 z>6EM7r9*po(Fap|LLN&Trw46&dO0d@aU?bB%V*54gSF%LC166msdjZPc_BMY5}lvT z&v2^({^L}1H8sQs%81c>H@mNE`n7xPrg_Z%1DHVv}Q;y(;h z3kIkSJx-|JACYl-KGQ#sKDh1vW&7<#1|6xRbrNuqqOJW8; z1mgtkBjKM5CRq*)I-!LAV7Bb68DlN9W8X|c*d)H&Ft0TJOP%)9(C!fz9Nlv@iENj_ zQ^Qu$v3WKXvgJ%iM+}_59O3hlFK|3U?MGz54b$!TOV0q9G!J}71{*ZpQCTb=h?K5E zaSfHzo{BLJTmo)_D+PcJ0Ick_9df=yR zlDPYP017~^XeY){YGjg5J+(P-l=@zZebygJ_2fSlP|tTdJXCAjWaz89Z4(y5b*2<6 zMRBW=hy^OZcytvni3kz&3MKvHk#I)Fwae7RFzOPO;eKOT>0Izvx{6rNjvptSf1EA9 zcO2;dDZKZuw+R$)aC4}w2~vQ-9#)xdOI6_W7Zwxee&aBHlKtAs=E=&2x1t4xrn?IC zv=he}r?Ii6;3XnQT(52hZU!+d$KIo_(mGFEia1!)oSKfdk!pVY>PeyOL4KN#lslWp z>rmvk3q$zP3%)(KVm5nUhBe8)7(HGj{$}DfW%H}&sRwIc+}DKT|Fd{w{cUO#lGV1a zWL$4Er!TgZEszJ4EEtm7&^H|5o$*y`b&=Qvgs*EiJ_w(c+v}g=)`s> z3VRBelxy4ig2-##Lx3p=(~jF={L8%68-rRm&yUL~BR$0=_nf}SW#21#d7|?lml7V2 zxI9NGw1;&>6�m(EVy%9-E~PCmyDcwgdHD)To(EMI6;XLjBy*FdmU4943RWIZv9D z^&4)1$M{AZoRHJthq`%0)OvO%^heAwyqN5K`yan-0_(xlKc59mQR%w_Cj$riCa7Tg z;hsf)-Bdde69Rc(0x?0IV5yHAS;7NRjbP~7)PJ0m1-;ULNp*R)BVRX6(1N(yOowLM zUywAaB|_l5Oq2Ibb#}Ied(WCF6NxX$RRRs6@4KWHi6o}nAX(8+*IH?Nm=osO`*)dO@GjMCDgWt? z5f12+E5p>JQ4}U&|X{j>tGBuri%z1LnF7XFe%OXhC|y$N{Nibup;Kd z0`~}wJdWMpxVOZ7N_SMm9lkem@pX&Ad=S}7!Gvd&UK}VjTmAeLL+f}FEkJ7<|Ebs9 z=u|YJQ6ge>1a5!ILk~_upL%`}o8+0J^4K!Pz0u{=^EYQm3hGDP$7z=zl8*&?FdIu` zzq}7CJJE1U&GQtRrac`|!^LCfM z<>&^#8a@}*+xwTsi~r?RWETXLZWP1O0hwl*gl#1U^L12<;X&lCQ%>SC2-O9&xxf`S zn4!ud1=mA$mj>D`DD{XAS-DXpBMzTkBNj`orSe;8l(Zx=Qy%9}m?bS`Rmg6S1r8UT zjhkj71f-Lv}{D77z3QFOFNw^G2ol`Hxw&mVM<5l`@zZF@lvIRzV zV*7lWI`1yjYO3RYGwpj8cdqhk(IBF+n{H2RpZd8uPB?aDTO0>rtCYO-08zt{Gh|~0 ztPD(7wwH^&?`ftfSJDw#lx86{-q^Y*i}#_=5E7>}m4xWWs?nk7tBE^kqFzD0}d$w!2aq7W)Yic>n90zGM+v`ATJDEnETAQP`4I zGHGy&86{!qVa4O0IRrzCq$T~jYmYeRO9d=82xqeDviavb>BI|gSvqmbK1OV|jlDo)ySGL0{_b^I$IT+( zYxR#LCwH20{O%^O2Gb016QH-n26LzcE8r$LqbFcTVCSmKTSw~+PIu<>Ar0gV_Rm@t z^CA&aksKaq=}x+lN^{P67}Pdvg4c=il2~Ks>m@rZsT^@~#;`j^k=gi!bwE%EQ=q?!4%lO5@@=KreL`V(&U&eT-| z8Sw?%g~#hbBmUpvSR46-k0p0@i9-sbKsD6xp~HcuSYyPPI+E}ko#;7F`rb6Ji>|42(m!p0+col19IYBIy>F$VS_L8>zh9ITh0 z6i+bWEsdUq@Qu-4+jW*k^Wf&lDX8w zO^}7e=O~%06Ai|?Q$1h8SVx#~Lh>Ta;j6B6rs~F1=wO|Hu+;3moQcF+UX;OsQnbAt z+oPd;M)8cSL$`!Pmw}-kkJ9q!z2pK+->ivdMQy>HBm?#|rK8yUEl<|i)A~utta-)` zWBIY9@u}YZMQO4tvpvPXT=~fSru*y>=ub-e^$9V0X<$NbttILCshbfpIpjgRO(nbG zn)5_-M81Op>e0v?F~LyVpa@{p{DB1DVQB<@kG6_4`fWi63$K{6t%K32oWz19eGPqr zPuB`0hx-{UDOe02UnqvvzHFcN#kNmYa--Z+J}C!Y%Kz!kmjb8sA9=`soze!bvR2-n zBC$n8ve}Mil_t9r)DaKh&YX6%PFMHVuI$KFSK8~Ya?Nc|fu=?}A>ul~%R7M<(;Vnt zQNk;va>Sa(Gf;>IiHP_EoI$8aq5e+-*Do{|2nqzusyw~emf#jqsGL9KN^3azY@@{& zq(4tnkm}Qf)I5zNX`x?4{0?i^QsD7;Iq725)YJoQ#eaH>hA7_ICCLjw4+v0KFl%G@ zyh#3@cG%Ot)o-}%p#+E#zG`m{J^j5?)aVsA|4MU4Yb&N4Fi-g4NujgVn0BJv|< z;Ya{=PiSI1zo2JlNYk5+4ivxj0xcjK8+bOb+pwX~O!VG|GArwj(FJE@R9}V6%sk{=^QTAaeh4KKy^M1BmaO4c9Ye(wd#c zvV>sFU1eqMIi1X6ncg2$YIon5&tU*+(q1@P|yN1yaVNim@EoaGmi&y|S&@a8W!!P<*o)DcvD)`^)z%;C+YYY`u*BXEaUrxm zDvWZt!TZZ+sl(gA`TV?lWrtso_H+lRK>53`oKdA2=*($zIh{4&mdvmRC4op_(-v7= z5{PGq92$;xC~86QGvlWGKuKP`!&DjPl6kE5>F7^Rpx~2h<+geF@(Hp%;_ltZ4Vz@^5`j;RB2%~Jh-i1Zr(TDdN62BH%{{qBcoC4mSXWy0uujmxy&i5{&;+GU=sStI6nPAjVlS`&{^yS(w1XN zDLEU(936jT2mWsCT!+r`;F$0bdN3I2CWGv)*|jh*?7jNtfs(sT~1jaqSsf%sYaC-}|R3+pMENQ1G?pMWo!igVH_E z^?RG~rZ%_GVSFN9KY=pKQ79j;_;BjuOV97q8r~}RZv)kO*0M%U&L~h8XV`gEn0SbY zXXoo)%<;?8Ile1hFIC}kR*+@inOzr;Y7FRE_Cbi(OKgg2UMJVcKAGqlJ^r{i$NZgw z$>=t#zP8hwPcovLg64Zl{}*L({{fvdkLcLPgFx+99;2B-rRZTKiH1<9&-|Xdl%55M z6M$$&fWVms0HTK>@OUXYH|RnHL9Enih~yZc_ne z6FF>nx?!ScXErq&Rhg-K&80JIS7JkltO!#(QC50`0DoHOxZ`XXKSfTOG~zA}Pf9%#Y!>k1kJJ`8Fe~{1c9bsnAUVWvBAe zth%0;rHIw2$omcpAWQ61&cCe6Fu10OOUijjve3!?&1?GS>d3H#Qc+n0m4wWvMvD06 zs)g78->&=oz}dptROP}nT!osEHGBEYZtLX$2{&W zz<6=H?h>%n=-ZjB^w*Z@Y-+23d^hK?ie3#X7N)+sOa?X_7WBRDol`Z6L&mi&hy&ym z&^A}h^FTmp>C`=L@-vRO1j|Cmpv+_lt^FqNP7oqEMG!BA0fb==c0;mCn*a@)Ji~XD zzIZ!V(Tb-RXIISB>2VWZHdxVS5lijFaNvR$0_wzC z$UbpcGE49`8dmIupVo$(^kWF#L59}jec`_xZvAzm9dtXe=e%)~S+gc4%%9+mdg?}j zn(8)-k5B6CS&b9C=4Bz(%)@Ws^#b4AxE)?qhEzmzMD`CSNoejna(GXi%vK#UKFvxo z-{>PS#Wne*Iq<(WAXBU=uh3|%fC8mij}aU;vSP#R_C+L z8fk*s^?38zk8Qa0;}OHJC)7S}$`rfRw=InWQ}|rn@pXB{Ii08|wA9Qv;8dB(@@~^C zSZG7dxd_~dCzyt#65+8((m)he;pB~0lQW_@VF*eS$bNp7t;JKE$vEslJjRGtVq82e zexb4Rj=7XO4Va81V3B=3X7)M+LzPFT{K{aq+ZZ zgroQ`tHW?|K#NE+B7U_i53%+0zO=+UPk0_xcb@T(IKotk#~#l|uAZPpRtrz??BnnV zbvH*P5L;HOJfVDQ%O%TWL_51r;xBFbN;(cP2=cFZmRcx`H%{1ny4i?dm*B-WD!hc{ zTb^1pzQ64E+#RV}m}K#F1?hiIMS>{kfZNd(JV(aBm>odtRu zMjtayZ{$KGgk}GV>s~rvwjiN7$J;KIv_U^aZOH2YTnGfD9wUzT)fh)O-MvM1eaPbl z%fz70xH{GQJ__Alq_634UE#%rv@!j1pq428dgFGM|B==$|3oRer)QwGXN2YKuqF|D zaUNWQ#8%l)yn@_n1FM4v;n!er{dQg7r?r4v+=sPj^p16?m+zI$ppEVYXoc0B|_%!Fs{LGf_knFhsB zK}>6BC^W@_6VOhTJ}Wz9S>x<=`R<3o__k##d9R>$c1hw#$OnY)x4KDeIHj1&bYTMz zFqqht$(VsA$Ft0W_T?7=ZEx5GX-WMK@)62E-orR>B!8X#en3UPJBZ-lTRg0of-8XQ-)BGV%B~dk!zt({9S`#~z0La99@aL~Vd^0(Xh)WcDU>%h68(xrGp)2P5YK7! zLK3X(&TVY_gsgfRt7*fJSOTHuG`B~4ccf}xcA~qGZpFxdoOZquGLRkf8hdi=ZnP3q zKb(8r{*8m^Q*XEMM`Q))+;9;|W&kU=r)JW2_rkP-w16pqQ5VD<&NId8(g8*#0gMV- zmFH~_8H+qEj<}0#447hQ!FE3A5Gh^At3D_i|GWw_;+LcQlj8a3A|J~H0kj>uo%-;l zZz8@y7%lvMctHy11llbkI6z>ZIH8^OwD9NLtPaK49tf(F_DQ=O;idkhp~H_ZMjnyt z+K`uYBA>67};@%wzxk-9z~)crmm99o|b z6!%tspAWXQ-cc2!_m#q9r44L{HtqvI{*56-mUn+SIzwEYL-QHBXuHe$>_`prX`b;p z@#vYF0bH(2yI?n%{SG(yDC>M(llC8{`)z)@AwcGr*6gS+SH1zLLxD%&`M=Kx8@%kWm5 z)>1oAuBy25&^JxPWx*(?yys?2ay}`RGm8QqqXMLRqDA4Eq<~LcRz8VPActpGCXL(t zXth&=$p44W2%OUI%&h=Wt$z1Mq7A6iTz{lPo0)hj8IIj&wWy+Vt}6-5+R-Cga~%bG ze7NDJp(?9lJt2ff?K|24Y!m$+I2_geP+I{)d;Wf$CEV`Z%}S@-X)_Y#>cq_GlM80K zkU@Txq5vhQ8-6`0&ucDq4w0~YNO~n?i-?}Zfz<>Kb z?5tkFy9ob|>0mA2DQs-#uY>7l!H7N!O0k(?uRxw1cQvp_R=4fhaE2!4Ui9Zpe64WB zxw@9b7+g#Pzx-TXh}?;rci|nf{H>E?hkjB;kkByAZODK{%Np^dq}31NLA|DVSd>TY zBq!-rJ9RKvBYUl!Yu0Bo)vvkWz5VsKDpKaC6j zRvCX}+8YC0EFoA^|38>^>qT}Z=!sY3#zU{qkpn+D>b%&ODGz)siDwq+*AaJGB_G|?=!XVOi|6kqzS-Ll{%$mW zci5F9ARbz@TGO9ZLHe0*^HFA&vTOA*+1({y`|H|y^0oI*bVsmeQwp|*DQJAsZSRMW zFFN=|;$A(eeSh=Jn$gV@H?QNL-s>^GMCvb#68hy%)PK8MdZ_d51%(~EtwcoleD*1L za!Ww-3&KIqJZaU?vlQc3tet4#TiXJ%pTodIl){7JP+`&rNEFJokslqw3w;saG1*2! z`z>y=OO)#v0F8NqGHj)9s#DDj<)e%#Ikm{)DXI`CXF26-8;?U7TxqNbpc1^3Fgvdy zE7N34>R5546$cc4>A6hj=}qNc9rTS3=bvW^=%~z{fXy8oNK;e5O%_}{*JLGgq;;3e z!H-hm6btj%7OSF@qlR*O89)R$eWZMCK8`LZcQ)4d5;aja_14Rp5IU+5uDAs^xNz=S zjNtz*-O~Tt#|4|yW)UpyL`<^QZ%+_tui62)YZ@ny5EtF|1wjb>UPnE!%``K9k|wB@ zli4w~yO`4~AYUwIrnsUulO#waTr*J6k*0 zo3a_`!l=g#G`^Dl<#xDaxtZvmW-LrfL!iz0P1Uw7cg)5_uY9?EmZJGUaL_$ZVVS%s(~8-Td~}%gO5BC0#&a_I7pJ2F@3+Mfw*m)$J_Ijl z3WGDQ{;V{N71kMf($IcrFZP~W7EHl(XWxltNO(S4)RWk(bTd^cH5I1t)>CZSv{mGi z+Y6kSQXBop)k}N9XGCG$tia(GW2||BEfWX{IrW;_Ng@O)rV~6YHq}CsP~BMX1^m-# zQh3d7#2}UwOV};AT~(~YI$m7}ZLqY0-3{kOJl0bUr1YK@#Tx}S*JSNxiTEqoU7vc>)wx4;Hs(h&<6W9ITj{g4{jkpx}=Lw z_;yI&_E8CJWWu|;;aIaTMqAD#*q8Wkzsy$fPaFP+L*w_H6L61Ps8B!`23V~jX=^ua z0cu$Sg$0nnJuYGe+~YcxUAc(EkwD>R?Mc%+h#QU|Qs^awpmKFv8WsXoNk}9(1#<=} zF}`Zf3+#cORBKJG%>U!zUXk#x4>`ePtt? zw*g%48Nwc?JO!{2VPHMkmKJ4krMM~RDV2QHFwQva{hZdP;X?!dRZ)TZp=(&Dhiqi| zq+es$Hc$}pvWxwC_s_qQ^_-B`A z*w&&>2M>h?v}H}4%u@y7q-!S_e`xsY#Ma+F?LQWoaG&-XSY*O|+R+F8{j}Y|-VIFQ zDuZvgb*}8b(CFlVb5=Qe81(QVBC99W093Wnw2$}am?w*T^+rNsi=o_3X^tIFm4wi0 zyptB~Sq@)LAZB4JZJ@vnle44f%;LdpQqz@$)X+uef)jib?@=wfd{g zet?javNnI*F*dcZwSgLC(OYD+lo~KhIh6FaP7iH-6=`Q9Yt$S5X14Frp$Giwvx9!G z#;Ro_gFj5)z6(_KugN#9WM*s}ihP#5+m2u-?#3fLL)(xfYf&r2lgQ4qaHXJ}kx4l} ztV0I^pPqyI<7uyJTH5P4?$2nO|Bt85t)5BIA}c$y2#ST{+^5ah ztX&@cP;0IojGDMwL*ynKC@;S>M_`%OUJRvsIFrR8&`Mqmm@pUYXnA~s#2qM(PZ!j* zL3;7?;Jm{%)gCj;-S!&xHAbo} zVPK53DwnyM>?ZgASl7kF)eh({tBZ-$DFezhXHOUCED06~R+(2a5#d}qG+k6#pbdSt z*l}lLI6!u3cuzNe;ryIB(FA=Di=lzC?YHknHFPh8N4_VF)Jp&Sg>}F}WNoIU#QxrV z|5ckg#TbZn2b{{_sTslZNHgqyl%I6KqZEdS{TX8YO%tBOf$-(5j^EdJR9dA=?sIppJ^&n^=j&+wCIRE&f<8_hk%}-1rkON9i zPpoe)+Cc>#m~5wjZ(}7z8yCL2L9^RRL}7=Z7>^s|HVt-lF%@|ktnbhjf13~L2N|jR zO-lBRP-Y5pb@$)v2G;wI;!Ww__Kcobs5-a**$$1+`d80Rzi<7+$n|>SzgpiBi!p5N z{l)@5ef02e4~Oo40ndAwcIfA-J0YYb_SHl8Ct^aZmNDH|PnH^05JMM8$9{ec=0DmO zx+ylq_Y#O5f7L)?(0;`&eDQ+TDwqgACbBH?zu!WvI+JN3?LCwA|C{w4%qvGTFVXqr z!x0u*a&E(bs*JQ;1i;2F*`$1md=O`o0o5P zl&M}8j4SK6)gyR(9AGG_*=8}?GIDyL3ZOrOggcOzX@%Qz_%{p{|I?-^l0fj6veiDP z>6(K|yP26*in7Q>%Yl$2q?zK`Vq$xT*Y%~)wpinIMk}@G8-qaGd>7O5dwm_mSW3iT zB`7l$)YrYiZpC%JOBog|jpz`Vx-#|>fP54W_u}gXQEdVzM^W4^pR(F3f zjr{PHwlizZpLYiXhGA97@SmXwsL@OX-u5$o>*+h;s|%5eCB-4xG=8nxk~gcdx36 zDjjPc-x8F7f;<BiCHaJayP%L@@qfDPsj`|Z@0ZJCfyAx zz$aWoydleJhj`oa%7j(Q+VoYBVnICPlhfD0NgLU-9)oHW4d+p~yZ`v=@)aUYp`~-IXBN8VI|stE?i_ zc$UY$-?REFQs~fA>moj!$mVy9P##Hzgh|Ux0528Z>%IoA(!e;HJ{>Y3m(7VOJLV zG^pwq^T4&8t;93-|5#(koi&aAf|vOIxKp^R$ky)^khWW|v0G2u39R2KnDslQD~Wrj zB(L8oqUZqIW_nMNw6`FE7|iKQMuZCP2$yuFm+Z#qxjy!?tKfkgHA(Xkd;N&S+bF$S zC=PnC2t8Jo^7e1+Z*Gc9kwS!394-qv4$$H|&11Ktegl=Df3rDD$IL2UL&zPpbe=V! zs_U)@d#1s(2vW;GS$yvH&IUU+4BKj4DH#lCyUVm(2pL@5OQ!Bc8*wJLd+vxtY+PHK z+VXB#b%#e&=iIgAFL%S|gneE-8*%qaD#-@5xD$1>7hWFg#LbexYT#<{5ap?x-q1H@ z;Io{q@M8*t18t)*)h|1b_|d(>>3Pc4=bj~^7$ry&_;I2CW_h92^!pdJ2IUmHif*c0FgSEAb#e%N$Sca{~bssOn11{ zJnz&Uk3jvF8%alXF}d#BmHwcE84k(>Yw6dcZRSU*{c!8jL2%bE8hReF{)DkF z^alm32%0Z~oGt6u*p9#nUGsbq=O;;aw7F^%PR6R5)+mK!wb&K=$Ok4Oa7{{oe&qj1 zx@ds2FyK)G*5F3&wDkL4g6}1Iv7kgRnotMW0%-yJZWV4hj{HvjRkn5|`}Br7mh;1Y z!XsEJI#c~&HiZt;Ie6<*!RhM0lAV@DfnX0!1&?V2&Za9+b$`y@(iEmzF8&GAIJdmp z>D>j(D?9J%^B#VpG;)o*m*6kc(rX}z)?9^Ig@AGj;B-gdP6CnAAwFT$t#t9uL(^-^ z&Q?h)d#*T#X`#E)U;fzI#%28eDoXsZ50AU_rELW)eTn6++8|S>+nEZ64j@BIc$f}w zxo|QUm&@%akB$&)smRwM4N8(}pqvsR#u+y_U9~TkFY&1GEs4plOg=$9%mSIkeXFg` zuD{hrO@iR9zT_C|zN>JDvA*v}t?N)?y!^AvPB&q*k(H?*{a>7o{)e~v+VL*X`4w2N zzak5MPfAdE$7Pg0b&dJ<>zc67lS<{=B6n}qx9NHm_DW;EoI49FwKUniE8TMabP-IU zpl45jRy%it&ipnTV2sks^-s!JA<~cH47(Jfg;&p}M4q;2gEua{Q_4sx=~}P_J=UDs zU8+09@K24EuAz5*`{r5X8bR>nxHDB+*&aNRKQ1oW1X%9hc>XNk|LZmVu{%S(CSdXp zu;{)kK}PYs-ukXLt*zztH7j=AVAtC#^oaIs*ZQtEwOYG_Aj-!XwJLx;yxCj^?Jr7 zQmTs(&(UrJHyRI0Vq3``*(o=sTHS$(?gGy7(n90Zs;{!%3j;Nfyxz|X!LObL9xOVs z;dba~XQt`c#9gUv9;0{Ss&5ErUtgNerT@3<+IB>cQp7NYKeKm4TG3b9T&^c>kpD6_ z=?O>s(p<2X(Y%3Rgdy-@15{sF;0V8z)nDcd_}? z=qjjL>QB&@$IubOE}ds;4O^#9;a%N3a--EziUn7-0~x3I)C9zH0zwmyDe#VZ4-``C zce$73wuF&Sg|^WLWc_gBGG))Sec+1eB7*0G1r*BJ6?snc?CRps|7&)={qN@EDwrLh za#=@=5Mc0Ie?mE8D-m1sG9b6zx{yqd*P zrhHocNALuQfJw8Guo}yZpKpc1lg`7hhnQSBgS~I%brLJLdd6CCnNO)!NjU$t!n;ZW z{Z=7yN@y3$?DHVDNt||trdo+<@`2vASUbs|j$^Mb5i_OJ10slvFv87)J9gG=iJp(- zMX#JmQf6pLk~veTeOhs`TCI=+UTvuTUIuWG=8N&d&s92VuBZ7N6@T&KH1?YIViRfr zovjrs%esO*70?WkcxV#5V@mAwg%ToxZU7%H6J0d7!=ncn+d(nNS-oQ_W3ve67t+U-btI|47 zebcSzmmFGizPRV$o%8nddyA3CMKqwktaAAh#f8Oq&JWV}M45n;Qe|J>Po_tIDhQc5 z06(JZuQ77q)#Gap_Xp7z|H1(NzC@QM3!0Zy#7bLZkbv=;>n_+DBdX!So$uMFHI7ZZ zUJpd*ecS+Y5MGbycJDTP@3C9wLAa{pUpc;i-EQFDidG>`TEu2(?gfEsN=Le+X{6-K zOgEA`LB8ftbMjFa*Z(TRowz!{tM~Iq> zSF?Y$=K}Wft+r7+U+XLCXH9Hg<`RbUU5}Wk75&4%1-e^{H}xx7`HpmKeZWoMB!AHf zWvb}B&;>mm=kMIy;$ciT*qr#quzvQg=Tp6>-JM%J45*9jn?qq6(ESV&KDdo-%4$v# zSvMmVXk(wXiR2U!DmY;OS=rg`dne~&2diuhK!KOg?zAZ4`ga$qu1&x*jXEPd$h&!9k+(4?7OwSW~IEl03K*$iwK&r z$>WWEd&zrweh=M314j%9rc_izBGV;Db0{<(l{o1dAF0=5l2yjhG}+KO$I|0RO8CIi zql0i?n<`7$ZEj-?(AT5IsbRPm_h^6DYI}6iE6~&e{A+cmNjmGKCj{29$DO-FPu9lB zt#~_KO#AZ}TRR^EPbW`ju7`HS-_A9Cy`35p@rD1Co~@U@NANLc2~WclNxs+S&-iCW zmY7rGG7h+3pW72W^YB>Q{a&#fueKg(+~@otvB$SJt7qxdKX)Up{n+&ZcMBEQ}>A3V6IoM45nH3fB@4e^7Dj#3$Ec@Lz4!Ymet{ z)dTpC#>5zg)~``3iCG&lXY>HT0%H@YJi1BCgPo!$q+q~d(O{X|g2V~L1FW8Ogct%t zrg7}jcIAnHJ%$5ZfF(WrYX2+ZC8vf+WrY50cinNDDfE}E7Z2TZf1VCHPW;F6d?%|m z_&jSln&$bXt9$d`j7?yZf(%A`bP=Ho5=x70$!?uGph0oC7_|N2ah6P@3_eU!n(R}| zB#6eU=uhU=kJS2Krh;w~Dio9bYW;mol*mKh3Jo4teaGDK;M<4Qhg8lbg5mV@z<^&~ zXdm?d^=o7#n+d(4c=S_{m;Q8*;{GdG%za6C8QG2kGf3LoZ2e^CRf?Qn^c?SL_@v^EPEm_Oo%rwGR8O z1B0bcSYPuPGZK@(rruv2yluxqBcF z!K!W?=l+IW-Wa)af5XU<8kG-=niO*3I)k4=-Gvhb!)iW9DM{wp-7k|1BYkC2+ktyS z@zuj!>AtdVRX0AQjlY?(+N;~^H-Do+qA!SH_dc=q_>f7)a<|-6!-z}L@!v-1hq3hj z`eGn|B1=iW&1EGqlM#|~-m}Y_3UFoG><&5A%hba$9}Oj+cq9?XgV9L__O7nfnrBm~ z3~Ci3LCXJkkh}hi#(-b6z+XM8f2`>I5#%z#X3lm}hW;M~aa%OMqIACR2HDdzh+`K3 zkUL&*-J`u|JGUS%r=l~}p;nUXQSD8a0aouB?m}Z3QM4mL>{Yv!MqLVK_fz^)DD197 zj`fX8V-33n#IOseaj~}p-n~+G3Uj63kAew>zVynRt~4Jy1r1*fw^_PqyB>ul#2Rlb+8(J}DOuKf3od(sh2Mk?+}=aN~ww#_#?t+KaXXU-@ErlYe-Xy0%HECZL#m`rY+gtL;wJG5Ps^>o4#Wkopo zZV3W~!xaG{t#$nIim_}fbTq1loz+EU6RbY(C`iIM+j~2w;0u-sigausBd_yGgKMhI z(Ka})uI$s{m{1i}cT3M&m!#r{+Ea>(A#(sed)fwDF#~$p)qcO8cz9ev;$p8S^vM{Heyk4Fg^L8Tq&tGW#zZ>NKbuZh`#oX0+rs0av9nfT$+hTI&Q~;%g zUjYWtubkhi%gmj?;@TF0O`xPAHm;un5f-VXOyLvy17cyDYH0=pmttT$Ek7x85+5j> z!@vn7n#C=`?6MH18Hm1h0yc>V!deNiF2$2uK>UFfaXeX)TX11eYaqm@Jk^QtCJ2hZ z8Rg2<<+B*zP27ln6EyQyeky9_S?2SS>6S&TfwH zKL-)-dTovuq{oWcYEnfUV+F1IS25p6AM`B1*??(3iUwJ+~yeFA-J$W|6_m{&Xz z4G}=T?GxQ7#$KwcT0!hIpb1W%e)0JXhv(x7Edi6^jK$H`2U?7`^oa^HtbKLUQrW&^ z7k2?a*x)T$$9>p4UtE#r!Vr)TEyz2YA-^v232f#}?r~WwA;`5PZYSVGcQX`CgAMMO ztS+UT4adCp(t^Pkt{h1dD}m{A4VyfKJ8@_ELL9|*0j9NfbIWS4!Nb|Cz@u-2nx%_| zvq=W7sc=7mO_=0e@FjXqvLjlds{Sg;s4^F>{U=84AFmg#1^=&}z<*sY2+*UjRs(tz z__?0I$@k=f~3UFgK5u_)wSwsqid+{q*PtFf`KiI7H73}=S26eOdd!n9L@VY zd!Fwd#f=2T)Kd%_&q_F@9Pb*04wa2&KQ-*oesb@68^)CwIe(opGomMir|BU7>`y#sM&HvNh zwTDBY?(1QU8MldXABMRiA#xikYX~!Lg($Tmw=_y+DA7VQE}4%tZN>Zye z?v&(ONacPh3YDx@YpwH*wcFd?r*)ovp68su&iVU$#xwYR-}nCB@BO_mg*B3_{oPW^ zFXV_ZFv}pc(XxFe80=*8@K@Q)))v({Inz9cSHDGIBDAi_Q8khb-IWOU7X7G^Vw;k> zk0g=9e7C3REWLBxC{$=_qlOusosasp}--d{r{{lslhr&EP#O^f4BLhK57RxSF-rzzQN-s#2>mzF%Bf&Aug`@N;(92PLONOcY=Bq$ZS z89?nEXBFhsyFVLt2LIB`&VR z*XXZKP03_WCjhq%ANejpKmu0R0O{{6LJs#;)MY7sVv#J zc2UqcJ!RDyz5H&ftN7m^or(FBqO#tmgv~4d6=KPxVI!JEz|E!7)?&+E(e_}z!ZZNj zaa^P&F(3serxrROM~g>pLHI?ai1Fcx%4M`}m4nB1d>I6v-9?ztN?uX2d}_VDI#OLU zAfJqk#qcSkQ&Gjp>^H^fHrSGxX?XO`P zYcYwIYQY6wc;T=|?&6ju9#tcByLlldB$6|pyG5LjJ|Cr_J$l~m;hyqSX>)tVRj58C z6SzXTIvoH@!Vs*`&|v!_c~X0Ey0cI6mDr2o{+BZqn*B&u&gcAL`4X2zHl$bHoT_+p z{r>XnH#Y?08|MChbY?X!R|r0eK*O@=P;+N^=mRTuY^OTE={=70`24 z_0k328fPegqmvXqEXPUQ+Jt)g}CYQCTW%O~5)? zlCQW;QqCJFHmGO2lSJdv&=!)iJAGLRTYf-6hQcR{FIam^(oK4;-Lq79%L;<>*dE&L_M!0P zhf1B{I#o}L4x{18xmn{QKQYt?s(1EvRlLj87&r1B+cys(6>7tq z`=WJk=v^(Zog1}Vn@{!4S?{l{rL>|XlfJ+MQ>yD6bFD@pNI&{#aM}% z?0d+X>WhJ?oW02$Ec98ICc|`}^}H1fzkX!4jlM4~{AAAkvsslOJ^?dvTY|hWlBPBP za%-*;!}o5;A6EGf!vR_HEf1@!$aX~crDs=Ffh})R>U;_diSsWZU;i_S0xDj7LFmRi z=pCtxx4NCA{H|6=0Afo|Z57LtS)5fJ^h#<4j;?*=+s~rk(e1x1&H1z&TgypEy zjPO=qKHK8QsUnP3lB`9~*+tnPGhg@p6CGNe(+|nQf7(HpLx!9K&shO#C;f*)o@TpU zp`qAMBga$_Rb5?(?h3QLGFfWCkH9s0ul27N*3 zies*h5#BB?)!@*ujxeWA9kX|MMQ5Ushpx_62);St7zf`cX&dD>$QE`V*Bd)OCa{?4_->&bE6Qr zf$*FCD`p1ucTMMC9=Y=A%FqM!AAc!G%{Ja#C6AEXTYvUR)o7*8ypvs$=pd_cO1jlF zlzck2{oqHmaggAC8iu~?`1Ek+jF~D(w|j*HZm*46pBCDqvo{ya&A0~Hv(DcTX^h#l zyd|{eq+He<;>3ri!v)(63hE-%+t&^feal?v@9o)I_Gy^yI@(OdT6|@n9CY{5fx}wH zpXNekf=o?stDCDM58rK_5);C_V%5aGpdzmTn3tz}ZGe7I2=mICRxKm;HUjoZPGt94 z&vx+ozO3bR7BN1?_>e{0_$97QD0qI1;ZWQ^ME&oU%x4RU05>q#I&XU0q zA+LR>G6h(!#^-E!ee3hxThm<%CvtpVsD5n9stlk;=*!j2J+GA*>U_L;`Rz+Vyp-nl z`~NQcgqDLzHfgWZw3->;>+M5V!$bCe41 z!y+5VT_%DR&*Gj;edZ>BiUhS{s;(o|^0Z#G?U(XW&A?c4gmLBY{zdVypFXc_#|j6aAM|YQbcoxRx{xOw z%Y~vdM&KS?+YckpEPG}qP7OLiWe#Xdd@l8iABHAKrw00psni<6jZ%D|hsoUOBWb6@ z3UkMACc&}Ej~JQZjJs~WoKIg5Q5tiQ3;>Io2Hk1;p;}< z#Y70=@pA|4;vri2wbNPPEC!vMVr?dsNK_9?>VtY z@*9WP(=;NwVg*!&?mEehrqbkUfoUmvJuyf zaD;KSk9I19*jZQluDs~w+35nNxb940g3_UxOKh!*nWF!p6UzMUD233p zw$hZ{FTp zh>Lp5S~R#Fr~NUk^ymZ67mtqpjO_aLhU>JW=|cU0K0~j;Im%zZ(RRIbwYy1151S~z z?sls#pUAIvHSt|qY$IlcEVToE{N_pgjc9qSfiOEerqe0h>6BQsu~WfJ$U-On#jkBo zC`o|{+Fpo#AMC0}?tSQWrRsm|^VoglWB-3MKqYqm(<>(2U}Yph`RK|hTl@OTSc>`E zm2vKBBf9(OfA(<5I{JCG{PIy({m`qf$@mJ*`^TP7Pfgd%S5II6A^OK3U;O%Zp$_@O z^BTypFNC z;oqUvS61<#qEporwFODRyU@R-mny^Z3L3Lw+yjLES-)ymJajWvjY<;*#VT0PWh&ivk&HX~^P@O3f z-X|yQ2P*f3x1_jHQ)Drc2`1RpFa2Kw?!QFh-&$DxQNN04KsP{S{)Cp~HmLS?bvWZQ zLU<&_Q`52a^$p_b0t#$g;Z~b#Cr3tmdj@U1?NWPEB0blY5ocFGjonUOA;%?`B&JRz z%$(EV;i>7{&7u{O#ih1z(2EYlP}>~a6sEX$`Fm2+6&q!(rE!IBPxijv=;qgy?Gh@6 zp`jghvhLZk$Gh->Si}2GYA9;d0sHOS`iVR4=-3|Nv!jLvsks8TNbI+ejmz)A9N|VF(qW8oSTd-!D(g2931tcx$NU_z0iBb z7p|*5fqZS&w5?&+g8D;a<2J^m%|^BPosmSMK?DkBy~KXO3eLoOcDRyPE)gjvl=FpS zg$q=;lKOpxfnICu=IE+}C|l{6ewv08jHE)0nBcX!Xj!=TI5b*#TykA|(0}XCz1k7E zLLd-n2o(IewII!dqy0x--uF_A4e|2mx>yU`X>+&Tw(qGvfyCOUL|*^iJY_As-3~sA zguuW@sgPz7GEYpYLgvko1%=d^zm>J?&-Atpvk)vHSd39g(1p={(zi{ zk~e?CiVrV;O0%r#Jm7e7{gV@KcJ&@uqxtyD@B6KV)w&r3b#vfr-TY1J3ReS8!L8lJ zNl0w{=oq%Uk(xTfwCHsmv16-tSIxe!;$g^)vBFycptv{F$B1YzM0-a(7hPxDcCL_M zp}jpMd(lWiMp0M23cH8hBB5w=@YWZ^`i`B7q@FEtw}|Ck@;F-Thhm#E$*Q7vMID-& z-Rdhmh_~k)cKbdqGbUPu5Va2%*=RFq6@#rJujD+etcXsf>269Kw5fl_%to0ak4D*# z@}lcrmBOX;ReAOy-gPM|7KhNqZhO&OHHNot@W_5eib0u-d`;LZ6R~xdFGTS+3^|Y% z6yoHb`SzkXntAze1v*pqkso*IGg@y8>{Cc?e;g5wGMcqNO5iJhe^WQGGsff51c4~y6As&BRu6a55$aIk+mfx`<=`<9ZSrjI-9dj#<8Gd&vxn@ z^!8v&rAmBIv$a1_=7r}9%CexbSS<~EKjP&9JIT>YN_&DXk+_7|5H_5!If~AWJ%x*W zdh$LevlXRlPyXJI-$WkXCh;JWU>~MO+Mjr`WEz4sIc}E4gYTIZ5cK%w^2DQ7sE4R_ z7nr-Jp9Ko5p+rIj>@z>IURJS1MtmFE|M%v7JbKvy>S^PFdTL-8_UXuJR+odFM#KQC zb(f!Be;e_jQCybSzC^=U=RF-J^9%A64+U1lq$(*)h(lGqJYAJK1PYQqdA>G2S434x zD0&MVw}{!8TT_JCHl_~CM8JG*eadL4{CG09M<0(Q-^#@JJ3OTwn6VGa-lkYRa|Msp zk87)_i)!NHq1HA>uRYgje|H@7yG~FoY;8yUt-4&@VBcC;Em-09kzU72%Hu&02EZnP-5D~E9q#*MaD&($w>mSyxe{&0HZjHMoE~U0b zR8coUENKUxgMzsqM=ZKwLFF@>;@$i;Oc*E`y}&aQf)okmF-AgFnKcI-rc8#vn4$v; zmpCXW+ToCBv7?;q_JLO*?`pmxmdxNws_F(2!-yVlirJ{@XcF0|V@IZ0uccQlQ7qr~ z65Yge=5oUD+5M!J1B*30yUTW#kSucp zKFMysmE(lAA-;;)D-e?kZkL89`j&89nyp9q=9-gwZui0_?ZYC?A6jf1^;2R|b_QXz z#4sv`T<-A-QOmS~fYe9pAF4cwxL=EV^GBH*KK0luso5@Wa#MNl5;rFy=qFn{j>i7% zWyS>c%sQOONx`!E1UFuKVv=i5z)E`h z5X35Eo9qq;E7?n&WM*~8FZBml`}GuNLIs)63ZysbS?5bT zIogi$k+K$edmB?FA00#2Zcl<}=_T5Q!>RVqb0lsr2X4oyBD0nu)^egiTCs(B+<;eDH?pRi+Q6pZ@#GP>#sg7_*~aQ<)aNFY~n;;n$3IvXd^!@s}@ zxk=G6>R_8V1XqQ2={QTU%nlkJ$?)VikdPXQMMlLtx=93iH0Bo?Up+Hm&#h~gy>h#5 z^3|=EHvRbIxe516IfP%ui`NQKYpT*ezRXIWL zQSzx-8MVFiIfSl{%&!i?wlL{TZi9e=ntmjKb#C<65Q!I0$+iwo;rd12d_hf@jFD^5 za%Nk3t98#KFymQ7k1t!!{*-&rQqcnGxcT*4({8V8HXJMW=*-It_CBdT&(dBQdKD_y zGh8()GN*c+vrA27ojgLck|aH{JrUHq)-(MSpbB9@Nvl0xdOk|}tm%sy0=~yJzR@IX?*-hAS1jC1GZaq~JXd9tFOF8FcW8q3!^P>FU Ns+zwnj&Ck({tw$v8WR8j literal 0 HcmV?d00001 From af61b68c072d8ff60499aba4f9543adfd9e777f4 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 13 Apr 2021 10:43:42 +0200 Subject: [PATCH 19/50] Fixed color in xacros Signed-off-by: ahcorde --- .../urdf/test_cart_effort.xacro.urdf | 27 +++++++++++----- .../urdf/test_cart_position.xacro.urdf | 31 ++++++++++++------- .../urdf/test_cart_velocity.xacro.urdf | 27 +++++++++++----- 3 files changed, 60 insertions(+), 25 deletions(-) diff --git a/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf index 5cef6feb..46f8f30e 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf @@ -11,9 +11,6 @@ - - - @@ -26,9 +23,6 @@ - - - @@ -64,11 +58,30 @@ + + + + 0 0.8 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + + + + + 0 0 0.8 1 + 0 0 0.8 1 + 0 0 0.8 1 + + + + $(find ignition_ros2_control_demos)/config/cartpole_controller_effort.yaml - diff --git a/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf index f9967a18..14552d37 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -4,10 +4,6 @@ - - - - @@ -15,9 +11,6 @@ - - - @@ -30,9 +23,6 @@ - - - @@ -68,11 +58,30 @@ + + + + 0 0.8 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + + + + + 0 0 0.8 1 + 0 0 0.8 1 + 0 0 0.8 1 + + + + $(find ignition_ros2_control_demos)/config/cartpole_controller_position.yaml - diff --git a/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index 25f9eb63..618f0fc8 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -11,9 +11,6 @@ - - - @@ -26,9 +23,6 @@ - - - @@ -64,11 +58,30 @@ + + + + 0 0.8 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + + + + + 0 0 0.8 1 + 0 0 0.8 1 + 0 0 0.8 1 + + + + $(find ignition_ros2_control_demos)/config/cartpole_controller_velocity.yaml - From 57b34fe7fad25456d2544adb70a58a3660ff66f5 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 15 Apr 2021 22:53:32 +0200 Subject: [PATCH 20/50] Added missed dependency Signed-off-by: ahcorde --- ignition_ros2_control_demos/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ignition_ros2_control_demos/package.xml b/ignition_ros2_control_demos/package.xml index 12dc4684..04642887 100644 --- a/ignition_ros2_control_demos/package.xml +++ b/ignition_ros2_control_demos/package.xml @@ -31,6 +31,7 @@ launch_ros rclcpp robot_state_publisher + ros_ign_gazebo std_msgs velocity_controllers xacro From d7451adda6a472fc534fe31bf5026cc411cc4a75 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 15 Apr 2021 22:59:08 +0200 Subject: [PATCH 21/50] Added missed dependency Signed-off-by: ahcorde --- ignition_ros2_control_demos/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ignition_ros2_control_demos/package.xml b/ignition_ros2_control_demos/package.xml index 04642887..abbead9f 100644 --- a/ignition_ros2_control_demos/package.xml +++ b/ignition_ros2_control_demos/package.xml @@ -32,6 +32,7 @@ rclcpp robot_state_publisher ros_ign_gazebo + ros2controlcli std_msgs velocity_controllers xacro From d7991fe8adb43c2bb24b903a788ef1f17898d608 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 16 Apr 2021 10:09:37 +0200 Subject: [PATCH 22/50] use string option instaed of topic Signed-off-by: ahcorde --- .../launch/cart_example_effort.launch.py | 2 +- .../launch/cart_example_position.launch.py | 2 +- .../launch/cart_example_velocity.launch.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ignition_ros2_control_demos/launch/cart_example_effort.launch.py b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py index abb01136..5ee0003c 100644 --- a/ignition_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py @@ -55,7 +55,7 @@ def generate_launch_description(): package='ros_ign_gazebo', executable='create', output='screen', - arguments=['-topic', 'robot_description', + arguments=['-string', doc.toxml(), '-name', 'cartpole', '-allow_renaming', 'true'], ) diff --git a/ignition_ros2_control_demos/launch/cart_example_position.launch.py b/ignition_ros2_control_demos/launch/cart_example_position.launch.py index 13e2d62f..35fcf347 100644 --- a/ignition_ros2_control_demos/launch/cart_example_position.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_position.launch.py @@ -55,7 +55,7 @@ def generate_launch_description(): package='ros_ign_gazebo', executable='create', output='screen', - arguments=['-topic', 'robot_description', + arguments=['-string', doc.toxml(), '-name', 'cartpole', '-allow_renaming', 'true'], ) diff --git a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py index 09846af9..c33ae66e 100644 --- a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -55,7 +55,7 @@ def generate_launch_description(): package='ros_ign_gazebo', executable='create', output='screen', - arguments=['-topic', 'robot_description', + arguments=['-string', doc.toxml(), '-name', 'cartpole', '-allow_renaming', 'true'], ) From a5e092006e829e3b08c6a721dde72c6ef348afac Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 16 Apr 2021 10:14:08 +0200 Subject: [PATCH 23/50] Added Dockerfile Signed-off-by: ahcorde --- Dockerfile/Dockerfile | 44 ++++++++++++++++++++++++++++++++++++++++ Dockerfile/entrypoint.sh | 5 +++++ 2 files changed, 49 insertions(+) create mode 100644 Dockerfile/Dockerfile create mode 100755 Dockerfile/entrypoint.sh diff --git a/Dockerfile/Dockerfile b/Dockerfile/Dockerfile new file mode 100644 index 00000000..56239c29 --- /dev/null +++ b/Dockerfile/Dockerfile @@ -0,0 +1,44 @@ +FROM osrf/ros:foxy-desktop + +# Make sure everything is up to date before building from source +RUN apt-get update \ + && apt-get upgrade -y \ + && apt-get update && apt-get install -q -y --no-install-recommends \ + dirmngr \ + gnupg2 \ + lsb-release \ + wget \ + curl \ + && apt-get clean + +RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \ + sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-prerelease `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-prerelease.list' && \ + sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' && \ + wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - && \ + sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' && \ + curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - && \ + apt-get update && apt-get install -q -y --no-install-recommends \ + python3-colcon-ros \ + python3-colcon-common-extensions \ + python3-rosdep \ + libignition-gazebo5-dev \ + libignition-plugin-dev \ + libignition-physics4-dev \ + libignition-utils1-cli-dev \ + && apt-get clean + +RUN mkdir -p /home/ros2_ws/src \ + && cd /home/ros2_ws/src \ + && git clone https://github.com/ignitionrobotics/ign_ros2_control/ -b ahcorde/ign_ros2_control \ + && rosdep update \ + && rosdep install --from-paths ./ -i -y --rosdistro foxy \ + --ignore-src --skip-keys "ignition-gazebo5 ignition-physics4 ignition-plugin1" + +RUN cd /home/ros2_ws/ \ + && . /opt/ros/foxy/setup.sh \ + && colcon build --merge-install + +COPY entrypoint.sh /entrypoint.sh +ENTRYPOINT ["/entrypoint.sh"] + +CMD ros2 launch ignition_ros2_control_demos cart_example_position.launch.py diff --git a/Dockerfile/entrypoint.sh b/Dockerfile/entrypoint.sh new file mode 100755 index 00000000..7a4cfd84 --- /dev/null +++ b/Dockerfile/entrypoint.sh @@ -0,0 +1,5 @@ +#!/bin/sh + +. /opt/ros/foxy/setup.sh +. /home/ros2_ws/install/setup.sh +exec "$@" From 7e28cf27789db05852aba0f1ca759677b91c2a0a Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 16 Apr 2021 10:14:48 +0200 Subject: [PATCH 24/50] Improved README.md Signed-off-by: ahcorde --- README.md | 33 +++++++++++++++------------------ 1 file changed, 15 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index 119aff37..34ba3b70 100644 --- a/README.md +++ b/README.md @@ -1,20 +1,17 @@ # ign_ros2_control This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Ignition Robotics Gazebo](http://ignitionrobotics.org/) simulator. +More information about `ros2_control` can be found here: https://ros-controls.github.io/control.ros.org/ -This package provides a Ignition Gazebo system plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model. +This package provides an Ignition Gazebo system plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model. Tested on: - - ROS 2 Foxy - - Ignition Edifice + - [ROS 2 Foxy](https://docs.ros.org/en/foxy/Installation.html) + - [Ignition Edifice](https://ignitionrobotics.org/docs/edifice) # Usage -This repository contains the contents for testing ign_ros2_control - -It is running Gazebo and some other ROS 2 nodes. - ## Video + Pictures ![](img/ign_ros2_control.gif) @@ -32,23 +29,23 @@ docker build -t ignition_ros2_control . #### Using Docker -Docker allows us to run the demo without GUI if we don't configure it properly. The following command runs the demo without GUI: +Docker allows us to run the demo without the GUI if configured properly. The following command runs the demo without the GUI: ```bash docker run -it --rm --name ignition_ros2_control_demo --net host ignition_ros2_control ros2 launch ignition_ros2_control_demos cart_example_position.launch.py gui:=false ``` -The in your local machine you can run the Gazebo client: +Then on your local machine, you can run the Gazebo client: ```bash -gzclient +ign gazebo -g ``` #### Using Rocker -To run the demo with GUI we are going to use [rocker](https://github.com/osrf/rocker/) which is a tool to run docker -images with customized local support injected for things like nvidia support. And user id specific files for cleaner -mounting file permissions. You can install this tool with the following [instructions](https://github.com/osrf/rocker/#installation). +To run the demo with the GUI, we are going to use [rocker](https://github.com/osrf/rocker/), which is a tool to run docker +images with customized local support injected for things like nvidia support. Rocker also supports user id specific files for cleaner +mounting file permissions. You can install this tool with the following [instructions](https://github.com/osrf/rocker/#installation) (make sure you meet all of the [prerequisites](https://github.com/osrf/rocker/#prerequisites)). The following command will launch Gazebo: @@ -56,7 +53,7 @@ The following command will launch Gazebo: rocker --x11 --nvidia --name ignition_ros2_control_demo ignition_ros2_control:latest ``` -The following commands allow to move the cart in the rail: +The following commands allow the cart to be moved along the rail: ```bash docker exec -it ignition_ros2_control_demo bash @@ -68,7 +65,7 @@ ros2 run ignition_ros2_control_demos example_position 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 +include: - a specific `` for our robot - `` tag including the robot controllers: commands and states. @@ -126,7 +123,7 @@ The default behavior provides the following ros2_control interfaces: The `ignition_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 `ignition_ros2_control::GazeboSystemInterface` which implements a simulated `ros2_control` +These plugins must inherit the `ignition_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 @@ -182,7 +179,7 @@ cart_pole_controller: There are some examples in the `ignition_ros2_control_demos` package. These examples allow to launch a cart in a 30 meter rail. -You can run some of the configuration running the following commands: +You can run some of the example configurations by running the following commands: ```bash ros2 launch ignition_ros2_control_demos cart_example_position.launch.py @@ -192,7 +189,7 @@ ros2 launch ignition_ros2_control_demos cart_example_effort.launch.py Send example commands: -When the Gazebo world is launched you can run some of the following commads to move the cart. +When the Gazebo world is launched, you can run some of the following commands to move the cart. ```bash ros2 run ignition_ros2_control_demos example_position From 3efa1a91ab057fff7a33661c845f827405bb9bfd Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 23 Apr 2021 12:29:57 +0200 Subject: [PATCH 25/50] Removed ignition-physics dependency and other minor fixes Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 6 ++---- Dockerfile/Dockerfile | 5 +---- README.md | 2 +- ignition_ros2_control/CMakeLists.txt | 4 ---- ignition_ros2_control/package.xml | 1 - ignition_ros2_control/src/ignition_system.cpp | 8 -------- 6 files changed, 4 insertions(+), 22 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 2794e53b..fa975d6d 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -22,12 +22,10 @@ jobs: mkdir -p /home/ros2_ws/src cp -r ign_ros2_control /home/ros2_ws/src/ sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-prerelease `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-prerelease.list' - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - - IGN_DEPS="libignition-gazebo5-dev libignition-plugin-dev libignition-physics4-dev libignition-utils1-cli-dev" + IGN_DEPS="libignition-gazebo5-dev libignition-plugin-dev libignition-utils1-cli-dev" apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -qq -y \ dirmngr \ @@ -42,7 +40,7 @@ jobs: rosdep init rosdep update rosdep install --from-paths ./ -i -y --rosdistro foxy \ - --ignore-src --skip-keys "ignition-gazebo5 ignition-physics4 ignition-plugin1" + --ignore-src --skip-keys "ignition-gazebo5 ignition-plugin1" - name: Build project id: build run: | diff --git a/Dockerfile/Dockerfile b/Dockerfile/Dockerfile index 56239c29..fca0edeb 100644 --- a/Dockerfile/Dockerfile +++ b/Dockerfile/Dockerfile @@ -12,8 +12,6 @@ RUN apt-get update \ && apt-get clean RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \ - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-prerelease `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-prerelease.list' && \ - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-nightly `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-nightly.list' && \ wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - && \ sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' && \ curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - && \ @@ -23,7 +21,6 @@ RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb python3-rosdep \ libignition-gazebo5-dev \ libignition-plugin-dev \ - libignition-physics4-dev \ libignition-utils1-cli-dev \ && apt-get clean @@ -32,7 +29,7 @@ RUN mkdir -p /home/ros2_ws/src \ && git clone https://github.com/ignitionrobotics/ign_ros2_control/ -b ahcorde/ign_ros2_control \ && rosdep update \ && rosdep install --from-paths ./ -i -y --rosdistro foxy \ - --ignore-src --skip-keys "ignition-gazebo5 ignition-physics4 ignition-plugin1" + --ignore-src --skip-keys "ignition-gazebo5 ignition-plugin1" RUN cd /home/ros2_ws/ \ && . /opt/ros/foxy/setup.sh \ diff --git a/README.md b/README.md index 34ba3b70..5af58ab8 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # ign_ros2_control -This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Ignition Robotics Gazebo](http://ignitionrobotics.org/) simulator. +This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Ignition Gazebo](http://ignitionrobotics.org/) simulator. More information about `ros2_control` can be found here: https://ros-controls.github.io/control.ros.org/ This package provides an Ignition Gazebo system plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model. diff --git a/ignition_ros2_control/CMakeLists.txt b/ignition_ros2_control/CMakeLists.txt index efc3fba6..f22eaacd 100644 --- a/ignition_ros2_control/CMakeLists.txt +++ b/ignition_ros2_control/CMakeLists.txt @@ -32,9 +32,6 @@ set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) find_package(ignition-plugin1 REQUIRED) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) -find_package(ignition-physics4 REQUIRED) -set(IGN_PHYSICS_VER ${ignition-physics4_VERSION_MAJOR}) - include_directories(include) add_library(${PROJECT_NAME}-system SHARED @@ -43,7 +40,6 @@ add_library(${PROJECT_NAME}-system SHARED target_link_libraries(${PROJECT_NAME}-system ignition-gazebo${IGN_GAZEBO_VER}::core - ignition-physics${IGN_PHYSICS_VER}::core ignition-plugin${IGN_PLUGIN_VER}::register ) ament_target_dependencies(${PROJECT_NAME}-system diff --git a/ignition_ros2_control/package.xml b/ignition_ros2_control/package.xml index 4d8eac89..c8ac00a8 100644 --- a/ignition_ros2_control/package.xml +++ b/ignition_ros2_control/package.xml @@ -12,7 +12,6 @@ angles ament_index_cpp ignition-gazebo5 - ignition-physics4 ignition-plugin1 pluginlib rclcpp diff --git a/ignition_ros2_control/src/ignition_system.cpp b/ignition_ros2_control/src/ignition_system.cpp index 52a6d921..5c7ee234 100644 --- a/ignition_ros2_control/src/ignition_system.cpp +++ b/ignition_ros2_control/src/ignition_system.cpp @@ -120,14 +120,6 @@ bool IgnitionSystem::initSim( this->dataPtr->joint_vel_cmd_.resize(this->dataPtr->n_dof_); this->dataPtr->joint_eff_cmd_.resize(this->dataPtr->n_dof_); - // ignition::physics::PhysicsEnginePtr physics = ignition::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 Ignition."); - // return false; - // } - if (this->dataPtr->n_dof_ == 0) { RCLCPP_WARN_STREAM(this->nh_->get_logger(), "There is not joint available "); return false; From bfdc61cc0bba2669b1b668a273ea0fcd6eb63fd7 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 14 May 2021 16:09:24 +0200 Subject: [PATCH 26/50] Added some of the feedback Signed-off-by: ahcorde --- ignition_ros2_control/CMakeLists.txt | 15 +++- .../ignition_ros2_control_plugin.hpp | 16 +--- .../ignition_ros2_control/ignition_system.hpp | 2 - .../ignition_system_interface.hpp | 42 ++++++++--- ignition_ros2_control/package.xml | 6 +- .../src/ignition_ros2_control_plugin.cpp | 74 ++++++++----------- ignition_ros2_control/src/ignition_system.cpp | 38 +++++----- .../examples/example_effort.cpp | 11 ++- .../examples/example_position.cpp | 12 +-- .../examples/example_velocity.cpp | 14 ++-- .../launch/cart_example_effort.launch.py | 2 +- .../launch/cart_example_velocity.launch.py | 2 +- 12 files changed, 115 insertions(+), 119 deletions(-) diff --git a/ignition_ros2_control/CMakeLists.txt b/ignition_ros2_control/CMakeLists.txt index f22eaacd..735096fd 100644 --- a/ignition_ros2_control/CMakeLists.txt +++ b/ignition_ros2_control/CMakeLists.txt @@ -18,7 +18,6 @@ endif() # Find dependencies find_package(ament_cmake REQUIRED) find_package(ament_index_cpp REQUIRED) -find_package(angles REQUIRED) find_package(controller_manager REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) @@ -26,8 +25,17 @@ find_package(rclcpp REQUIRED) find_package(urdf REQUIRED) find_package(yaml_cpp_vendor REQUIRED) -find_package(ignition-gazebo5 REQUIRED) -set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) +if("$ENV{IGNITION_VERSION}" STREQUAL "citadel") + find_package(ignition-gazebo3 REQUIRED) + set(IGN_GAZEBO_VER ${ignition-gazebo3_VERSION_MAJOR}) + + message(STATUS "Compiling against Ignition Citadel") +else() + find_package(ignition-gazebo5 REQUIRED) + set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) + + message(STATUS "Compiling against Ignition Edifice") +endif() find_package(ignition-plugin1 REQUIRED) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) @@ -58,7 +66,6 @@ add_library(ignition_hardware_plugins SHARED src/ignition_system.cpp ) ament_target_dependencies(ignition_hardware_plugins - angles hardware_interface rclcpp ) diff --git a/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp b/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp index 2dc2dd39..5acc5b64 100644 --- a/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp +++ b/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp @@ -15,9 +15,9 @@ #ifndef IGNITION_ROS2_CONTROL__IGNITION_ROS2_CONTROL_PLUGIN_HPP_ #define IGNITION_ROS2_CONTROL__IGNITION_ROS2_CONTROL_PLUGIN_HPP_ -#include +#include -#include "ignition/gazebo/System.hh" +#include namespace ignition_ros2_control { @@ -29,19 +29,14 @@ class IgnitionROS2ControlPlugin public ignition::gazebo::ISystemConfigure, public ignition::gazebo::ISystemUpdate { - /// \brief Constructor - public: + /// \brief Constructor IgnitionROS2ControlPlugin(); /// \brief Destructor - -public: ~IgnitionROS2ControlPlugin() override = default; // Documentation inherited - -public: void Configure( const ignition::gazebo::Entity & _entity, const std::shared_ptr & _sdf, @@ -49,15 +44,12 @@ class IgnitionROS2ControlPlugin ignition::gazebo::EventManager & _eventMgr) override; // Documentation inherited - -public: void Update( const ignition::gazebo::UpdateInfo & _info, ignition::gazebo::EntityComponentManager & _ecm) override; - /// \brief Private data pointer. - private: + /// \brief Private data pointer. std::unique_ptr dataPtr; }; } // namespace ignition_ros2_control diff --git a/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp b/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp index 2a585a7d..00dc112f 100644 --- a/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp +++ b/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp @@ -21,8 +21,6 @@ #include #include -#include "angles/angles.h" - #include "ignition_ros2_control/ignition_system_interface.hpp" namespace ignition_ros2_control diff --git a/ignition_ros2_control/include/ignition_ros2_control/ignition_system_interface.hpp b/ignition_ros2_control/include/ignition_ros2_control/ignition_system_interface.hpp index eac99bb0..0dbd1ec9 100644 --- a/ignition_ros2_control/include/ignition_ros2_control/ignition_system_interface.hpp +++ b/ignition_ros2_control/include/ignition_ros2_control/ignition_system_interface.hpp @@ -16,22 +16,39 @@ #ifndef IGNITION_ROS2_CONTROL__IGNITION_SYSTEM_INTERFACE_HPP_ #define IGNITION_ROS2_CONTROL__IGNITION_SYSTEM_INTERFACE_HPP_ +#include + +#include +#include +#include + +#include + #include #include #include #include -#include "ignition/gazebo/System.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" - namespace ignition_ros2_control { +/// \brief This class allows us to handle flags easily, instead of using strings +/// +/// For example +/// enum ControlMethod_ +/// { +/// NONE = 0, +/// POSITION = (1 << 0), +/// VELOCITY = (1 << 1), +/// EFFORT = (1 << 2), +/// }; +/// typedef SafeEnum ControlMethod; +/// +/// ControlMethod foo; +/// foo |= POSITION // Foo has the position flag active +/// foo & POSITION -> True // Check if position is active in the flag +/// foo & VELOCITY -> False // Check if velocity is active in the flag + template::type> class SafeEnum { @@ -60,10 +77,11 @@ class IgnitionSystemInterface { public: /// \brief Initilize the system interface - /// param[in] model_nh - /// param[in] joints - /// param[in] hardware_info - /// param[in] _ecm + /// param[in] model_nh Pointer to the ros2 node + /// param[in] joints Map with the name of the joint as the key and the value is + /// related with the entity in Gazebo + /// param[in] hardware_info structure with data from URDF. + /// param[in] _ecm Entity-component manager. virtual bool initSim( rclcpp::Node::SharedPtr & model_nh, std::map & joints, diff --git a/ignition_ros2_control/package.xml b/ignition_ros2_control/package.xml index c8ac00a8..8d1e9c8b 100644 --- a/ignition_ros2_control/package.xml +++ b/ignition_ros2_control/package.xml @@ -2,16 +2,16 @@ ignition_ros2_control 0.0.0 - Lol + Ignition ROS2 control package. It allows to control joint using ROS 2 Alejandro Hernández Alejandro Hernández Apache 2 ament_cmake - angles ament_index_cpp - ignition-gazebo5 + ignition-gazebo3 + ignition-gazebo5 ignition-plugin1 pluginlib rclcpp diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index d1acd27d..8ffbe180 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -12,47 +12,45 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include + #include #include #include #include #include -#include "controller_manager/controller_manager.hpp" - -#include "hardware_interface/resource_manager.hpp" -#include "hardware_interface/component_parser.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" - - #include "ignition_ros2_control/ignition_ros2_control_plugin.hpp" #include "ignition_ros2_control/ignition_system.hpp" -#include "ignition/gazebo/components/Joint.hh" -#include "ignition/gazebo/components/JointType.hh" -#include "ignition/gazebo/components/Name.hh" -#include "ignition/gazebo/components/ParentEntity.hh" -#include "ignition/gazebo/components/Physics.hh" -#include "ignition/gazebo/components/World.hh" -#include "ignition/gazebo/Model.hh" - -#include "ignition/plugin/Register.hh" - -#include "pluginlib/class_loader.hpp" - -#include "rclcpp/rclcpp.hpp" - namespace ignition_ros2_control { ////////////////////////////////////////////////// class IgnitionROS2ControlPluginPrivate { - /// \brief Entity ID for sensor within Gazebo. - public: + /// \brief Entity ID for sensor within Gazebo. ignition::gazebo::Entity entity_; -public: + // Node Handles std::shared_ptr node; // Thread where the executor will sping @@ -71,9 +69,10 @@ class IgnitionROS2ControlPluginPrivate // Controller manager std::shared_ptr controller_manager_; - // String with the robot description + // String with the robot description param_name // TODO(ahcorde): Add param in plugin tag std::string robot_description_ = "robot_description"; + // String with the name of the node that contains the robot_description // TODO(ahcorde): Add param in plugin tag std::string robot_description_node_ = "robot_state_publisher"; @@ -107,7 +106,6 @@ class IgnitionROS2ControlPluginPrivate // search and wait for robot_description on param server while (urdf_string.empty()) { - std::string search_param_name; RCLCPP_DEBUG(node->get_logger(), "param_name %s", param_name.c_str()); try { @@ -124,12 +122,11 @@ class IgnitionROS2ControlPluginPrivate } else { RCLCPP_ERROR( node->get_logger(), "ignition_ros2_control plugin is waiting for model" - " URDF in parameter [%s] on the ROS param server.", search_param_name.c_str()); + " URDF in parameter [%s] on the ROS param server.", param_name.c_str()); } usleep(100000); } - RCLCPP_INFO( - node->get_logger(), "Recieved urdf from param server, parsing..."); + RCLCPP_INFO(node->get_logger(), "Received URDF from param server"); return urdf_string; } @@ -146,8 +143,7 @@ class IgnitionROS2ControlPluginPrivate std::vector enabledJoints; // Get all available joints - std::vector jointEntities; - jointEntities = _ecm.ChildrenByComponents(_entity, ignition::gazebo::components::Joint()); + auto jointEntities = _ecm.ChildrenByComponents(_entity, ignition::gazebo::components::Joint()); // Iterate over all joints and verify whether they can be enabled or not for (const auto & jointEntity : jointEntities) { @@ -224,7 +220,7 @@ void IgnitionROS2ControlPlugin::Configure( std::string paramFileName = _sdf->Get("parameters"); if (paramFileName.empty()) { - ignerr << "Ignition ros2 controle found an empty parameters file. " << + ignerr << "Ignition ros2 control found an empty parameters file. " << "Failed to initialize."; return; } @@ -250,7 +246,7 @@ void IgnitionROS2ControlPlugin::Configure( }; this->dataPtr->thread_executor_spin_ = std::thread(spin); - RCLCPP_ERROR_STREAM( + RCLCPP_DEBUG_STREAM( this->dataPtr->node->get_logger(), "[Ignition ROS 2 Control] Setting up controller for [" << "(Entity=" << _entity << ")].\n"); @@ -277,9 +273,6 @@ void IgnitionROS2ControlPlugin::Configure( return; } - RCLCPP_ERROR_STREAM( - this->dataPtr->node->get_logger(), "getURFD\n"); - std::unique_ptr resource_manager_ = std::make_unique(); @@ -292,12 +285,10 @@ void IgnitionROS2ControlPlugin::Configure( RCLCPP_ERROR( this->dataPtr->node->get_logger(), "Failed to create robot simulation interface loader: %s ", ex.what()); + return; } - RCLCPP_ERROR_STREAM( - this->dataPtr->node->get_logger(), "pluginlib\n"); - - for (unsigned int i = 0; i < control_hardware.size(); i++) { + 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( this->dataPtr->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); @@ -315,9 +306,6 @@ void IgnitionROS2ControlPlugin::Configure( resource_manager_->import_component(std::move(gazeboSystem)); } - RCLCPP_ERROR_STREAM( - this->dataPtr->node->get_logger(), "initSim\n"); - // Create the controller manager RCLCPP_INFO(this->dataPtr->node->get_logger(), "Loading controller_manager"); this->dataPtr->controller_manager_.reset( diff --git a/ignition_ros2_control/src/ignition_system.cpp b/ignition_ros2_control/src/ignition_system.cpp index 5c7ee234..6a080e63 100644 --- a/ignition_ros2_control/src/ignition_system.cpp +++ b/ignition_ros2_control/src/ignition_system.cpp @@ -12,6 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include +#include +#include +#include + #include #include #include @@ -20,13 +27,6 @@ #include "ignition_ros2_control/ignition_system.hpp" -#include "ignition/gazebo/components/JointForce.hh" -#include "ignition/gazebo/components/JointForceCmd.hh" -#include "ignition/gazebo/components/JointPosition.hh" -#include "ignition/gazebo/components/JointPositionReset.hh" -#include "ignition/gazebo/components/JointVelocity.hh" -#include "ignition/gazebo/components/JointVelocityCmd.hh" - class ignition_ros2_control::IgnitionSystemPrivate { public: @@ -167,20 +167,18 @@ bool IgnitionSystem::initSim( 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++) { + 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") { + } else 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") { + } else 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( @@ -191,7 +189,7 @@ bool IgnitionSystem::initSim( 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++) { + 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_state_[j] = std::make_shared( @@ -226,21 +224,21 @@ IgnitionSystem::export_state_interfaces() { std::vector state_interfaces; - for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); 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_POSITION, &this->dataPtr->joint_position_[i])); } - for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); 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++) { + for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); ++i) { state_interfaces.emplace_back( hardware_interface::StateInterface( this->dataPtr->joint_names_[i], @@ -255,21 +253,21 @@ IgnitionSystem::export_command_interfaces() { std::vector command_interfaces; - for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); 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_POSITION, &this->dataPtr->joint_position_cmd_[i])); } - for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); 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++) { + for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); ++i) { command_interfaces.emplace_back( hardware_interface::CommandInterface( this->dataPtr->joint_names_[i], @@ -368,7 +366,7 @@ hardware_interface::return_type IgnitionSystem::write() } } - // // Get the simulation time and period + // Get the simulation time and period // ignition::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_; diff --git a/ignition_ros2_control_demos/examples/example_effort.cpp b/ignition_ros2_control_demos/examples/example_effort.cpp index dd4bff8b..579ef941 100644 --- a/ignition_ros2_control_demos/examples/example_effort.cpp +++ b/ignition_ros2_control_demos/examples/example_effort.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. +// 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. @@ -12,13 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include -#include "rclcpp/rclcpp.hpp" +#include + +#include -#include "std_msgs/msg/float64_multi_array.hpp" +#include std::shared_ptr node; diff --git a/ignition_ros2_control_demos/examples/example_position.cpp b/ignition_ros2_control_demos/examples/example_position.cpp index 160fd869..51eb2ea3 100644 --- a/ignition_ros2_control_demos/examples/example_position.cpp +++ b/ignition_ros2_control_demos/examples/example_position.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. +// 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. @@ -12,15 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + +#include + #include #include #include -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" - -#include "control_msgs/action/follow_joint_trajectory.hpp" - std::shared_ptr node; bool common_goal_accepted = false; rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN; diff --git a/ignition_ros2_control_demos/examples/example_velocity.cpp b/ignition_ros2_control_demos/examples/example_velocity.cpp index 66076916..fa336e6a 100644 --- a/ignition_ros2_control_demos/examples/example_velocity.cpp +++ b/ignition_ros2_control_demos/examples/example_velocity.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Open Source Robotics Foundation, Inc. +// 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. @@ -12,21 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" +#include -#include "std_msgs/msg/float64_multi_array.hpp" +#include -std::shared_ptr node; +#include int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - node = std::make_shared("velocity_test_node"); + auto node = std::make_shared("velocity_test_node"); auto publisher = node->create_publisher( "/velocity_controller/commands", 10); diff --git a/ignition_ros2_control_demos/launch/cart_example_effort.launch.py b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py index 5ee0003c..ad801c8c 100644 --- a/ignition_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py @@ -76,7 +76,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( [os.path.join(get_package_share_directory('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')]), - launch_arguments=[('ign_args', [' -r -v 3'])]), + launch_arguments=[('ign_args', [' -r -v 3 empty.sdf'])]), RegisterEventHandler( event_handler=OnProcessExit( target_action=ignition_spawn_entity, diff --git a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py index c33ae66e..73cfe0ac 100644 --- a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -76,7 +76,7 @@ def generate_launch_description(): PythonLaunchDescriptionSource( [os.path.join(get_package_share_directory('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')]), - launch_arguments=[('ign_args', [' -r -v 3'])]), + launch_arguments=[('ign_args', [' -r -v 3 empty.sdf'])]), RegisterEventHandler( event_handler=OnProcessExit( target_action=ignition_spawn_entity, From 24b07f46d0175ae83203a312e8fca49e125f05ca Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 15 Jun 2021 16:47:50 +0200 Subject: [PATCH 27/50] Write joints on each simulation update period Signed-off-by: ahcorde --- ignition_ros2_control/src/ignition_ros2_control_plugin.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index 8ffbe180..ac62a580 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -372,8 +372,10 @@ void IgnitionROS2ControlPlugin::Update( this->dataPtr->last_update_sim_time_ros_ = sim_time_ros; this->dataPtr->controller_manager_->read(); this->dataPtr->controller_manager_->update(); - this->dataPtr->controller_manager_->write(); } + // Always set commands on joints, otherwise at low control frequencies the joints tremble + // as they are updated at a fraction of gazebo sim time + this->dataPtr->controller_manager_->write(); } } // namespace ignition_ros2_control From 117331d71934046dfc105aff2cd5658a882f2cf3 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 15 Jun 2021 16:49:39 +0200 Subject: [PATCH 28/50] Ensure that sim_joints_ always has the same number of elements as the joint_names Signed-off-by: ahcorde --- ignition_ros2_control/src/ignition_system.cpp | 116 ++++++++++-------- 1 file changed, 66 insertions(+), 50 deletions(-) diff --git a/ignition_ros2_control/src/ignition_system.cpp b/ignition_ros2_control/src/ignition_system.cpp index 6a080e63..6bd1b2b0 100644 --- a/ignition_ros2_control/src/ignition_system.cpp +++ b/ignition_ros2_control/src/ignition_system.cpp @@ -129,13 +129,13 @@ bool IgnitionSystem::initSim( std::string joint_name = this->dataPtr->joint_names_[j] = hardware_info.joints[j].name; ignition::gazebo::Entity simjoint = enableJoints[joint_name]; + this->dataPtr->sim_joints_.push_back(simjoint); if (!simjoint) { RCLCPP_WARN_STREAM( this->nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name << "' which is not in the ignition model."); continue; } - this->dataPtr->sim_joints_.push_back(simjoint); // Create joint position component if one doesn't exist if (!_ecm.EntityHasComponentType( @@ -225,25 +225,31 @@ IgnitionSystem::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])); + if (this->dataPtr->sim_joints_[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])); + if (this->dataPtr->sim_joints_[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])); + if (this->dataPtr->sim_joints_[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; } @@ -254,25 +260,31 @@ IgnitionSystem::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])); + if (this->dataPtr->sim_joints_[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])); + if (this->dataPtr->sim_joints_[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])); + if (this->dataPtr->sim_joints_[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; } @@ -292,25 +304,27 @@ hardware_interface::return_type IgnitionSystem::stop() hardware_interface::return_type IgnitionSystem::read() { for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { - // Get the joint velocity - const auto * jointVelocity = - this->dataPtr->ecm->Component( - this->dataPtr->sim_joints_[j]); - - // TODO(ahcorde): Revisit this part - // Get the joint force - // const auto * jointForce = - // this->dataPtr->ecm->Component( - // this->dataPtr->sim_joints_[j]); - - // Get the joint position - const auto * jointPositions = - this->dataPtr->ecm->Component( - this->dataPtr->sim_joints_[j]); - - this->dataPtr->joint_position_[j] = jointPositions->Data()[0]; - this->dataPtr->joint_velocity_[j] = jointVelocity->Data()[0]; - // this->dataPtr->joint_effort_[j] = jointForce->Data()[0]; + if (this->dataPtr->sim_joints_[j]) { + // Get the joint velocity + const auto * jointVelocity = + this->dataPtr->ecm->Component( + this->dataPtr->sim_joints_[j]); + + // TODO(ahcorde): Revisit this part + // Get the joint force + // const auto * jointForce = + // this->dataPtr->ecm->Component( + // this->dataPtr->sim_joints_[j]); + + // Get the joint position + const auto * jointPositions = + this->dataPtr->ecm->Component( + this->dataPtr->sim_joints_[j]); + + this->dataPtr->joint_position_[j] = jointPositions->Data()[0]; + this->dataPtr->joint_velocity_[j] = jointVelocity->Data()[0]; + // this->dataPtr->joint_effort_[j] = jointForce->Data()[0]; + } } return hardware_interface::return_type::OK; } @@ -318,6 +332,8 @@ hardware_interface::return_type IgnitionSystem::read() hardware_interface::return_type IgnitionSystem::write() { for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { + if (!this->dataPtr->sim_joints_[j]) + continue; if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { if (!this->dataPtr->ecm->Component( this->dataPtr->sim_joints_[j])) From 82074200f4ed31350e39e881911b98fa61291f9c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 15 Jun 2021 16:52:17 +0200 Subject: [PATCH 29/50] Join with the controller manager's executor thread on exit Signed-off-by: ahcorde --- .../ignition_ros2_control_plugin.hpp | 2 +- .../src/ignition_ros2_control_plugin.cpp | 16 +++++++++++++++- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp b/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp index 5acc5b64..c16381c0 100644 --- a/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp +++ b/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp @@ -34,7 +34,7 @@ class IgnitionROS2ControlPlugin IgnitionROS2ControlPlugin(); /// \brief Destructor - ~IgnitionROS2ControlPlugin() override = default; + ~IgnitionROS2ControlPlugin() override; // Documentation inherited void Configure( diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index ac62a580..84378e9a 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -56,6 +56,9 @@ class IgnitionROS2ControlPluginPrivate // Thread where the executor will sping std::thread thread_executor_spin_; + // Flag to stop the executor thread when this plugin is exiting + bool stop_; + // Executor to spin the controller rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; @@ -199,6 +202,16 @@ IgnitionROS2ControlPlugin::IgnitionROS2ControlPlugin() { } +////////////////////////////////////////////////// +IgnitionROS2ControlPlugin::~IgnitionROS2ControlPlugin() +{ + // Stop controller manager thread + this->dataPtr->stop_ = true; + this->dataPtr->executor_->remove_node(this->dataPtr->controller_manager_); + this->dataPtr->executor_->cancel(); + this->dataPtr->thread_executor_spin_.join(); +} + ////////////////////////////////////////////////// void IgnitionROS2ControlPlugin::Configure( const ignition::gazebo::Entity & _entity, @@ -238,9 +251,10 @@ void IgnitionROS2ControlPlugin::Configure( } this->dataPtr->executor_ = std::make_shared(); this->dataPtr->executor_->add_node(this->dataPtr->node); + this->dataPtr->stop_ = false; auto spin = [this]() { - while (rclcpp::ok()) { + while (rclcpp::ok() && !this->dataPtr->stop_) { this->dataPtr->executor_->spin_once(); } }; From c917a8c42c7cdb3eb83a4d1b8c9f73f1f9fab77c Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 15 Jun 2021 17:03:36 +0200 Subject: [PATCH 30/50] Forward sdf ros remappings to loaded controllers Signed-off-by: ahcorde --- .../src/ignition_ros2_control_plugin.cpp | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index 84378e9a..1ef18258 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -239,6 +239,36 @@ void IgnitionROS2ControlPlugin::Configure( } std::vector arguments = {"--ros-args", "--params-file", paramFileName}; + auto sdfPtr = const_cast(_sdf.get()); + + if (sdfPtr->HasElement("ros")) { + sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); + + // Set namespace if tag is present + if (sdfRos->HasElement("namespace")) { + std::string ns = sdfRos->GetElement("namespace")->Get(); + // prevent exception: namespace must be absolute, it must lead with a '/' + if (ns.empty() || ns[0] != '/') { + ns = '/' + ns; + } + std::string ns_arg = std::string("__ns:=") + ns; + arguments.push_back(RCL_REMAP_FLAG); + arguments.push_back(ns_arg); + } + + // Get list of remapping rules from SDF + if (sdfRos->HasElement("remapping")) { + sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); + + arguments.push_back(RCL_ROS_ARGS_FLAG); + while (argument_sdf) { + std::string argument = argument_sdf->Get(); + arguments.push_back(RCL_REMAP_FLAG); + arguments.push_back(argument); + argument_sdf = argument_sdf->GetNextElement("remapping"); + } + } + } std::vector argv; for (const auto & arg : arguments) { From 5a21c7aec649f1f6594d43f41bbc6bbcba5b4292 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 15 Jun 2021 21:09:53 +0200 Subject: [PATCH 31/50] Update ign_ros2_control Signed-off-by: ahcorde --- .../ignition_ros2_control_plugin.hpp | 9 +- .../src/ignition_ros2_control_plugin.cpp | 326 ++++++++++-------- ignition_ros2_control/src/ignition_system.cpp | 311 +++++++---------- .../config/cartpole_controller_effort.yaml | 10 +- .../config/cartpole_controller_position.yaml | 9 +- .../config/cartpole_controller_velocity.yaml | 9 +- .../launch/cart_example_effort.launch.py | 5 +- .../launch/cart_example_position.launch.py | 5 +- .../launch/cart_example_velocity.launch.py | 5 +- .../urdf/test_cart_effort.xacro.urdf | 2 +- .../urdf/test_cart_position.xacro.urdf | 2 +- .../urdf/test_cart_velocity.xacro.urdf | 2 +- 12 files changed, 352 insertions(+), 343 deletions(-) diff --git a/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp b/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp index c16381c0..653035d4 100644 --- a/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp +++ b/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp @@ -27,7 +27,8 @@ class IgnitionROS2ControlPluginPrivate; class IgnitionROS2ControlPlugin : public ignition::gazebo::System, public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemUpdate + public ignition::gazebo::ISystemPreUpdate, + public ignition::gazebo::ISystemPostUpdate { public: /// \brief Constructor @@ -44,10 +45,14 @@ class IgnitionROS2ControlPlugin ignition::gazebo::EventManager & _eventMgr) override; // Documentation inherited - void Update( + void PreUpdate( const ignition::gazebo::UpdateInfo & _info, ignition::gazebo::EntityComponentManager & _ecm) override; + void PostUpdate( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) override; + private: /// \brief Private data pointer. std::unique_ptr dataPtr; diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index 1ef18258..4362031a 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -46,155 +46,178 @@ namespace ignition_ros2_control ////////////////////////////////////////////////// class IgnitionROS2ControlPluginPrivate { -public: + + /// \brief Get the URDF XML from the parameter server + public: std::string getURDF() const; + + /// \brief Get a list of enabled, unique, 1-axis joints of the model. If no + /// joint names are specified in the plugin configuration, all valid 1-axis + /// joints are returned + /// \param[in] _entity Entity of the model that the plugin is being + /// configured for + /// \param[in] _ecm Ignition Entity Component Manager + /// \return List of entities containinig all enabled joints + public: std::map GetEnabledJoints( + const ignition::gazebo::Entity & _entity, + ignition::gazebo::EntityComponentManager & _ecm) const; + /// \brief Entity ID for sensor within Gazebo. - ignition::gazebo::Entity entity_; + public: ignition::gazebo::Entity entity_; - // Node Handles - std::shared_ptr node; + /// \brief Node Handles + public: std::shared_ptr node; - // Thread where the executor will sping - std::thread thread_executor_spin_; + /// \brief Thread where the executor will sping + public: std::thread thread_executor_spin_; - // Flag to stop the executor thread when this plugin is exiting - bool stop_; + /// \brief Flag to stop the executor thread when this plugin is exiting + public: bool stop_; - // Executor to spin the controller - rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + /// \brief Executor to spin the controller + public: rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; - // Timing - rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); + /// \brief Timing + public: rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); - // Interface loader - boost::shared_ptr> robot_hw_sim_loader_; - // Controller manager - std::shared_ptr controller_manager_; + /// \brief Controller manager + public: std::shared_ptr controller_manager_; - // String with the robot description param_name + /// \brief String with the robot description param_name // TODO(ahcorde): Add param in plugin tag - std::string robot_description_ = "robot_description"; + public: std::string robot_description_ = "robot_description"; - // String with the name of the node that contains the robot_description + /// \brief String with the name of the node that contains the robot_description // TODO(ahcorde): Add param in plugin tag - std::string robot_description_node_ = "robot_state_publisher"; + public: std::string robot_description_node_ = "robot_state_publisher"; - // Last time the update method was called - rclcpp::Time last_update_sim_time_ros_; + /// \brief Last time the update method was called + public: rclcpp::Time last_update_sim_time_ros_; - std::string getURDF(std::string param_name) const - { - std::string urdf_string; - - using namespace std::chrono_literals; - auto parameters_client = std::make_shared( - node, robot_description_node_); - while (!parameters_client->wait_for_service(0.5s)) { - if (!rclcpp::ok()) { - RCLCPP_ERROR( - node->get_logger(), "Interrupted while waiting for %s service. Exiting.", - robot_description_node_.c_str()); - return 0; - } - RCLCPP_ERROR( - node->get_logger(), "%s service not available, waiting again...", - robot_description_node_.c_str()); + ignition::gazebo::EntityComponentManager * ecm; +}; + +////////////////////////////////////////////////// +std::map + IgnitionROS2ControlPluginPrivate::GetEnabledJoints( + const ignition::gazebo::Entity & _entity, + ignition::gazebo::EntityComponentManager & _ecm) const +{ + std::map output; + + std::vector enabledJoints; + + // Get all available joints + auto jointEntities = _ecm.ChildrenByComponents(_entity, ignition::gazebo::components::Joint()); + + // Iterate over all joints and verify whether they can be enabled or not + for (const auto & jointEntity : jointEntities) { + const auto jointName = _ecm.Component( + jointEntity)->Data(); + + // Make sure the joint type is supported, i.e. it has exactly one + // actuated axis + const auto * jointType = _ecm.Component(jointEntity); + switch (jointType->Data()) { + case sdf::JointType::PRISMATIC: + case sdf::JointType::REVOLUTE: + case sdf::JointType::CONTINUOUS: + case sdf::JointType::GEARBOX: + { + // Supported joint type + break; + } + case sdf::JointType::FIXED: + { + RCLCPP_INFO( + node->get_logger(), + "[ignition_ros2_control] Fixed joint [%s] (Entity=%d)] is skipped", + jointName.c_str(), jointEntity); + continue; + } + case sdf::JointType::REVOLUTE2: + case sdf::JointType::SCREW: + case sdf::JointType::BALL: + case sdf::JointType::UNIVERSAL: + { + RCLCPP_WARN( + node->get_logger(), + "[ignition_ros2_control] Joint [%s] (Entity=%d)] is of unsupported type." + " Only joints with a single axis are supported.", + jointName.c_str(), jointEntity); + continue; + } + default: + { + RCLCPP_WARN( + node->get_logger(), + "[ignition_ros2_control] Joint [%s] (Entity=%d)] is of unknown type", + jointName.c_str(), jointEntity); + continue; + } } + output[jointName] = jointEntity; + } - RCLCPP_INFO( - node->get_logger(), "connected to service!! %s asking for %s", - robot_description_node_.c_str(), - param_name.c_str()); - - // search and wait for robot_description on param server - while (urdf_string.empty()) { - RCLCPP_DEBUG(node->get_logger(), "param_name %s", param_name.c_str()); - - try { - auto f = parameters_client->get_parameters({param_name}); - f.wait(); - std::vector values = f.get(); - urdf_string = values[0].as_string(); - } catch (const std::exception & e) { - RCLCPP_ERROR(node->get_logger(), "%s", e.what()); - } + return output; +} - if (!urdf_string.empty()) { - break; - } else { - RCLCPP_ERROR( - node->get_logger(), "ignition_ros2_control plugin is waiting for model" - " URDF in parameter [%s] on the ROS param server.", param_name.c_str()); - } - usleep(100000); - } - RCLCPP_INFO(node->get_logger(), "Received URDF from param server"); +////////////////////////////////////////////////// +std::string IgnitionROS2ControlPluginPrivate::getURDF() const +{ + std::string urdf_string; - return urdf_string; + using namespace std::chrono_literals; + auto parameters_client = std::make_shared( + node, robot_description_node_); + while (!parameters_client->wait_for_service(0.5s)) { + if (!rclcpp::ok()) { + RCLCPP_ERROR( + node->get_logger(), "Interrupted while waiting for %s service. Exiting.", + robot_description_node_.c_str()); + return 0; + } + RCLCPP_ERROR( + node->get_logger(), "%s service not available, waiting again...", + robot_description_node_.c_str()); } - ////////////////////////////////////////////////// - -public: - std::map GetEnabledJoints( - const ignition::gazebo::Entity & _entity, - ignition::gazebo::EntityComponentManager & _ecm) const - { - std::map output; - - std::vector enabledJoints; - - // Get all available joints - auto jointEntities = _ecm.ChildrenByComponents(_entity, ignition::gazebo::components::Joint()); - - // Iterate over all joints and verify whether they can be enabled or not - for (const auto & jointEntity : jointEntities) { - const auto jointName = _ecm.Component( - jointEntity)->Data(); - - // Make sure the joint type is supported, i.e. it has exactly one - // actuated axis - const auto * jointType = _ecm.Component(jointEntity); - switch (jointType->Data()) { - case sdf::JointType::PRISMATIC: - case sdf::JointType::REVOLUTE: - case sdf::JointType::CONTINUOUS: - case sdf::JointType::GEARBOX: - { - // Supported joint type - break; - } - case sdf::JointType::FIXED: - { - igndbg << "[ignition_ros2_control] Fixed joint [" << jointName << - "(Entity=" << jointEntity << ")] is skipped.\n"; - continue; - } - case sdf::JointType::REVOLUTE2: - case sdf::JointType::SCREW: - case sdf::JointType::BALL: - case sdf::JointType::UNIVERSAL: - { - ignwarn << "[ignition_ros2_control] Joint [" << jointName << - "(Entity=" << jointEntity << - ")] is of unsupported type. Only joints with a single axis" - " are supported.\n"; - continue; - } - default: - { - ignwarn << "[ignition_ros2_control] Joint [" << jointName << - "(Entity=" << jointEntity << ")] is of unknown type.\n"; - continue; - } - } - output[jointName] = jointEntity; + RCLCPP_INFO( + node->get_logger(), "connected to service!! %s asking for %s", + robot_description_node_.c_str(), + this->robot_description_.c_str()); + + // search and wait for robot_description on param server + while (urdf_string.empty()) { + RCLCPP_DEBUG(node->get_logger(), "param_name %s", + this->robot_description_.c_str()); + + try { + auto f = parameters_client->get_parameters({this->robot_description_}); + f.wait(); + std::vector values = f.get(); + urdf_string = values[0].as_string(); + } catch (const std::exception & e) { + RCLCPP_ERROR(node->get_logger(), "%s", e.what()); } - return output; + if (!urdf_string.empty()) { + break; + } else { + RCLCPP_ERROR( + node->get_logger(), "ignition_ros2_control plugin is waiting for model" + " URDF in parameter [%s] on the ROS param server.", + this->robot_description_.c_str()); + } + usleep(100000); } -}; + RCLCPP_INFO(node->get_logger(), "Received URDF from param server"); + + return urdf_string; +} ////////////////////////////////////////////////// IgnitionROS2ControlPlugin::IgnitionROS2ControlPlugin() @@ -222,10 +245,11 @@ void IgnitionROS2ControlPlugin::Configure( // Make sure the controller is attached to a valid model const auto model = ignition::gazebo::Model(_entity); if (!model.Valid(_ecm)) { - ignerr << "[Ignition ROS 2 Control] Failed to initialize because [" << - model.Name(_ecm) << "(Entity=" << _entity << - ")] is not a model. Please make sure that" - " Ignition ROS 2 Control is attached to a valid model.\n"; + RCLCPP_ERROR( + this->dataPtr->node->get_logger(), + "[Ignition ROS 2 Control] Failed to initialize because [%s] (Entity=%u)] is not a model." + "Please make sure that Ignition ROS 2 Control is attached to a valid model.", + model.Name(_ecm).c_str(), _entity); return; } @@ -233,8 +257,9 @@ void IgnitionROS2ControlPlugin::Configure( std::string paramFileName = _sdf->Get("parameters"); if (paramFileName.empty()) { - ignerr << "Ignition ros2 control found an empty parameters file. " << - "Failed to initialize."; + RCLCPP_ERROR( + this->dataPtr->node->get_logger(), + "Ignition ros2 control found an empty parameters file. Failed to initialize."); return; } @@ -292,23 +317,27 @@ void IgnitionROS2ControlPlugin::Configure( RCLCPP_DEBUG_STREAM( this->dataPtr->node->get_logger(), "[Ignition ROS 2 Control] Setting up controller for [" << - "(Entity=" << _entity << ")].\n"); - - ignmsg << "[Ignition ROS 2 Control] Setting up controller for [" << - model.Name(_ecm) << "(Entity=" << _entity << ")].\n"; + model.Name(_ecm) << "] (Entity=" << _entity << ")]."); // Get list of enabled joints auto enabledJoints = this->dataPtr->GetEnabledJoints( _entity, _ecm); + if (enabledJoints.size() == 0) + { + RCLCPP_DEBUG_STREAM( + this->dataPtr->node->get_logger(), "[Ignition ROS 2 Control] There are no available Joints."); + return; + } + // 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 = this->dataPtr->getURDF(this->dataPtr->robot_description_); + urdf_string = this->dataPtr->getURDF(); control_hardware = hardware_interface::parse_control_resources_from_urdf(urdf_string); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM( @@ -403,9 +432,19 @@ void IgnitionROS2ControlPlugin::Configure( } ////////////////////////////////////////////////// -void IgnitionROS2ControlPlugin::Update( - const ignition::gazebo::UpdateInfo & _info, +void IgnitionROS2ControlPlugin::PreUpdate( + const ignition::gazebo::UpdateInfo & /*_info*/, ignition::gazebo::EntityComponentManager & /*_ecm*/) +{ + // Always set commands on joints, otherwise at low control frequencies the joints tremble + // as they are updated at a fraction of gazebo sim time + this->dataPtr->controller_manager_->write(); +} + +////////////////////////////////////////////////// +void IgnitionROS2ControlPlugin::PostUpdate( + const ignition::gazebo::UpdateInfo & _info, + const ignition::gazebo::EntityComponentManager & /*_ecm*/) { // Get the simulation time and period rclcpp::Time sim_time_ros(std::chrono::duration_cast( @@ -414,12 +453,12 @@ void IgnitionROS2ControlPlugin::Update( if (sim_period >= this->dataPtr->control_period_) { this->dataPtr->last_update_sim_time_ros_ = sim_time_ros; + auto ign_controller_manager = + std::dynamic_pointer_cast( + this->dataPtr->controller_manager_); this->dataPtr->controller_manager_->read(); this->dataPtr->controller_manager_->update(); } - // Always set commands on joints, otherwise at low control frequencies the joints tremble - // as they are updated at a fraction of gazebo sim time - this->dataPtr->controller_manager_->write(); } } // namespace ignition_ros2_control @@ -427,8 +466,9 @@ IGNITION_ADD_PLUGIN( ignition_ros2_control::IgnitionROS2ControlPlugin, ignition::gazebo::System, ignition_ros2_control::IgnitionROS2ControlPlugin::ISystemConfigure, - ignition_ros2_control::IgnitionROS2ControlPlugin::ISystemUpdate) + ignition_ros2_control::IgnitionROS2ControlPlugin::ISystemPreUpdate, + ignition_ros2_control::IgnitionROS2ControlPlugin::ISystemPostUpdate) IGNITION_ADD_PLUGIN_ALIAS( ignition_ros2_control::IgnitionROS2ControlPlugin, - "ignition::gazebo::systems::IgnitionROS2ControlPlugin") + "ignition_ros2_control::IgnitionROS2ControlPlugin") diff --git a/ignition_ros2_control/src/ignition_system.cpp b/ignition_ros2_control/src/ignition_system.cpp index 6bd1b2b0..103a63b6 100644 --- a/ignition_ros2_control/src/ignition_system.cpp +++ b/ignition_ros2_control/src/ignition_system.cpp @@ -27,62 +27,68 @@ #include "ignition_ros2_control/ignition_system.hpp" -class ignition_ros2_control::IgnitionSystemPrivate +struct jointData { -public: - IgnitionSystemPrivate() = default; - - ~IgnitionSystemPrivate() = default; - /// \brief Degrees od freedom. - size_t n_dof_; - - /// \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_; + std::string name; /// \brief vector with the current joint position - std::vector joint_position_; + double joint_position; /// \brief vector with the current joint velocity - std::vector joint_velocity_; + double joint_velocity; /// \brief vector with the current joint effort - std::vector joint_effort_; + double joint_effort; /// \brief vector with the current cmd joint position - std::vector joint_position_cmd_; + double joint_position_cmd; /// \brief vector with the current cmd joint velocity - std::vector joint_velocity_cmd_; + double joint_velocity_cmd; /// \brief vector with the current cmd joint effort - std::vector joint_effort_cmd_; + double joint_effort_cmd; + + /// \brief handles to the joints from within Gazebo + ignition::gazebo::Entity sim_joint; + + /// \brief vector with the control method defined in the URDF for each joint. + ignition_ros2_control::IgnitionSystemInterface::ControlMethod joint_control_method; /// \brief The current positions of the joints - std::vector> joint_pos_state_; + std::shared_ptr joint_pos_state; /// \brief The current velocities of the joints - std::vector> joint_vel_state_; + std::shared_ptr joint_vel_state; /// \brief The current effort forces applied to the joints - std::vector> joint_eff_state_; + std::shared_ptr joint_eff_state; /// \brief The position command interfaces of the joints - std::vector> joint_pos_cmd_; + std::shared_ptr joint_pos_cmd; /// \brief The velocity command interfaces of the joints - std::vector> joint_vel_cmd_; + std::shared_ptr joint_vel_cmd; /// \brief The effort command interfaces of the joints - std::vector> joint_eff_cmd_; + std::shared_ptr joint_eff_cmd; +}; + +class ignition_ros2_control::IgnitionSystemPrivate +{ +public: + IgnitionSystemPrivate() = default; + + ~IgnitionSystemPrivate() = default; + /// \brief Degrees od freedom. + size_t n_dof_; + + /// \brief last time the write method was called. + rclcpp::Time last_update_sim_time_ros_; + + /// \brief vector with the joint's names. + std::vector joints_; ignition::gazebo::EntityComponentManager * ecm; }; @@ -105,20 +111,7 @@ bool IgnitionSystem::initSim( RCLCPP_ERROR(this->nh_->get_logger(), "n_dof_ %d", this->dataPtr->n_dof_); - 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_); + this->dataPtr->joints_.resize(this->dataPtr->n_dof_); if (this->dataPtr->n_dof_ == 0) { RCLCPP_WARN_STREAM(this->nh_->get_logger(), "There is not joint available "); @@ -126,16 +119,10 @@ bool IgnitionSystem::initSim( } 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; + std::string joint_name = this->dataPtr->joints_[j].name = hardware_info.joints[j].name; ignition::gazebo::Entity simjoint = enableJoints[joint_name]; - this->dataPtr->sim_joints_.push_back(simjoint); - if (!simjoint) { - RCLCPP_WARN_STREAM( - this->nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name << - "' which is not in the ignition model."); - continue; - } + this->dataPtr->joints_[j].sim_joint = simjoint; // Create joint position component if one doesn't exist if (!_ecm.EntityHasComponentType( @@ -170,40 +157,39 @@ bool IgnitionSystem::initSim( 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]); + this->dataPtr->joints_[j].joint_control_method |= POSITION; + this->dataPtr->joints_[j].joint_pos_cmd = std::make_shared( + joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joints_[j].joint_position_cmd); } else 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]); + this->dataPtr->joints_[j].joint_control_method |= VELOCITY; + this->dataPtr->joints_[j].joint_vel_cmd = std::make_shared( + joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joints_[j].joint_velocity_cmd); } else if (hardware_info.joints[j].command_interfaces[i].name == "effort") { - this->dataPtr->joint_control_methods_[j] |= EFFORT; + this->dataPtr->joints_[j].joint_control_method |= 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]); + this->dataPtr->joints_[j].joint_eff_cmd = std::make_shared( + joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[j].joint_effort_cmd); } } - RCLCPP_INFO_STREAM( - this->nh_->get_logger(), "\tState:"); + 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_state_[j] = std::make_shared( - joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joint_position_[j]); + this->dataPtr->joints_[j].joint_pos_state = std::make_shared( + joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joints_[j].joint_position); } if (hardware_info.joints[j].state_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); - this->dataPtr->joint_vel_state_[j] = std::make_shared( - joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joint_velocity_[j]); + this->dataPtr->joints_[j].joint_vel_state = std::make_shared( + joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joints_[j].joint_velocity); } if (hardware_info.joints[j].state_interfaces[i].name == "effort") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); - this->dataPtr->joint_eff_state_[j] = std::make_shared( - joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joint_effort_[j]); + this->dataPtr->joints_[j].joint_eff_state = std::make_shared( + joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[j].joint_effort); } } } @@ -224,32 +210,26 @@ IgnitionSystem::export_state_interfaces() { std::vector state_interfaces; - for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); ++i) { - if (this->dataPtr->sim_joints_[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->joints_.size(); ++i) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + this->dataPtr->joints_[i].name, + hardware_interface::HW_IF_POSITION, + &this->dataPtr->joints_[i].joint_position)); } - for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); ++i) { - if (this->dataPtr->sim_joints_[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->joints_.size(); ++i) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + this->dataPtr->joints_[i].name, + hardware_interface::HW_IF_VELOCITY, + &this->dataPtr->joints_[i].joint_velocity)); } - for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); ++i) { - if (this->dataPtr->sim_joints_[i]) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - this->dataPtr->joint_names_[i], - hardware_interface::HW_IF_EFFORT, - &this->dataPtr->joint_effort_[i])); - } + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + this->dataPtr->joints_[i].name, + hardware_interface::HW_IF_EFFORT, + &this->dataPtr->joints_[i].joint_effort)); } return state_interfaces; } @@ -259,32 +239,26 @@ IgnitionSystem::export_command_interfaces() { std::vector command_interfaces; - for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); ++i) { - if (this->dataPtr->sim_joints_[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->joints_.size(); ++i) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + this->dataPtr->joints_[i].name, + hardware_interface::HW_IF_POSITION, + &this->dataPtr->joints_[i].joint_position_cmd)); } - for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); ++i) { - if (this->dataPtr->sim_joints_[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->joints_.size(); ++i) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + this->dataPtr->joints_[i].name, + hardware_interface::HW_IF_VELOCITY, + &this->dataPtr->joints_[i].joint_velocity_cmd)); } - for (unsigned int i = 0; i < this->dataPtr->joint_names_.size(); ++i) { - if (this->dataPtr->sim_joints_[i]) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - this->dataPtr->joint_names_[i], - hardware_interface::HW_IF_EFFORT, - &this->dataPtr->joint_effort_cmd_[i])); - } + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + this->dataPtr->joints_[i].name, + hardware_interface::HW_IF_EFFORT, + &this->dataPtr->joints_[i].joint_effort_cmd)); } return command_interfaces; } @@ -303,110 +277,81 @@ hardware_interface::return_type IgnitionSystem::stop() hardware_interface::return_type IgnitionSystem::read() { - for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { - if (this->dataPtr->sim_joints_[j]) { - // Get the joint velocity - const auto * jointVelocity = - this->dataPtr->ecm->Component( - this->dataPtr->sim_joints_[j]); - - // TODO(ahcorde): Revisit this part - // Get the joint force - // const auto * jointForce = - // this->dataPtr->ecm->Component( - // this->dataPtr->sim_joints_[j]); - - // Get the joint position - const auto * jointPositions = - this->dataPtr->ecm->Component( - this->dataPtr->sim_joints_[j]); - - this->dataPtr->joint_position_[j] = jointPositions->Data()[0]; - this->dataPtr->joint_velocity_[j] = jointVelocity->Data()[0]; - // this->dataPtr->joint_effort_[j] = jointForce->Data()[0]; - } + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + // Get the joint velocity + const auto * jointVelocity = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 + // Get the joint force + // const auto * jointForce = + // _ecm.Component( + // this->dataPtr->sim_joints_[j]); + + // Get the joint position + const auto * jointPositions = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); + + this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0]; + this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0]; + // this->dataPtr->joint_effort_[j] = jointForce->Data()[0]; } return hardware_interface::return_type::OK; } hardware_interface::return_type IgnitionSystem::write() { - for (unsigned int j = 0; j < this->dataPtr->joint_names_.size(); j++) { - if (!this->dataPtr->sim_joints_[j]) - continue; - if (this->dataPtr->joint_control_methods_[j] & VELOCITY) { + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { + if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) { if (!this->dataPtr->ecm->Component( - this->dataPtr->sim_joints_[j])) + this->dataPtr->joints_[i].sim_joint)) { this->dataPtr->ecm->CreateComponent( - this->dataPtr->sim_joints_[j], + this->dataPtr->joints_[i].sim_joint, ignition::gazebo::components::JointVelocityCmd({0})); } else { const auto jointVelCmd = this->dataPtr->ecm->Component( - this->dataPtr->sim_joints_[j]); + this->dataPtr->joints_[i].sim_joint); *jointVelCmd = ignition::gazebo::components::JointVelocityCmd( - {this->dataPtr->joint_velocity_cmd_[j]}); + {this->dataPtr->joints_[i].joint_velocity_cmd}); } } - if (this->dataPtr->joint_control_methods_[j] & POSITION) { + if (this->dataPtr->joints_[i].joint_control_method & POSITION) { if (!this->dataPtr->ecm->Component( - this->dataPtr->sim_joints_[j])) + this->dataPtr->joints_[i].sim_joint)) { this->dataPtr->ecm->CreateComponent( - this->dataPtr->sim_joints_[j], - ignition::gazebo::components::JointPositionReset({this->dataPtr->joint_position_[j]})); + this->dataPtr->joints_[i].sim_joint, + ignition::gazebo::components::JointPositionReset({this->dataPtr->joints_[i].joint_position})); const auto jointPosCmd = this->dataPtr->ecm->Component( - this->dataPtr->sim_joints_[j]); + this->dataPtr->joints_[i].sim_joint); *jointPosCmd = ignition::gazebo::components::JointPositionReset( - {this->dataPtr->joint_position_cmd_[j]}); + {this->dataPtr->joints_[i].joint_position_cmd}); } } - if (this->dataPtr->joint_control_methods_[j] & EFFORT) { + if (this->dataPtr->joints_[i].joint_control_method & EFFORT) { if (!this->dataPtr->ecm->Component( - this->dataPtr->sim_joints_[j])) + this->dataPtr->joints_[i].sim_joint)) { this->dataPtr->ecm->CreateComponent( - this->dataPtr->sim_joints_[j], + this->dataPtr->joints_[i].sim_joint, ignition::gazebo::components::JointForceCmd({0})); } else { const auto jointEffortCmd = this->dataPtr->ecm->Component( - this->dataPtr->sim_joints_[j]); + this->dataPtr->joints_[i].sim_joint); *jointEffortCmd = ignition::gazebo::components::JointForceCmd( - {this->dataPtr->joint_effort_cmd_[j]}); + {this->dataPtr->joints_[i].joint_effort_cmd}); } } } - // Get the simulation time and period - // ignition::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 ignition_ros2_control diff --git a/ignition_ros2_control_demos/config/cartpole_controller_effort.yaml b/ignition_ros2_control_demos/config/cartpole_controller_effort.yaml index 11c26f19..54b0be31 100644 --- a/ignition_ros2_control_demos/config/cartpole_controller_effort.yaml +++ b/ignition_ros2_control_demos/config/cartpole_controller_effort.yaml @@ -5,10 +5,16 @@ controller_manager: effort_controllers: type: effort_controllers/JointGroupEffortController - joint_state_controller: - type: joint_state_controller/JointStateController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster effort_controllers: ros__parameters: joints: - slider_to_cart + command_interfaces: + - effort + state_interfaces: + - position + - velocity + - effort diff --git a/ignition_ros2_control_demos/config/cartpole_controller_position.yaml b/ignition_ros2_control_demos/config/cartpole_controller_position.yaml index 935af69e..f51d2fa5 100644 --- a/ignition_ros2_control_demos/config/cartpole_controller_position.yaml +++ b/ignition_ros2_control_demos/config/cartpole_controller_position.yaml @@ -5,10 +5,15 @@ controller_manager: joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController - joint_state_controller: - type: joint_state_controller/JointStateController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster joint_trajectory_controller: ros__parameters: joints: - slider_to_cart + command_interfaces: + - position + state_interfaces: + - position + - velocity diff --git a/ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml b/ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml index ad259e5e..582b4c85 100644 --- a/ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml +++ b/ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml @@ -5,10 +5,15 @@ controller_manager: velocity_controller: type: velocity_controllers/JointGroupVelocityController - joint_state_controller: - type: joint_state_controller/JointStateController + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster velocity_controller: ros__parameters: joints: - slider_to_cart + command_interfaces: + - velocity + state_interfaces: + - position + - velocity diff --git a/ignition_ros2_control_demos/launch/cart_example_effort.launch.py b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py index ad801c8c..1de9c3a9 100644 --- a/ignition_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py @@ -61,12 +61,13 @@ def generate_launch_description(): ) load_joint_state_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + 'joint_state_broadcaster'], output='screen' ) load_joint_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_start_controller', 'effort_controllers'], + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'effort_controllers'], output='screen' ) diff --git a/ignition_ros2_control_demos/launch/cart_example_position.launch.py b/ignition_ros2_control_demos/launch/cart_example_position.launch.py index 35fcf347..aa217d56 100644 --- a/ignition_ros2_control_demos/launch/cart_example_position.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_position.launch.py @@ -61,12 +61,13 @@ def generate_launch_description(): ) load_joint_state_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + 'joint_state_broadcaster'], output='screen' ) load_joint_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_start_controller', 'joint_trajectory_controller'], + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'joint_trajectory_controller'], output='screen' ) diff --git a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py index 73cfe0ac..7c243b49 100644 --- a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -61,12 +61,13 @@ def generate_launch_description(): ) load_joint_state_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_start_controller', 'joint_state_controller'], + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + 'joint_state_broadcaster'], output='screen' ) load_joint_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_start_controller', 'velocity_controller'], + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'velocity_controller'], output='screen' ) diff --git a/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf index 46f8f30e..0dcbf0e6 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf @@ -80,7 +80,7 @@ - + $(find ignition_ros2_control_demos)/config/cartpole_controller_effort.yaml diff --git a/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf index 14552d37..88e59db5 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -80,7 +80,7 @@ - + $(find ignition_ros2_control_demos)/config/cartpole_controller_position.yaml diff --git a/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index 618f0fc8..d788674a 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -80,7 +80,7 @@ - + $(find ignition_ros2_control_demos)/config/cartpole_controller_velocity.yaml From 6e5656419c91803a4dbfc941eb176c827eb608d1 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 15 Jun 2021 21:14:59 +0200 Subject: [PATCH 32/50] make linters happy Signed-off-by: ahcorde --- .../ignition_ros2_control_plugin.hpp | 4 +- .../src/ignition_ros2_control_plugin.cpp | 98 +++++++++---------- ignition_ros2_control/src/ignition_system.cpp | 36 ++++--- 3 files changed, 75 insertions(+), 63 deletions(-) diff --git a/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp b/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp index 653035d4..3b23bfbf 100644 --- a/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp +++ b/ignition_ros2_control/include/ignition_ros2_control/ignition_ros2_control_plugin.hpp @@ -50,8 +50,8 @@ class IgnitionROS2ControlPlugin ignition::gazebo::EntityComponentManager & _ecm) override; void PostUpdate( - const ignition::gazebo::UpdateInfo &_info, - const ignition::gazebo::EntityComponentManager &_ecm) override; + const ignition::gazebo::UpdateInfo & _info, + const ignition::gazebo::EntityComponentManager & _ecm) override; private: /// \brief Private data pointer. diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index 4362031a..b0c4935f 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -46,9 +46,8 @@ namespace ignition_ros2_control ////////////////////////////////////////////////// class IgnitionROS2ControlPluginPrivate { - /// \brief Get the URDF XML from the parameter server - public: std::string getURDF() const; + std::string getURDF() const; /// \brief Get a list of enabled, unique, 1-axis joints of the model. If no /// joint names are specified in the plugin configuration, all valid 1-axis @@ -57,54 +56,55 @@ class IgnitionROS2ControlPluginPrivate /// configured for /// \param[in] _ecm Ignition Entity Component Manager /// \return List of entities containinig all enabled joints - public: std::map GetEnabledJoints( + std::map GetEnabledJoints( const ignition::gazebo::Entity & _entity, ignition::gazebo::EntityComponentManager & _ecm) const; /// \brief Entity ID for sensor within Gazebo. - public: ignition::gazebo::Entity entity_; + ignition::gazebo::Entity entity_; /// \brief Node Handles - public: std::shared_ptr node; + std::shared_ptr node; /// \brief Thread where the executor will sping - public: std::thread thread_executor_spin_; + std::thread thread_executor_spin_; /// \brief Flag to stop the executor thread when this plugin is exiting - public: bool stop_; + bool stop_; /// \brief Executor to spin the controller - public: rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; /// \brief Timing - public: rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); + rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); /// \brief Interface loader - public: boost::shared_ptr> robot_hw_sim_loader_; /// \brief Controller manager - public: std::shared_ptr controller_manager_; + std::shared_ptr controller_manager_; /// \brief String with the robot description param_name // TODO(ahcorde): Add param in plugin tag - public: std::string robot_description_ = "robot_description"; + std::string robot_description_ = "robot_description"; /// \brief String with the name of the node that contains the robot_description // TODO(ahcorde): Add param in plugin tag - public: std::string robot_description_node_ = "robot_state_publisher"; + std::string robot_description_node_ = "robot_state_publisher"; /// \brief Last time the update method was called - public: rclcpp::Time last_update_sim_time_ros_; + rclcpp::Time last_update_sim_time_ros_; + /// \brief ECM pointer ignition::gazebo::EntityComponentManager * ecm; }; ////////////////////////////////////////////////// std::map - IgnitionROS2ControlPluginPrivate::GetEnabledJoints( - const ignition::gazebo::Entity & _entity, - ignition::gazebo::EntityComponentManager & _ecm) const +IgnitionROS2ControlPluginPrivate::GetEnabledJoints( + const ignition::gazebo::Entity & _entity, + ignition::gazebo::EntityComponentManager & _ecm) const { std::map output; @@ -192,7 +192,8 @@ std::string IgnitionROS2ControlPluginPrivate::getURDF() const // search and wait for robot_description on param server while (urdf_string.empty()) { - RCLCPP_DEBUG(node->get_logger(), "param_name %s", + RCLCPP_DEBUG( + node->get_logger(), "param_name %s", this->robot_description_.c_str()); try { @@ -267,33 +268,33 @@ void IgnitionROS2ControlPlugin::Configure( auto sdfPtr = const_cast(_sdf.get()); if (sdfPtr->HasElement("ros")) { - sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); - - // Set namespace if tag is present - if (sdfRos->HasElement("namespace")) { - std::string ns = sdfRos->GetElement("namespace")->Get(); - // prevent exception: namespace must be absolute, it must lead with a '/' - if (ns.empty() || ns[0] != '/') { - ns = '/' + ns; - } - std::string ns_arg = std::string("__ns:=") + ns; - arguments.push_back(RCL_REMAP_FLAG); - arguments.push_back(ns_arg); - } - - // Get list of remapping rules from SDF - if (sdfRos->HasElement("remapping")) { - sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); - - arguments.push_back(RCL_ROS_ARGS_FLAG); - while (argument_sdf) { - std::string argument = argument_sdf->Get(); - arguments.push_back(RCL_REMAP_FLAG); - arguments.push_back(argument); - argument_sdf = argument_sdf->GetNextElement("remapping"); - } - } - } + sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); + + // Set namespace if tag is present + if (sdfRos->HasElement("namespace")) { + std::string ns = sdfRos->GetElement("namespace")->Get(); + // prevent exception: namespace must be absolute, it must lead with a '/' + if (ns.empty() || ns[0] != '/') { + ns = '/' + ns; + } + std::string ns_arg = std::string("__ns:=") + ns; + arguments.push_back(RCL_REMAP_FLAG); + arguments.push_back(ns_arg); + } + + // Get list of remapping rules from SDF + if (sdfRos->HasElement("remapping")) { + sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); + + arguments.push_back(RCL_ROS_ARGS_FLAG); + while (argument_sdf) { + std::string argument = argument_sdf->Get(); + arguments.push_back(RCL_REMAP_FLAG); + arguments.push_back(argument); + argument_sdf = argument_sdf->GetNextElement("remapping"); + } + } + } std::vector argv; for (const auto & arg : arguments) { @@ -317,15 +318,14 @@ void IgnitionROS2ControlPlugin::Configure( RCLCPP_DEBUG_STREAM( this->dataPtr->node->get_logger(), "[Ignition ROS 2 Control] Setting up controller for [" << - model.Name(_ecm) << "] (Entity=" << _entity << ")]."); + model.Name(_ecm) << "] (Entity=" << _entity << ")]."); // Get list of enabled joints auto enabledJoints = this->dataPtr->GetEnabledJoints( _entity, _ecm); - if (enabledJoints.size() == 0) - { + if (enabledJoints.size() == 0) { RCLCPP_DEBUG_STREAM( this->dataPtr->node->get_logger(), "[Ignition ROS 2 Control] There are no available Joints."); return; @@ -455,7 +455,7 @@ void IgnitionROS2ControlPlugin::PostUpdate( this->dataPtr->last_update_sim_time_ros_ = sim_time_ros; auto ign_controller_manager = std::dynamic_pointer_cast( - this->dataPtr->controller_manager_); + this->dataPtr->controller_manager_); this->dataPtr->controller_manager_->read(); this->dataPtr->controller_manager_->update(); } diff --git a/ignition_ros2_control/src/ignition_system.cpp b/ignition_ros2_control/src/ignition_system.cpp index 103a63b6..7c3e43c7 100644 --- a/ignition_ros2_control/src/ignition_system.cpp +++ b/ignition_ros2_control/src/ignition_system.cpp @@ -158,18 +158,24 @@ bool IgnitionSystem::initSim( if (hardware_info.joints[j].command_interfaces[i].name == "position") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); this->dataPtr->joints_[j].joint_control_method |= POSITION; - this->dataPtr->joints_[j].joint_pos_cmd = std::make_shared( - joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joints_[j].joint_position_cmd); + this->dataPtr->joints_[j].joint_pos_cmd = + std::make_shared( + joint_name, hardware_interface::HW_IF_POSITION, + &this->dataPtr->joints_[j].joint_position_cmd); } else if (hardware_info.joints[j].command_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); this->dataPtr->joints_[j].joint_control_method |= VELOCITY; - this->dataPtr->joints_[j].joint_vel_cmd = std::make_shared( - joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joints_[j].joint_velocity_cmd); + this->dataPtr->joints_[j].joint_vel_cmd = + std::make_shared( + joint_name, hardware_interface::HW_IF_VELOCITY, + &this->dataPtr->joints_[j].joint_velocity_cmd); } else if (hardware_info.joints[j].command_interfaces[i].name == "effort") { this->dataPtr->joints_[j].joint_control_method |= EFFORT; RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); - this->dataPtr->joints_[j].joint_eff_cmd = std::make_shared( - joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[j].joint_effort_cmd); + this->dataPtr->joints_[j].joint_eff_cmd = + std::make_shared( + joint_name, hardware_interface::HW_IF_EFFORT, + &this->dataPtr->joints_[j].joint_effort_cmd); } } @@ -178,17 +184,22 @@ bool IgnitionSystem::initSim( 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->joints_[j].joint_pos_state = std::make_shared( - joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joints_[j].joint_position); + this->dataPtr->joints_[j].joint_pos_state = + std::make_shared( + joint_name, hardware_interface::HW_IF_POSITION, + &this->dataPtr->joints_[j].joint_position); } if (hardware_info.joints[j].state_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); - this->dataPtr->joints_[j].joint_vel_state = std::make_shared( - joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joints_[j].joint_velocity); + this->dataPtr->joints_[j].joint_vel_state = + std::make_shared( + joint_name, hardware_interface::HW_IF_VELOCITY, + &this->dataPtr->joints_[j].joint_velocity); } if (hardware_info.joints[j].state_interfaces[i].name == "effort") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); - this->dataPtr->joints_[j].joint_eff_state = std::make_shared( + this->dataPtr->joints_[j].joint_eff_state = + std::make_shared( joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[j].joint_effort); } } @@ -326,7 +337,8 @@ hardware_interface::return_type IgnitionSystem::write() { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, - ignition::gazebo::components::JointPositionReset({this->dataPtr->joints_[i].joint_position})); + ignition::gazebo::components::JointPositionReset( + {this->dataPtr->joints_[i].joint_position})); const auto jointPosCmd = this->dataPtr->ecm->Component( this->dataPtr->joints_[i].sim_joint); From 5f596eca7d89fdbf4c80af182b8fcede0c0ccb9a Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 15 Jun 2021 21:26:04 +0200 Subject: [PATCH 33/50] make linters happy Signed-off-by: ahcorde --- ignition_ros2_control/src/ignition_ros2_control_plugin.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index b0c4935f..a7684c9d 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -46,6 +46,7 @@ namespace ignition_ros2_control ////////////////////////////////////////////////// class IgnitionROS2ControlPluginPrivate { +public: /// \brief Get the URDF XML from the parameter server std::string getURDF() const; From d50c181b1bf8f2ecbde1f18b6316ea8828cfdf18 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 16 Jun 2021 18:03:49 +0200 Subject: [PATCH 34/50] make linters happy Signed-off-by: ahcorde --- .../launch/cart_example_position.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ignition_ros2_control_demos/launch/cart_example_position.launch.py b/ignition_ros2_control_demos/launch/cart_example_position.launch.py index aa217d56..16037935 100644 --- a/ignition_ros2_control_demos/launch/cart_example_position.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_position.launch.py @@ -67,7 +67,8 @@ def generate_launch_description(): ) load_joint_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', 'joint_trajectory_controller'], + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + 'joint_trajectory_controller'], output='screen' ) From 0554ee5ba63939ab5965409ad8ab38da35e72440 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Fri, 25 Jun 2021 01:13:47 -0700 Subject: [PATCH 35/50] Citadel support (#4) * Citadel compatible Signed-off-by: Louise Poubel * cleanup Signed-off-by: Louise Poubel * make linters happy Signed-off-by: ahcorde * make linters happy Signed-off-by: ahcorde Co-authored-by: ahcorde --- .github/workflows/ci.yaml | 2 +- Dockerfile/Dockerfile | 2 +- ignition_ros2_control/package.xml | 2 +- .../src/ignition_ros2_control_plugin.cpp | 52 ++++++++----------- 4 files changed, 25 insertions(+), 33 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index fa975d6d..916359eb 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -40,7 +40,7 @@ jobs: rosdep init rosdep update rosdep install --from-paths ./ -i -y --rosdistro foxy \ - --ignore-src --skip-keys "ignition-gazebo5 ignition-plugin1" + --ignore-src --skip-keys "ignition-gazebo5" - name: Build project id: build run: | diff --git a/Dockerfile/Dockerfile b/Dockerfile/Dockerfile index fca0edeb..e949a5c8 100644 --- a/Dockerfile/Dockerfile +++ b/Dockerfile/Dockerfile @@ -29,7 +29,7 @@ RUN mkdir -p /home/ros2_ws/src \ && git clone https://github.com/ignitionrobotics/ign_ros2_control/ -b ahcorde/ign_ros2_control \ && rosdep update \ && rosdep install --from-paths ./ -i -y --rosdistro foxy \ - --ignore-src --skip-keys "ignition-gazebo5 ignition-plugin1" + --ignore-src --skip-keys "ignition-gazebo5" RUN cd /home/ros2_ws/ \ && . /opt/ros/foxy/setup.sh \ diff --git a/ignition_ros2_control/package.xml b/ignition_ros2_control/package.xml index 8d1e9c8b..a3277a76 100644 --- a/ignition_ros2_control/package.xml +++ b/ignition_ros2_control/package.xml @@ -12,7 +12,7 @@ ament_index_cpp ignition-gazebo3 ignition-gazebo5 - ignition-plugin1 + ignition-plugin pluginlib rclcpp yaml_cpp_vendor diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index a7684c9d..bbc577b8 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -22,7 +22,6 @@ #include #include #include -#include #include #include @@ -396,47 +395,40 @@ void IgnitionROS2ControlPlugin::Configure( return; } - auto worldEntity = - _ecm.EntityByComponents(ignition::gazebo::components::World()); - - auto physicsComp = - _ecm.Component(worldEntity); - const auto & physicsParams = physicsComp->Data(); - const auto newStepSize = - std::chrono::duration(physicsParams.MaxStepSize()); - - // Get the Gazebo simulation period - rclcpp::Duration gazebo_period( - std::chrono::duration_cast( - std::chrono::duration(newStepSize))); - auto cm_update_rate = this->dataPtr->controller_manager_->get_parameter("update_rate").as_int(); this->dataPtr->control_period_ = rclcpp::Duration( std::chrono::duration_cast( std::chrono::duration(1.0 / static_cast(cm_update_rate)))); - // Check the period against the simulation period - if (this->dataPtr->control_period_ < gazebo_period) { - RCLCPP_ERROR_STREAM( - this->dataPtr->node->get_logger(), - "Desired controller update period (" << this->dataPtr->control_period_.seconds() << - " s) is faster than the gazebo simulation period (" << - gazebo_period.seconds() << " s)."); - } else if (this->dataPtr->control_period_ > gazebo_period) { - RCLCPP_WARN_STREAM( - this->dataPtr->node->get_logger(), - " Desired controller update period (" << this->dataPtr->control_period_.seconds() << - " s) is slower than the gazebo simulation period (" << - gazebo_period.seconds() << " s)."); - } this->dataPtr->entity_ = _entity; } ////////////////////////////////////////////////// void IgnitionROS2ControlPlugin::PreUpdate( - const ignition::gazebo::UpdateInfo & /*_info*/, + const ignition::gazebo::UpdateInfo & _info, ignition::gazebo::EntityComponentManager & /*_ecm*/) { + static bool warned{false}; + if (!warned) { + rclcpp::Duration gazebo_period(_info.dt); + + // Check the period against the simulation period + if (this->dataPtr->control_period_ < _info.dt) { + RCLCPP_ERROR_STREAM( + this->dataPtr->node->get_logger(), + "Desired controller update period (" << this->dataPtr->control_period_.seconds() << + " s) is faster than the gazebo simulation period (" << + gazebo_period.seconds() << " s)."); + } else if (this->dataPtr->control_period_ > gazebo_period) { + RCLCPP_WARN_STREAM( + this->dataPtr->node->get_logger(), + " Desired controller update period (" << this->dataPtr->control_period_.seconds() << + " s) is slower than the gazebo simulation period (" << + gazebo_period.seconds() << " s)."); + } + warned = true; + } + // Always set commands on joints, otherwise at low control frequencies the joints tremble // as they are updated at a fraction of gazebo sim time this->dataPtr->controller_manager_->write(); From 6b8a22717d83fd3ce35a94374b6299ac81d31a22 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 20 Sep 2021 17:15:34 +0200 Subject: [PATCH 36/50] Added some fixes Signed-off-by: ahcorde --- .../ignition_ros2_control/ignition_system.hpp | 3 + ignition_ros2_control/src/ignition_system.cpp | 237 +++++++++++++++++- .../config/cartpole_controller_velocity.yaml | 8 + .../examples/example_velocity.cpp | 4 +- .../launch/cart_example_velocity.launch.py | 14 +- .../urdf/test_cart_velocity.xacro.urdf | 94 ++++++- 6 files changed, 347 insertions(+), 13 deletions(-) diff --git a/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp b/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp index 00dc112f..19ce9af8 100644 --- a/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp +++ b/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp @@ -64,6 +64,9 @@ class IgnitionSystem : public IgnitionSystemInterface ignition::gazebo::EntityComponentManager & _ecm) override; private: + void registerSensors( + const hardware_interface::HardwareInfo & hardware_info); + /// \brief Private data class std::unique_ptr dataPtr; }; diff --git a/ignition_ros2_control/src/ignition_system.cpp b/ignition_ros2_control/src/ignition_system.cpp index 7c3e43c7..3fbde7d9 100644 --- a/ignition_ros2_control/src/ignition_system.cpp +++ b/ignition_ros2_control/src/ignition_system.cpp @@ -12,12 +12,18 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include #include #include #include #include +#include +#include +#include +#include #include #include @@ -75,6 +81,23 @@ struct jointData std::shared_ptr joint_eff_cmd; }; +struct ImuData +{ + /// \brief imu's name. + std::string name; + + std::string state_interface; + + /// \brief handles to the imu from within Gazebo + ignition::gazebo::Entity sim_imu_sensors_; + + /// \brief An array per IMU with 4 orientation, 3 angular velocity and 3 linear acceleration + std::array imu_sensor_data_; + + /// \brief The ROS2 Control state interface for the current imu readings + std::vector> imu_state_; +}; + class ignition_ros2_control::IgnitionSystemPrivate { public: @@ -90,6 +113,18 @@ class ignition_ros2_control::IgnitionSystemPrivate /// \brief vector with the joint's names. std::vector joints_; + /// \brief vector with the imus . + std::vector imus_; + + /// \brief handles to the FT sensors from within Gazebo + std::vector sim_ft_sensors_; + + /// \brief An array per FT sensor for 3D force and torquee + std::vector> ft_sensor_data_; + + /// \brief The ROS2 Control state interface for the current FT sensor readings + std::vector> ft_sensor_state_; + ignition::gazebo::EntityComponentManager * ecm; }; @@ -109,7 +144,7 @@ bool IgnitionSystem::initSim( this->dataPtr->ecm = &_ecm; this->dataPtr->n_dof_ = hardware_info.joints.size(); - RCLCPP_ERROR(this->nh_->get_logger(), "n_dof_ %d", this->dataPtr->n_dof_); + RCLCPP_DEBUG(this->nh_->get_logger(), "n_dof_ %d", this->dataPtr->n_dof_); this->dataPtr->joints_.resize(this->dataPtr->n_dof_); @@ -204,9 +239,81 @@ bool IgnitionSystem::initSim( } } } + + registerSensors(hardware_info); + return true; } +void IgnitionSystem::registerSensors( + const hardware_interface::HardwareInfo & hardware_info) +{ + // Collect gazebo sensor handles + size_t n_sensors = hardware_info.sensors.size(); + std::vector sensor_components_; + + for (unsigned int j = 0; j < n_sensors; j++) { + hardware_interface::ComponentInfo component = hardware_info.sensors[j]; + sensor_components_.push_back(component); + } + // This is split in two steps: Count the number and type of sensor and associate the interfaces + // So we have resize only once the structures where the data will be stored, and we can safely + // use pointers to the structures + + this->dataPtr->imus_.resize(n_sensors); + + int imuIndex = 0; + + this->dataPtr->ecm->Each( + [&](const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::Imu *, + const ignition::gazebo::components::Name *_name) -> bool + { + if (imuIndex > 0 ) + return true; + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data()); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "\tState:"); + this->dataPtr->imus_[imuIndex].name = _name->Data(); + this->dataPtr->imus_[imuIndex].sim_imu_sensors_ = _entity; + + hardware_interface::ComponentInfo component; + for (auto & comp: sensor_components_) + { + if (comp.name == _name->Data()) + { + component = comp; + } + } + + static const std::map interface_name_map = { + {"orientation.x", 0}, + {"orientation.y", 1}, + {"orientation.z", 2}, + {"orientation.w", 3}, + {"angular_velocity.x", 4}, + {"angular_velocity.y", 5}, + {"angular_velocity.z", 6}, + {"linear_acceleration.x", 7}, + {"linear_acceleration.y", 8}, + {"linear_acceleration.z", 9}, + }; + for (const auto & state_interface : component.state_interfaces) { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << state_interface.name); + + size_t data_index = interface_name_map.at(state_interface.name); + this->dataPtr->imus_[imuIndex].imu_state_.push_back( + std::make_shared( + this->dataPtr->imus_[imuIndex].name, + state_interface.name, + &this->dataPtr->imus_[imuIndex].imu_sensor_data_[data_index])); + } + imuIndex++; + return true; + }); +} + hardware_interface::return_type IgnitionSystem::configure(const hardware_interface::HardwareInfo & actuator_info) { @@ -242,6 +349,29 @@ IgnitionSystem::export_state_interfaces() hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[i].joint_effort)); } + + static const std::vector interface_names = { + "orientation.x", + "orientation.y", + "orientation.z", + "orientation.w", + "angular_velocity.x", + "angular_velocity.y", + "angular_velocity.z", + "linear_acceleration.x", + "linear_acceleration.y", + "linear_acceleration.z" + }; + + for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { + for (unsigned int j = 0; j < this->dataPtr->imus_[i].imu_state_.size(); ++j) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + this->dataPtr->imus_[i].name, + interface_names[j], + &this->dataPtr->imus_[i].imu_sensor_data_[j])); + } + } return state_interfaces; } @@ -292,7 +422,8 @@ hardware_interface::return_type IgnitionSystem::read() // Get the joint velocity const auto * jointVelocity = this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + this->dataPtr->joints_[i].sim_joint); + // std::cerr << "this->dataPtr->joints_[i].sim_joint " << this->dataPtr->joints_[i].sim_joint << '\n'; // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 // Get the joint force @@ -303,12 +434,112 @@ hardware_interface::return_type IgnitionSystem::read() // Get the joint position const auto * jointPositions = this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + this->dataPtr->joints_[i].sim_joint); this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0]; this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0]; // this->dataPtr->joint_effort_[j] = jointForce->Data()[0]; } + + for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { + std::cerr << "this->dataPtr->imus_[i].sim_imu_sensors_ " << this->dataPtr->imus_[i].sim_imu_sensors_ << '\n'; + const auto * worldPose = this->dataPtr->ecm->Component( + this->dataPtr->imus_[i].sim_imu_sensors_); + if (!worldPose) + { + std::cerr << "fail! worldPose" << '\n'; + continue; + } + this->dataPtr->imus_[i].imu_sensor_data_[0] = worldPose->Data().Rot().X(); + this->dataPtr->imus_[i].imu_sensor_data_[1] = worldPose->Data().Rot().Y(); + this->dataPtr->imus_[i].imu_sensor_data_[2] = worldPose->Data().Rot().Z(); + this->dataPtr->imus_[i].imu_sensor_data_[3] = worldPose->Data().Rot().W(); + + // std::cerr << "worldPose->Data() " << std::setprecision(5) << worldPose->Data().Rot().X() << "\t" << worldPose->Data().Rot().X() << "\t" << worldPose->Data().Rot().Z() << "\t" << worldPose->Data().Rot().W() << '\n'; + + const auto * angularVel = this->dataPtr->ecm->Component( + this->dataPtr->imus_[i].sim_imu_sensors_); + if (!angularVel) + { + std::cerr << "fail! angularVel" << '\n'; + continue; + } + this->dataPtr->imus_[i].imu_sensor_data_[4] = angularVel->Data().X(); + this->dataPtr->imus_[i].imu_sensor_data_[5] = angularVel->Data().Y(); + this->dataPtr->imus_[i].imu_sensor_data_[6] = angularVel->Data().Z(); + // std::cerr << "angularVel->Data() " << std::setprecision(5) << angularVel->Data().X() << "\t" << angularVel->Data().X() << "\t" << angularVel->Data().Z() << '\n'; + + const auto * _linearAcc = this->dataPtr->ecm->Component( + this->dataPtr->imus_[i].sim_imu_sensors_); + if (!_linearAcc) + { + std::cerr << "fail!" << '\n'; + continue; + } + this->dataPtr->imus_[i].imu_sensor_data_[7] = _linearAcc->Data().X(); + this->dataPtr->imus_[i].imu_sensor_data_[8] = _linearAcc->Data().Y(); + this->dataPtr->imus_[i].imu_sensor_data_[9] = _linearAcc->Data().Z(); + std::cerr << "linearAcc->Data() " << std::setprecision(5) << _linearAcc->Data().X() << "\t" << _linearAcc->Data().X() << "\t" << _linearAcc->Data().Z() << '\n'; + } + // this->dataPtr->ecm->Each( + // [&](const ignition::gazebo::Entity &_entity, + // const ignition::gazebo::components::Imu *, + // const ignition::gazebo::components::Name *_name, + // const ignition::gazebo::components::WorldPose *_worldPose, + // const ignition::gazebo::components::AngularVelocity *_angularVel, + // const ignition::gazebo::components::LinearAcceleration *_linearAcc) -> bool + // { + // std::cerr << "worldPose->Data() " << std::setprecision(5) << _entity << "\t" << _name->Data() + // << "\t" << _worldPose->Data().Rot().X() << "\t" << _worldPose->Data().Rot().X() + // << "\t" << _worldPose->Data().Rot().Z() << "\t" << _worldPose->Data().Rot().W() << '\n'; + // + // const auto * parentEntity = this->dataPtr->ecm->Component( + // _entity); + // if (parentEntity) + // { + // const auto * worldPoseParent = this->dataPtr->ecm->Component( + // parentEntity->Data()); + // if (worldPoseParent) + // { + // std::cerr << "worldPose->Data() Parent " << std::setprecision(5) << parentEntity->Data() + // << "\t" << worldPoseParent->Data().Rot().X() << "\t" << worldPoseParent->Data().Rot().X() + // << "\t" << worldPoseParent->Data().Rot().Z() << "\t" << worldPoseParent->Data().Rot().W() << '\n'; + // } + // const auto * nameParent = this->dataPtr->ecm->Component( + // parentEntity->Data()); + // if (nameParent) + // { + // std::cerr << "nameParent " << std::setprecision(5) << parentEntity->Data() << "\t" << nameParent->Data() << "\n"; + // } + // } + // + // std::cerr << "_linearAcc->Data() " << std::setprecision(5) << _entity << "\t" << _name->Data() << "\t" << _linearAcc->Data().X() << "\t" << _linearAcc->Data().X() << "\t" << _linearAcc->Data().Z() << '\n'; + // std::cerr << "_angularVel->Data() " << std::setprecision(5) << _entity << "\t" << _name->Data() << "\t" << _angularVel->Data().X() << "\t" << _angularVel->Data().X() << "\t" << _angularVel->Data().Z() << '\n'; + // // unsigned int index = 0; + // // for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { + // // if (this->dataPtr->imus_[i].name == _name->Data()) { + // // index = i; + // // break; + // // } + // // } + // // this->dataPtr->imus_[index].imu_sensor_data_[0] = _worldPose->Data().Rot().X(); + // // this->dataPtr->imus_[index].imu_sensor_data_[1] = _worldPose->Data().Rot().Y(); + // // this->dataPtr->imus_[index].imu_sensor_data_[2] = _worldPose->Data().Rot().Z(); + // // this->dataPtr->imus_[index].imu_sensor_data_[3] = _worldPose->Data().Rot().W(); + // // + // // this->dataPtr->imus_[index].imu_sensor_data_[4] = _angularVel->Data().X(); + // // this->dataPtr->imus_[index].imu_sensor_data_[5] = _angularVel->Data().Y(); + // // this->dataPtr->imus_[index].imu_sensor_data_[6] = _angularVel->Data().Z(); + // // + // // this->dataPtr->imus_[index].imu_sensor_data_[7] = _linearAcc->Data().X(); + // // this->dataPtr->imus_[index].imu_sensor_data_[8] = _linearAcc->Data().Y(); + // // this->dataPtr->imus_[index].imu_sensor_data_[9] = _linearAcc->Data().Z(); + // return true; + // }); return hardware_interface::return_type::OK; } diff --git a/ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml b/ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml index 582b4c85..8d6b6ab0 100644 --- a/ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml +++ b/ignition_ros2_control_demos/config/cartpole_controller_velocity.yaml @@ -8,6 +8,9 @@ controller_manager: joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster + imu_sensor_broadcaster: + type: imu_sensor_broadcaster/IMUSensorBroadcaster + velocity_controller: ros__parameters: joints: @@ -17,3 +20,8 @@ velocity_controller: state_interfaces: - position - velocity + +imu_sensor_broadcaster: + ros__parameters: + sensor_name: cart_imu_sensor + frame_id: imu diff --git a/ignition_ros2_control_demos/examples/example_velocity.cpp b/ignition_ros2_control_demos/examples/example_velocity.cpp index fa336e6a..57fa7e7a 100644 --- a/ignition_ros2_control_demos/examples/example_velocity.cpp +++ b/ignition_ros2_control_demos/examples/example_velocity.cpp @@ -39,11 +39,11 @@ int main(int argc, char * argv[]) commands.data[0] = 1; publisher->publish(commands); - std::this_thread::sleep_for(1s); + std::this_thread::sleep_for(5s); commands.data[0] = -1; publisher->publish(commands); - std::this_thread::sleep_for(1s); + std::this_thread::sleep_for(5s); commands.data[0] = 0; publisher->publish(commands); diff --git a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py index 7c243b49..7487f7f9 100644 --- a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -71,13 +71,19 @@ def generate_launch_description(): output='screen' ) + load_imu_sensor_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'start', + 'imu_sensor_broadcaster'], + output='screen' + ) + return LaunchDescription([ # Launch gazebo environment IncludeLaunchDescription( PythonLaunchDescriptionSource( [os.path.join(get_package_share_directory('ros_ign_gazebo'), 'launch', 'ign_gazebo.launch.py')]), - launch_arguments=[('ign_args', [' -r -v 3 empty.sdf'])]), + launch_arguments=[('ign_args', [' -r -v 4 empty.sdf'])]), RegisterEventHandler( event_handler=OnProcessExit( target_action=ignition_spawn_entity, @@ -90,6 +96,12 @@ def generate_launch_description(): on_exit=[load_joint_trajectory_controller], ) ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_trajectory_controller, + on_exit=[load_imu_sensor_broadcaster], + ) + ), node_robot_state_publisher, ignition_spawn_entity, # Launch Arguments diff --git a/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index d788674a..d713bab4 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -4,13 +4,12 @@ - - + @@ -22,18 +21,52 @@ + + + + + + + + + + + + + + + + + + + - - - - - + + + + + + + + + + + + + + + + + + + + @@ -42,6 +75,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + 10.0 + true + imu + + @@ -56,6 +124,18 @@ + + + + + + + + + + + + From c1573544738683afed20310cfc00a25f4717d5c3 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Wed, 22 Sep 2021 19:25:54 +0200 Subject: [PATCH 37/50] fixed IMU Signed-off-by: ahcorde --- ignition_ros2_control/src/ignition_system.cpp | 341 +++++------------- .../examples/example_velocity.cpp | 4 +- .../urdf/test_cart_velocity.xacro.urdf | 4 + 3 files changed, 103 insertions(+), 246 deletions(-) diff --git a/ignition_ros2_control/src/ignition_system.cpp b/ignition_ros2_control/src/ignition_system.cpp index 3fbde7d9..61fe368c 100644 --- a/ignition_ros2_control/src/ignition_system.cpp +++ b/ignition_ros2_control/src/ignition_system.cpp @@ -12,6 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "ignition_ros2_control/ignition_system.hpp" + +#include + #include #include #include @@ -24,6 +28,9 @@ #include #include #include +#include + +#include #include #include @@ -31,8 +38,6 @@ #include #include -#include "ignition_ros2_control/ignition_system.hpp" - struct jointData { /// \brief vector with the joint's names. @@ -61,43 +66,41 @@ struct jointData /// \brief vector with the control method defined in the URDF for each joint. ignition_ros2_control::IgnitionSystemInterface::ControlMethod joint_control_method; - - /// \brief The current positions of the joints - std::shared_ptr joint_pos_state; - - /// \brief The current velocities of the joints - std::shared_ptr joint_vel_state; - - /// \brief The current effort forces applied to the joints - std::shared_ptr joint_eff_state; - - /// \brief The position command interfaces of the joints - std::shared_ptr joint_pos_cmd; - - /// \brief The velocity command interfaces of the joints - std::shared_ptr joint_vel_cmd; - - /// \brief The effort command interfaces of the joints - std::shared_ptr joint_eff_cmd; }; -struct ImuData +class ImuData { +public: /// \brief imu's name. - std::string name; + std::string name{}; - std::string state_interface; + /// \brief imu's topic name. + std::string topicName{}; /// \brief handles to the imu from within Gazebo - ignition::gazebo::Entity sim_imu_sensors_; + ignition::gazebo::Entity sim_imu_sensors_ = ignition::gazebo::kNullEntity; /// \brief An array per IMU with 4 orientation, 3 angular velocity and 3 linear acceleration std::array imu_sensor_data_; - /// \brief The ROS2 Control state interface for the current imu readings - std::vector> imu_state_; + /// \brief callback to get the IMU topic values + void OnIMU(const ignition::msgs::IMU & _msg); }; +void ImuData::OnIMU(const ignition::msgs::IMU & _msg) +{ + this->imu_sensor_data_[0] = _msg.orientation().x(); + this->imu_sensor_data_[1] = _msg.orientation().y(); + this->imu_sensor_data_[2] = _msg.orientation().z(); + this->imu_sensor_data_[3] = _msg.orientation().w(); + this->imu_sensor_data_[4] = _msg.angular_velocity().x(); + this->imu_sensor_data_[5] = _msg.angular_velocity().y(); + this->imu_sensor_data_[6] = _msg.angular_velocity().z(); + this->imu_sensor_data_[7] = _msg.linear_acceleration().x(); + this->imu_sensor_data_[8] = _msg.linear_acceleration().y(); + this->imu_sensor_data_[9] = _msg.linear_acceleration().z(); +} + class ignition_ros2_control::IgnitionSystemPrivate { public: @@ -114,18 +117,20 @@ class ignition_ros2_control::IgnitionSystemPrivate std::vector joints_; /// \brief vector with the imus . - std::vector imus_; - - /// \brief handles to the FT sensors from within Gazebo - std::vector sim_ft_sensors_; + std::vector> imus_; - /// \brief An array per FT sensor for 3D force and torquee - std::vector> ft_sensor_data_; + /// \brief state interfaces that will be exported to the Resource Manager + std::vector state_interfaces_; - /// \brief The ROS2 Control state interface for the current FT sensor readings - std::vector> ft_sensor_state_; + /// \brief command interfaces that will be exported to the Resource Manager + std::vector command_interfaces_; ignition::gazebo::EntityComponentManager * ecm; + + /// \brief Ignition communication node. + +public: + ignition::transport::Node node; }; namespace ignition_ros2_control @@ -193,23 +198,23 @@ bool IgnitionSystem::initSim( if (hardware_info.joints[j].command_interfaces[i].name == "position") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); this->dataPtr->joints_[j].joint_control_method |= POSITION; - this->dataPtr->joints_[j].joint_pos_cmd = - std::make_shared( - joint_name, hardware_interface::HW_IF_POSITION, + this->dataPtr->command_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_POSITION, &this->dataPtr->joints_[j].joint_position_cmd); } else if (hardware_info.joints[j].command_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); this->dataPtr->joints_[j].joint_control_method |= VELOCITY; - this->dataPtr->joints_[j].joint_vel_cmd = - std::make_shared( - joint_name, hardware_interface::HW_IF_VELOCITY, + this->dataPtr->command_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joints_[j].joint_velocity_cmd); } else if (hardware_info.joints[j].command_interfaces[i].name == "effort") { this->dataPtr->joints_[j].joint_control_method |= EFFORT; RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); - this->dataPtr->joints_[j].joint_eff_cmd = - std::make_shared( - joint_name, hardware_interface::HW_IF_EFFORT, + this->dataPtr->command_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[j].joint_effort_cmd); } } @@ -219,23 +224,24 @@ bool IgnitionSystem::initSim( 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->joints_[j].joint_pos_state = - std::make_shared( - joint_name, hardware_interface::HW_IF_POSITION, + this->dataPtr->state_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_POSITION, &this->dataPtr->joints_[j].joint_position); } if (hardware_info.joints[j].state_interfaces[i].name == "velocity") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); - this->dataPtr->joints_[j].joint_vel_state = - std::make_shared( - joint_name, hardware_interface::HW_IF_VELOCITY, + this->dataPtr->state_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joints_[j].joint_velocity); } if (hardware_info.joints[j].state_interfaces[i].name == "effort") { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); - this->dataPtr->joints_[j].joint_eff_state = - std::make_shared( - joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[j].joint_effort); + this->dataPtr->state_interfaces_.emplace_back( + joint_name, + hardware_interface::HW_IF_EFFORT, + &this->dataPtr->joints_[j].joint_effort); } } } @@ -260,29 +266,29 @@ void IgnitionSystem::registerSensors( // So we have resize only once the structures where the data will be stored, and we can safely // use pointers to the structures - this->dataPtr->imus_.resize(n_sensors); - - int imuIndex = 0; - this->dataPtr->ecm->Each( - [&](const ignition::gazebo::Entity &_entity, - const ignition::gazebo::components::Imu *, - const ignition::gazebo::components::Name *_name) -> bool + ignition::gazebo::components::Name>( + [&](const ignition::gazebo::Entity & _entity, + const ignition::gazebo::components::Imu *, + const ignition::gazebo::components::Name * _name) -> bool { - if (imuIndex > 0 ) - return true; + auto imuData = std::make_shared(); RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data()); + + auto sensorTopicComp = this->dataPtr->ecm->Component< + ignition::gazebo::components::SensorTopic>(_entity); + if (sensorTopicComp) { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data()); + } + RCLCPP_INFO_STREAM( this->nh_->get_logger(), "\tState:"); - this->dataPtr->imus_[imuIndex].name = _name->Data(); - this->dataPtr->imus_[imuIndex].sim_imu_sensors_ = _entity; + imuData->name = _name->Data(); + imuData->sim_imu_sensors_ = _entity; hardware_interface::ComponentInfo component; - for (auto & comp: sensor_components_) - { - if (comp.name == _name->Data()) - { + for (auto & comp : sensor_components_) { + if (comp.name == _name->Data()) { component = comp; } } @@ -299,17 +305,17 @@ void IgnitionSystem::registerSensors( {"linear_acceleration.y", 8}, {"linear_acceleration.z", 9}, }; + for (const auto & state_interface : component.state_interfaces) { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << state_interface.name); size_t data_index = interface_name_map.at(state_interface.name); - this->dataPtr->imus_[imuIndex].imu_state_.push_back( - std::make_shared( - this->dataPtr->imus_[imuIndex].name, - state_interface.name, - &this->dataPtr->imus_[imuIndex].imu_sensor_data_[data_index])); + this->dataPtr->state_interfaces_.emplace_back( + imuData->name, + state_interface.name, + &imuData->imu_sensor_data_[data_index]); } - imuIndex++; + this->dataPtr->imus_.push_back(imuData); return true; }); } @@ -326,82 +332,13 @@ IgnitionSystem::configure(const hardware_interface::HardwareInfo & actuator_info std::vector IgnitionSystem::export_state_interfaces() { - std::vector state_interfaces; - - for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - this->dataPtr->joints_[i].name, - hardware_interface::HW_IF_POSITION, - &this->dataPtr->joints_[i].joint_position)); - } - for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - this->dataPtr->joints_[i].name, - hardware_interface::HW_IF_VELOCITY, - &this->dataPtr->joints_[i].joint_velocity)); - } - for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - this->dataPtr->joints_[i].name, - hardware_interface::HW_IF_EFFORT, - &this->dataPtr->joints_[i].joint_effort)); - } - - static const std::vector interface_names = { - "orientation.x", - "orientation.y", - "orientation.z", - "orientation.w", - "angular_velocity.x", - "angular_velocity.y", - "angular_velocity.z", - "linear_acceleration.x", - "linear_acceleration.y", - "linear_acceleration.z" - }; - - for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { - for (unsigned int j = 0; j < this->dataPtr->imus_[i].imu_state_.size(); ++j) { - state_interfaces.emplace_back( - hardware_interface::StateInterface( - this->dataPtr->imus_[i].name, - interface_names[j], - &this->dataPtr->imus_[i].imu_sensor_data_[j])); - } - } - return state_interfaces; + return std::move(this->dataPtr->state_interfaces_); } std::vector IgnitionSystem::export_command_interfaces() { - std::vector command_interfaces; - - for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - this->dataPtr->joints_[i].name, - hardware_interface::HW_IF_POSITION, - &this->dataPtr->joints_[i].joint_position_cmd)); - } - for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - this->dataPtr->joints_[i].name, - hardware_interface::HW_IF_VELOCITY, - &this->dataPtr->joints_[i].joint_velocity_cmd)); - } - for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - command_interfaces.emplace_back( - hardware_interface::CommandInterface( - this->dataPtr->joints_[i].name, - hardware_interface::HW_IF_EFFORT, - &this->dataPtr->joints_[i].joint_effort_cmd)); - } - return command_interfaces; + return std::move(this->dataPtr->command_interfaces_); } hardware_interface::return_type IgnitionSystem::start() @@ -422,8 +359,7 @@ hardware_interface::return_type IgnitionSystem::read() // Get the joint velocity const auto * jointVelocity = this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); - // std::cerr << "this->dataPtr->joints_[i].sim_joint " << this->dataPtr->joints_[i].sim_joint << '\n'; + this->dataPtr->joints_[i].sim_joint); // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124 // Get the joint force @@ -434,7 +370,7 @@ hardware_interface::return_type IgnitionSystem::read() // Get the joint position const auto * jointPositions = this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + this->dataPtr->joints_[i].sim_joint); this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0]; this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0]; @@ -442,104 +378,21 @@ hardware_interface::return_type IgnitionSystem::read() } for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { - std::cerr << "this->dataPtr->imus_[i].sim_imu_sensors_ " << this->dataPtr->imus_[i].sim_imu_sensors_ << '\n'; - const auto * worldPose = this->dataPtr->ecm->Component( - this->dataPtr->imus_[i].sim_imu_sensors_); - if (!worldPose) - { - std::cerr << "fail! worldPose" << '\n'; - continue; - } - this->dataPtr->imus_[i].imu_sensor_data_[0] = worldPose->Data().Rot().X(); - this->dataPtr->imus_[i].imu_sensor_data_[1] = worldPose->Data().Rot().Y(); - this->dataPtr->imus_[i].imu_sensor_data_[2] = worldPose->Data().Rot().Z(); - this->dataPtr->imus_[i].imu_sensor_data_[3] = worldPose->Data().Rot().W(); - - // std::cerr << "worldPose->Data() " << std::setprecision(5) << worldPose->Data().Rot().X() << "\t" << worldPose->Data().Rot().X() << "\t" << worldPose->Data().Rot().Z() << "\t" << worldPose->Data().Rot().W() << '\n'; - - const auto * angularVel = this->dataPtr->ecm->Component( - this->dataPtr->imus_[i].sim_imu_sensors_); - if (!angularVel) - { - std::cerr << "fail! angularVel" << '\n'; - continue; - } - this->dataPtr->imus_[i].imu_sensor_data_[4] = angularVel->Data().X(); - this->dataPtr->imus_[i].imu_sensor_data_[5] = angularVel->Data().Y(); - this->dataPtr->imus_[i].imu_sensor_data_[6] = angularVel->Data().Z(); - // std::cerr << "angularVel->Data() " << std::setprecision(5) << angularVel->Data().X() << "\t" << angularVel->Data().X() << "\t" << angularVel->Data().Z() << '\n'; - - const auto * _linearAcc = this->dataPtr->ecm->Component( - this->dataPtr->imus_[i].sim_imu_sensors_); - if (!_linearAcc) - { - std::cerr << "fail!" << '\n'; - continue; + if (this->dataPtr->imus_[i]->topicName.empty()) { + auto sensorTopicComp = this->dataPtr->ecm->Component< + ignition::gazebo::components::SensorTopic>(this->dataPtr->imus_[i]->sim_imu_sensors_); + if (sensorTopicComp) { + this->dataPtr->imus_[i]->topicName = sensorTopicComp->Data(); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "IMU " << this->dataPtr->imus_[i]->name << + " has a topic name: " << sensorTopicComp->Data()); + + this->dataPtr->node.Subscribe( + this->dataPtr->imus_[i]->topicName, &ImuData::OnIMU, + this->dataPtr->imus_[i].get()); + } } - this->dataPtr->imus_[i].imu_sensor_data_[7] = _linearAcc->Data().X(); - this->dataPtr->imus_[i].imu_sensor_data_[8] = _linearAcc->Data().Y(); - this->dataPtr->imus_[i].imu_sensor_data_[9] = _linearAcc->Data().Z(); - std::cerr << "linearAcc->Data() " << std::setprecision(5) << _linearAcc->Data().X() << "\t" << _linearAcc->Data().X() << "\t" << _linearAcc->Data().Z() << '\n'; } - // this->dataPtr->ecm->Each( - // [&](const ignition::gazebo::Entity &_entity, - // const ignition::gazebo::components::Imu *, - // const ignition::gazebo::components::Name *_name, - // const ignition::gazebo::components::WorldPose *_worldPose, - // const ignition::gazebo::components::AngularVelocity *_angularVel, - // const ignition::gazebo::components::LinearAcceleration *_linearAcc) -> bool - // { - // std::cerr << "worldPose->Data() " << std::setprecision(5) << _entity << "\t" << _name->Data() - // << "\t" << _worldPose->Data().Rot().X() << "\t" << _worldPose->Data().Rot().X() - // << "\t" << _worldPose->Data().Rot().Z() << "\t" << _worldPose->Data().Rot().W() << '\n'; - // - // const auto * parentEntity = this->dataPtr->ecm->Component( - // _entity); - // if (parentEntity) - // { - // const auto * worldPoseParent = this->dataPtr->ecm->Component( - // parentEntity->Data()); - // if (worldPoseParent) - // { - // std::cerr << "worldPose->Data() Parent " << std::setprecision(5) << parentEntity->Data() - // << "\t" << worldPoseParent->Data().Rot().X() << "\t" << worldPoseParent->Data().Rot().X() - // << "\t" << worldPoseParent->Data().Rot().Z() << "\t" << worldPoseParent->Data().Rot().W() << '\n'; - // } - // const auto * nameParent = this->dataPtr->ecm->Component( - // parentEntity->Data()); - // if (nameParent) - // { - // std::cerr << "nameParent " << std::setprecision(5) << parentEntity->Data() << "\t" << nameParent->Data() << "\n"; - // } - // } - // - // std::cerr << "_linearAcc->Data() " << std::setprecision(5) << _entity << "\t" << _name->Data() << "\t" << _linearAcc->Data().X() << "\t" << _linearAcc->Data().X() << "\t" << _linearAcc->Data().Z() << '\n'; - // std::cerr << "_angularVel->Data() " << std::setprecision(5) << _entity << "\t" << _name->Data() << "\t" << _angularVel->Data().X() << "\t" << _angularVel->Data().X() << "\t" << _angularVel->Data().Z() << '\n'; - // // unsigned int index = 0; - // // for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { - // // if (this->dataPtr->imus_[i].name == _name->Data()) { - // // index = i; - // // break; - // // } - // // } - // // this->dataPtr->imus_[index].imu_sensor_data_[0] = _worldPose->Data().Rot().X(); - // // this->dataPtr->imus_[index].imu_sensor_data_[1] = _worldPose->Data().Rot().Y(); - // // this->dataPtr->imus_[index].imu_sensor_data_[2] = _worldPose->Data().Rot().Z(); - // // this->dataPtr->imus_[index].imu_sensor_data_[3] = _worldPose->Data().Rot().W(); - // // - // // this->dataPtr->imus_[index].imu_sensor_data_[4] = _angularVel->Data().X(); - // // this->dataPtr->imus_[index].imu_sensor_data_[5] = _angularVel->Data().Y(); - // // this->dataPtr->imus_[index].imu_sensor_data_[6] = _angularVel->Data().Z(); - // // - // // this->dataPtr->imus_[index].imu_sensor_data_[7] = _linearAcc->Data().X(); - // // this->dataPtr->imus_[index].imu_sensor_data_[8] = _linearAcc->Data().Y(); - // // this->dataPtr->imus_[index].imu_sensor_data_[9] = _linearAcc->Data().Z(); - // return true; - // }); return hardware_interface::return_type::OK; } diff --git a/ignition_ros2_control_demos/examples/example_velocity.cpp b/ignition_ros2_control_demos/examples/example_velocity.cpp index 57fa7e7a..fa336e6a 100644 --- a/ignition_ros2_control_demos/examples/example_velocity.cpp +++ b/ignition_ros2_control_demos/examples/example_velocity.cpp @@ -39,11 +39,11 @@ int main(int argc, char * argv[]) commands.data[0] = 1; publisher->publish(commands); - std::this_thread::sleep_for(5s); + std::this_thread::sleep_for(1s); commands.data[0] = -1; publisher->publish(commands); - std::this_thread::sleep_for(5s); + std::this_thread::sleep_for(1s); commands.data[0] = 0; publisher->publish(commands); diff --git a/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index d713bab4..d46af2b0 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -163,5 +163,9 @@ $(find ignition_ros2_control_demos)/config/cartpole_controller_velocity.yaml + + From b834eae7e5f4275fdca6e83482aeff91336013a4 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Thu, 25 Nov 2021 18:20:54 +0100 Subject: [PATCH 38/50] Use std instead of boost Signed-off-by: ahcorde --- ignition_ros2_control/src/ignition_ros2_control_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index bbc577b8..2e9e9f59 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -79,7 +79,7 @@ class IgnitionROS2ControlPluginPrivate rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); /// \brief Interface loader - boost::shared_ptr> robot_hw_sim_loader_; /// \brief Controller manager From 766ccad698ee5d9e16ea78dfda391f4807006b6d Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Fri, 26 Nov 2021 11:48:01 +0300 Subject: [PATCH 39/50] Fortress support from cmake (#12) Signed-off-by: Vatan Aksoy Tezer --- ignition_ros2_control/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ignition_ros2_control/CMakeLists.txt b/ignition_ros2_control/CMakeLists.txt index 735096fd..b73308c4 100644 --- a/ignition_ros2_control/CMakeLists.txt +++ b/ignition_ros2_control/CMakeLists.txt @@ -30,6 +30,11 @@ if("$ENV{IGNITION_VERSION}" STREQUAL "citadel") set(IGN_GAZEBO_VER ${ignition-gazebo3_VERSION_MAJOR}) message(STATUS "Compiling against Ignition Citadel") +elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress") + find_package(ignition-gazebo6 REQUIRED) + set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) + + message(STATUS "Compiling against Ignition Fortress") else() find_package(ignition-gazebo5 REQUIRED) set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) From 054d2b5059d02b6e794efaafe4b4f3d5d4662803 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 29 Nov 2021 09:50:27 +0100 Subject: [PATCH 40/50] Pass ROS time instead of SYSTEM time to update function Signed-off-by: ahcorde --- ignition_ros2_control/src/ignition_ros2_control_plugin.cpp | 5 +++-- ignition_ros2_control/src/ignition_system.cpp | 6 ------ 2 files changed, 3 insertions(+), 8 deletions(-) diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index 2e9e9f59..bac839a7 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -94,7 +94,8 @@ class IgnitionROS2ControlPluginPrivate std::string robot_description_node_ = "robot_state_publisher"; /// \brief Last time the update method was called - rclcpp::Time last_update_sim_time_ros_; + rclcpp::Time last_update_sim_time_ros_ = + rclcpp::Time((int64_t)0, RCL_ROS_TIME); /// \brief ECM pointer ignition::gazebo::EntityComponentManager * ecm; @@ -441,7 +442,7 @@ void IgnitionROS2ControlPlugin::PostUpdate( { // Get the simulation time and period rclcpp::Time sim_time_ros(std::chrono::duration_cast( - _info.simTime).count()); + _info.simTime).count(), RCL_ROS_TIME); rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; if (sim_period >= this->dataPtr->control_period_) { diff --git a/ignition_ros2_control/src/ignition_system.cpp b/ignition_ros2_control/src/ignition_system.cpp index 61fe368c..5f7843e0 100644 --- a/ignition_ros2_control/src/ignition_system.cpp +++ b/ignition_ros2_control/src/ignition_system.cpp @@ -110,9 +110,6 @@ class ignition_ros2_control::IgnitionSystemPrivate /// \brief Degrees od freedom. size_t n_dof_; - /// \brief last time the write method was called. - rclcpp::Time last_update_sim_time_ros_; - /// \brief vector with the joint's names. std::vector joints_; @@ -128,8 +125,6 @@ class ignition_ros2_control::IgnitionSystemPrivate ignition::gazebo::EntityComponentManager * ecm; /// \brief Ignition communication node. - -public: ignition::transport::Node node; }; @@ -143,7 +138,6 @@ bool IgnitionSystem::initSim( ignition::gazebo::EntityComponentManager & _ecm) { this->dataPtr = std::make_unique(); - this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); this->nh_ = model_nh; this->dataPtr->ecm = &_ecm; From a11c0c3ac7e1c5dc642924e3d3cadcb9d5385d7b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 29 Nov 2021 09:57:14 +0100 Subject: [PATCH 41/50] Fixed cmake and package Signed-off-by: ahcorde --- ignition_ros2_control/CMakeLists.txt | 16 ++++++++++------ ignition_ros2_control/package.xml | 1 + 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/ignition_ros2_control/CMakeLists.txt b/ignition_ros2_control/CMakeLists.txt index b73308c4..965c3b9a 100644 --- a/ignition_ros2_control/CMakeLists.txt +++ b/ignition_ros2_control/CMakeLists.txt @@ -28,18 +28,22 @@ find_package(yaml_cpp_vendor REQUIRED) if("$ENV{IGNITION_VERSION}" STREQUAL "citadel") find_package(ignition-gazebo3 REQUIRED) set(IGN_GAZEBO_VER ${ignition-gazebo3_VERSION_MAJOR}) - message(STATUS "Compiling against Ignition Citadel") + +elseif("$ENV{IGNITION_VERSION}" STREQUAL "edifice") + find_package(ignition-gazebo5 REQUIRED) + set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) + message(STATUS "Compiling against Ignition Edifice") + elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress") find_package(ignition-gazebo6 REQUIRED) set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) - message(STATUS "Compiling against Ignition Fortress") -else() - find_package(ignition-gazebo5 REQUIRED) - set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) - message(STATUS "Compiling against Ignition Edifice") +else() + find_package(ignition-gazebo3 REQUIRED) + set(IGN_GAZEBO_VER ${ignition-gazebo3_VERSION_MAJOR}) + message(STATUS "Compiling against Ignition Citadel") endif() find_package(ignition-plugin1 REQUIRED) diff --git a/ignition_ros2_control/package.xml b/ignition_ros2_control/package.xml index a3277a76..bd1459b5 100644 --- a/ignition_ros2_control/package.xml +++ b/ignition_ros2_control/package.xml @@ -10,6 +10,7 @@ ament_cmake ament_index_cpp + ignition-gazebo3 ignition-gazebo3 ignition-gazebo5 ignition-plugin From abd6d69d1d0372b94ff79a7480ff4d15aa0a80af Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 30 Nov 2021 11:14:42 +0100 Subject: [PATCH 42/50] added feedback Signed-off-by: ahcorde --- README.md | 10 ++++++---- ignition_ros2_control/package.xml | 2 +- .../src/ignition_ros2_control_plugin.cpp | 16 +++++++++++----- .../launch/cart_example_effort.launch.py | 2 +- .../launch/cart_example_position.launch.py | 2 +- .../launch/cart_example_velocity.launch.py | 2 +- 6 files changed, 21 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index 5af58ab8..541e5dc2 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ign_ros2_control This is a ROS 2 package for integrating the `ros2_control` controller architecture with the [Ignition Gazebo](http://ignitionrobotics.org/) simulator. -More information about `ros2_control` can be found here: https://ros-controls.github.io/control.ros.org/ +More information about `ros2_control` can be found here: https://control.ros.org/ This package provides an Ignition Gazebo system plugin which instantiates a `ros2_control` controller manager and connects it to a Gazebo model. @@ -73,7 +73,7 @@ include: ```xml - ignition_ros2_control/GazeboSystem + ignition_ros2_control/IgnitionSystem @@ -131,7 +131,7 @@ robot model is loaded. For example, the following XML will load the default plug ```xml - ignition_ros2_control/GazeboSystem + ignition_ros2_control/IgnitionSystem ... @@ -144,12 +144,14 @@ robot model is loaded. For example, the following XML will load the default plug #### Set up controllers -Use the tag `` inside `` to set the YAML file with the controller configuration. +Use the tag `` inside `` to set the YAML file with the controller configuration +and use the tag `` to set the controller manager node name. ```xml $(find ignition_ros2_control_demos)/config/cartpole_controller.yaml + controller_manager ``` diff --git a/ignition_ros2_control/package.xml b/ignition_ros2_control/package.xml index bd1459b5..da9d4c44 100644 --- a/ignition_ros2_control/package.xml +++ b/ignition_ros2_control/package.xml @@ -2,7 +2,7 @@ ignition_ros2_control 0.0.0 - Ignition ROS2 control package. It allows to control joint using ROS 2 + Ignition ros2_control package allows to control simulated robots using ros2_control framework. Alejandro Hernández Alejandro Hernández Apache 2 diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index bac839a7..8d0c29e8 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -265,6 +265,12 @@ void IgnitionROS2ControlPlugin::Configure( return; } + // Get controller manager node name + std::string controllerManagerNodeName = _sdf->Get("controller_manager_node_name"); + if (controllerManagerNodeName.empty()) { + controllerManagerNodeName = "controller_manager"; + } + std::vector arguments = {"--ros-args", "--params-file", paramFileName}; auto sdfPtr = const_cast(_sdf.get()); @@ -304,7 +310,7 @@ void IgnitionROS2ControlPlugin::Configure( if (!rclcpp::ok()) { rclcpp::init(static_cast(argv.size()), argv.data()); - this->dataPtr->node = rclcpp::Node::make_shared("ignition_ros2_control"); + this->dataPtr->node = rclcpp::Node::make_shared("controller_manager_ignition_ros2_control"); } this->dataPtr->executor_ = std::make_shared(); this->dataPtr->executor_->add_node(this->dataPtr->node); @@ -364,10 +370,10 @@ void IgnitionROS2ControlPlugin::Configure( 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( + auto ignitionSystem = std::unique_ptr( this->dataPtr->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); - if (!gazeboSystem->initSim( + if (!ignitionSystem->initSim( this->dataPtr->node, enabledJoints, control_hardware[i], @@ -378,7 +384,7 @@ void IgnitionROS2ControlPlugin::Configure( return; } - resource_manager_->import_component(std::move(gazeboSystem)); + resource_manager_->import_component(std::move(ignitionSystem)); } // Create the controller manager RCLCPP_INFO(this->dataPtr->node->get_logger(), "Loading controller_manager"); @@ -386,7 +392,7 @@ void IgnitionROS2ControlPlugin::Configure( new controller_manager::ControllerManager( std::move(resource_manager_), this->dataPtr->executor_, - "controller_manager")); + controllerManagerNodeName)); this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_); if (!this->dataPtr->controller_manager_->has_parameter("update_rate")) { diff --git a/ignition_ros2_control_demos/launch/cart_example_effort.launch.py b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py index 1de9c3a9..7db3dd77 100644 --- a/ignition_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 Open Source Robotics Foundation, Inc. +# 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. diff --git a/ignition_ros2_control_demos/launch/cart_example_position.launch.py b/ignition_ros2_control_demos/launch/cart_example_position.launch.py index 16037935..85e18a28 100644 --- a/ignition_ros2_control_demos/launch/cart_example_position.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_position.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 Open Source Robotics Foundation, Inc. +# 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. diff --git a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py index 7487f7f9..3c4dc78c 100644 --- a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -1,4 +1,4 @@ -# Copyright 2020 Open Source Robotics Foundation, Inc. +# 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. From 6ce42638a5c88926e76a48222ab7b42c5ca951ec Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 30 Nov 2021 11:14:52 +0100 Subject: [PATCH 43/50] Update License Signed-off-by: ahcorde --- ignition_ros2_control/LICENSE | 198 ++++++++++++++++++++++++++++++++-- 1 file changed, 187 insertions(+), 11 deletions(-) diff --git a/ignition_ros2_control/LICENSE b/ignition_ros2_control/LICENSE index 1dee6304..6d8d58fb 100644 --- a/ignition_ros2_control/LICENSE +++ b/ignition_ros2_control/LICENSE @@ -1,15 +1,191 @@ -Software License Agreement (Apache License) -Copyright 2021 Open Source Robotics Foundation + Apache License + Version 2.0, January 2004 + https://www.apache.org/licenses/ -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 + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - http://www.apache.org/licenses/LICENSE-2.0 + 1. Definitions. -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. + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + Copyright 2013-2018 Docker, 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 + + https://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. From c458ec1cbd1e2c1ff8cb44a6756aafc7baba730d Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 30 Nov 2021 11:19:02 +0100 Subject: [PATCH 44/50] typo Signed-off-by: ahcorde --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 541e5dc2..53519333 100644 --- a/README.md +++ b/README.md @@ -47,7 +47,7 @@ To run the demo with the GUI, we are going to use [rocker](https://github.com/os images with customized local support injected for things like nvidia support. Rocker also supports user id specific files for cleaner mounting file permissions. You can install this tool with the following [instructions](https://github.com/osrf/rocker/#installation) (make sure you meet all of the [prerequisites](https://github.com/osrf/rocker/#prerequisites)). -The following command will launch Gazebo: +The following command will launch Ignition: ```bash rocker --x11 --nvidia --name ignition_ros2_control_demo ignition_ros2_control:latest From 18780914faf273f17fcc11569fa40d20aec3682b Mon Sep 17 00:00:00 2001 From: ahcorde Date: Tue, 30 Nov 2021 17:14:14 +0100 Subject: [PATCH 45/50] Added feedback Signed-off-by: ahcorde --- README.md | 12 ++++++------ .../src/ignition_ros2_control_plugin.cpp | 9 ++++++--- .../launch/cart_example_effort.launch.py | 2 +- .../launch/cart_example_position.launch.py | 2 +- .../launch/cart_example_velocity.launch.py | 2 +- .../urdf/test_cart_effort.xacro.urdf | 2 +- .../urdf/test_cart_position.xacro.urdf | 2 +- .../urdf/test_cart_velocity.xacro.urdf | 2 +- 8 files changed, 18 insertions(+), 15 deletions(-) diff --git a/README.md b/README.md index 53519333..698a28ec 100644 --- a/README.md +++ b/README.md @@ -71,7 +71,7 @@ include: - `` tag including the robot controllers: commands and states. ```xml - + ignition_ros2_control/IgnitionSystem @@ -123,13 +123,13 @@ The default behavior provides the following ros2_control interfaces: The `ignition_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 the `ignition_ros2_control::GazeboSystemInterface`, which implements a simulated `ros2_control` +These plugins must inherit the `ignition_ros2_control::IgnitionSystemInterface`, 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 +The respective IgnitionSystemInterface 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 - + ignition_ros2_control/IgnitionSystem @@ -145,13 +145,13 @@ robot model is loaded. For example, the following XML will load the default plug #### Set up controllers Use the tag `` inside `` to set the YAML file with the controller configuration -and use the tag `` to set the controller manager node name. +and use the tag `` to set the controller manager node name. ```xml $(find ignition_ros2_control_demos)/config/cartpole_controller.yaml - controller_manager + controller_manager ``` diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index 8d0c29e8..d8d36b90 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -266,9 +266,12 @@ void IgnitionROS2ControlPlugin::Configure( } // Get controller manager node name - std::string controllerManagerNodeName = _sdf->Get("controller_manager_node_name"); - if (controllerManagerNodeName.empty()) { - controllerManagerNodeName = "controller_manager"; + std::string controllerManagerNodeName{"controller_manager"}; + + std::string controllerManagerPrefixNodeName = + _sdf->Get("controller_manager_prefix_node_name"); + if (!controllerManagerPrefixNodeName.empty()) { + controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; } std::vector arguments = {"--ros-args", "--params-file", paramFileName}; diff --git a/ignition_ros2_control_demos/launch/cart_example_effort.launch.py b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py index 7db3dd77..61135213 100644 --- a/ignition_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_effort.launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): # Launch Arguments - use_sim_time = LaunchConfiguration('use_sim_time', default=False) + use_sim_time = LaunchConfiguration('use_sim_time', default=True) ignition_ros2_control_demos_path = os.path.join( get_package_share_directory('ignition_ros2_control_demos')) diff --git a/ignition_ros2_control_demos/launch/cart_example_position.launch.py b/ignition_ros2_control_demos/launch/cart_example_position.launch.py index 85e18a28..888a87a9 100644 --- a/ignition_ros2_control_demos/launch/cart_example_position.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_position.launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): # Launch Arguments - use_sim_time = LaunchConfiguration('use_sim_time', default=False) + use_sim_time = LaunchConfiguration('use_sim_time', default=True) ignition_ros2_control_demos_path = os.path.join( get_package_share_directory('ignition_ros2_control_demos')) diff --git a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py index 3c4dc78c..f586c0d3 100644 --- a/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/ignition_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -31,7 +31,7 @@ def generate_launch_description(): # Launch Arguments - use_sim_time = LaunchConfiguration('use_sim_time', default=False) + use_sim_time = LaunchConfiguration('use_sim_time', default=True) ignition_ros2_control_demos_path = os.path.join( get_package_share_directory('ignition_ros2_control_demos')) diff --git a/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf index 0dcbf0e6..4fa58919 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_effort.xacro.urdf @@ -43,7 +43,7 @@ - + ignition_ros2_control/IgnitionSystem diff --git a/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf index 88e59db5..6672bafd 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_position.xacro.urdf @@ -43,7 +43,7 @@ - + ignition_ros2_control/IgnitionSystem diff --git a/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf index d46af2b0..a3165dcb 100644 --- a/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf +++ b/ignition_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf @@ -111,7 +111,7 @@ - + ignition_ros2_control/IgnitionSystem From 785cac41489d0ee43090e743dc903e42c0e47e08 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 3 Dec 2021 11:23:46 +0100 Subject: [PATCH 46/50] Added feedback Signed-off-by: ahcorde --- README.md | 7 +- ignition_ros2_control/package.xml | 1 + .../src/ignition_ros2_control_plugin.cpp | 71 ++++++++++--------- ignition_ros2_control/src/ignition_system.cpp | 18 ++--- .../examples/example_effort.cpp | 5 +- ignition_ros2_control_demos/package.xml | 4 +- 6 files changed, 56 insertions(+), 50 deletions(-) diff --git a/README.md b/README.md index 698a28ec..76679fd2 100644 --- a/README.md +++ b/README.md @@ -7,8 +7,9 @@ This package provides an Ignition Gazebo system plugin which instantiates a `ros Tested on: - - [ROS 2 Foxy](https://docs.ros.org/en/foxy/Installation.html) - - [Ignition Edifice](https://ignitionrobotics.org/docs/edifice) + - [Ignition Citadel](https://ignitionrobotics.org/docs/citadel) + [ROS 2 Foxy](https://docs.ros.org/en/foxy/Installation.html) + - [Ignition Edifice](https://ignitionrobotics.org/docs/edifice) + [ROS 2 Foxy](https://docs.ros.org/en/foxy/Installation.html) + - [Ignition Edifice](https://ignitionrobotics.org/docs/fortress) + [ROS 2 Foxy](https://docs.ros.org/en/foxy/Installation.html) # Usage @@ -21,7 +22,7 @@ Tested on: ### Modifying or building your own ```bash -cd Docker +cd Dockerfile docker build -t ignition_ros2_control . ``` diff --git a/ignition_ros2_control/package.xml b/ignition_ros2_control/package.xml index da9d4c44..36c41d9c 100644 --- a/ignition_ros2_control/package.xml +++ b/ignition_ros2_control/package.xml @@ -13,6 +13,7 @@ ignition-gazebo3 ignition-gazebo3 ignition-gazebo5 + ignition-gazebo6 ignition-plugin pluginlib rclcpp diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index d8d36b90..948bcb3d 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -64,13 +64,13 @@ class IgnitionROS2ControlPluginPrivate ignition::gazebo::Entity entity_; /// \brief Node Handles - std::shared_ptr node; + std::shared_ptr node_{nullptr}; /// \brief Thread where the executor will sping std::thread thread_executor_spin_; /// \brief Flag to stop the executor thread when this plugin is exiting - bool stop_; + bool stop_{false}; /// \brief Executor to spin the controller rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; @@ -80,10 +80,12 @@ class IgnitionROS2ControlPluginPrivate /// \brief Interface loader std::shared_ptr> robot_hw_sim_loader_; + ignition_ros2_control::IgnitionSystemInterface>> + robot_hw_sim_loader_{nullptr}; /// \brief Controller manager - std::shared_ptr controller_manager_; + std::shared_ptr + controller_manager_{nullptr}; /// \brief String with the robot description param_name // TODO(ahcorde): Add param in plugin tag @@ -98,7 +100,7 @@ class IgnitionROS2ControlPluginPrivate rclcpp::Time((int64_t)0, RCL_ROS_TIME); /// \brief ECM pointer - ignition::gazebo::EntityComponentManager * ecm; + ignition::gazebo::EntityComponentManager * ecm{nullptr}; }; ////////////////////////////////////////////////// @@ -134,7 +136,7 @@ IgnitionROS2ControlPluginPrivate::GetEnabledJoints( case sdf::JointType::FIXED: { RCLCPP_INFO( - node->get_logger(), + node_->get_logger(), "[ignition_ros2_control] Fixed joint [%s] (Entity=%d)] is skipped", jointName.c_str(), jointEntity); continue; @@ -145,7 +147,7 @@ IgnitionROS2ControlPluginPrivate::GetEnabledJoints( case sdf::JointType::UNIVERSAL: { RCLCPP_WARN( - node->get_logger(), + node_->get_logger(), "[ignition_ros2_control] Joint [%s] (Entity=%d)] is of unsupported type." " Only joints with a single axis are supported.", jointName.c_str(), jointEntity); @@ -154,7 +156,7 @@ IgnitionROS2ControlPluginPrivate::GetEnabledJoints( default: { RCLCPP_WARN( - node->get_logger(), + node_->get_logger(), "[ignition_ros2_control] Joint [%s] (Entity=%d)] is of unknown type", jointName.c_str(), jointEntity); continue; @@ -173,28 +175,28 @@ std::string IgnitionROS2ControlPluginPrivate::getURDF() const using namespace std::chrono_literals; auto parameters_client = std::make_shared( - node, robot_description_node_); + node_, robot_description_node_); while (!parameters_client->wait_for_service(0.5s)) { if (!rclcpp::ok()) { RCLCPP_ERROR( - node->get_logger(), "Interrupted while waiting for %s service. Exiting.", + node_->get_logger(), "Interrupted while waiting for %s service. Exiting.", robot_description_node_.c_str()); return 0; } RCLCPP_ERROR( - node->get_logger(), "%s service not available, waiting again...", + node_->get_logger(), "%s service not available, waiting again...", robot_description_node_.c_str()); } RCLCPP_INFO( - node->get_logger(), "connected to service!! %s asking for %s", + node_->get_logger(), "connected to service!! %s asking for %s", robot_description_node_.c_str(), this->robot_description_.c_str()); // search and wait for robot_description on param server while (urdf_string.empty()) { RCLCPP_DEBUG( - node->get_logger(), "param_name %s", + node_->get_logger(), "param_name %s", this->robot_description_.c_str()); try { @@ -203,20 +205,20 @@ std::string IgnitionROS2ControlPluginPrivate::getURDF() const std::vector values = f.get(); urdf_string = values[0].as_string(); } catch (const std::exception & e) { - RCLCPP_ERROR(node->get_logger(), "%s", e.what()); + RCLCPP_ERROR(node_->get_logger(), "%s", e.what()); } if (!urdf_string.empty()) { break; } else { RCLCPP_ERROR( - node->get_logger(), "ignition_ros2_control plugin is waiting for model" + node_->get_logger(), "ignition_ros2_control plugin is waiting for model" " URDF in parameter [%s] on the ROS param server.", this->robot_description_.c_str()); } usleep(100000); } - RCLCPP_INFO(node->get_logger(), "Received URDF from param server"); + RCLCPP_INFO(node_->get_logger(), "Received URDF from param server"); return urdf_string; } @@ -248,7 +250,7 @@ void IgnitionROS2ControlPlugin::Configure( const auto model = ignition::gazebo::Model(_entity); if (!model.Valid(_ecm)) { RCLCPP_ERROR( - this->dataPtr->node->get_logger(), + this->dataPtr->node_->get_logger(), "[Ignition ROS 2 Control] Failed to initialize because [%s] (Entity=%u)] is not a model." "Please make sure that Ignition ROS 2 Control is attached to a valid model.", model.Name(_ecm).c_str(), _entity); @@ -260,7 +262,7 @@ void IgnitionROS2ControlPlugin::Configure( if (paramFileName.empty()) { RCLCPP_ERROR( - this->dataPtr->node->get_logger(), + this->dataPtr->node_->get_logger(), "Ignition ros2 control found an empty parameters file. Failed to initialize."); return; } @@ -313,10 +315,15 @@ void IgnitionROS2ControlPlugin::Configure( if (!rclcpp::ok()) { rclcpp::init(static_cast(argv.size()), argv.data()); - this->dataPtr->node = rclcpp::Node::make_shared("controller_manager_ignition_ros2_control"); + std::string node_name = "ignition_ros_control"; + if (!controllerManagerPrefixNodeName.empty()) + { + node_name = controllerManagerPrefixNodeName + "_" + node_name; + } + this->dataPtr->node_ = rclcpp::Node::make_shared(node_name); } this->dataPtr->executor_ = std::make_shared(); - this->dataPtr->executor_->add_node(this->dataPtr->node); + this->dataPtr->executor_->add_node(this->dataPtr->node_); this->dataPtr->stop_ = false; auto spin = [this]() { @@ -327,7 +334,7 @@ void IgnitionROS2ControlPlugin::Configure( this->dataPtr->thread_executor_spin_ = std::thread(spin); RCLCPP_DEBUG_STREAM( - this->dataPtr->node->get_logger(), "[Ignition ROS 2 Control] Setting up controller for [" << + this->dataPtr->node_->get_logger(), "[Ignition ROS 2 Control] Setting up controller for [" << model.Name(_ecm) << "] (Entity=" << _entity << ")]."); // Get list of enabled joints @@ -337,7 +344,7 @@ void IgnitionROS2ControlPlugin::Configure( if (enabledJoints.size() == 0) { RCLCPP_DEBUG_STREAM( - this->dataPtr->node->get_logger(), "[Ignition ROS 2 Control] There are no available Joints."); + this->dataPtr->node_->get_logger(), "[Ignition ROS 2 Control] There are no available Joints."); return; } @@ -351,7 +358,7 @@ void IgnitionROS2ControlPlugin::Configure( control_hardware = hardware_interface::parse_control_resources_from_urdf(urdf_string); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM( - this->dataPtr->node->get_logger(), + this->dataPtr->node_->get_logger(), "Error parsing URDF in ignition_ros2_control plugin, plugin not active : " << ex.what()); return; } @@ -366,7 +373,7 @@ void IgnitionROS2ControlPlugin::Configure( "ignition_ros2_control::IgnitionSystemInterface")); } catch (pluginlib::LibraryLoadException & ex) { RCLCPP_ERROR( - this->dataPtr->node->get_logger(), "Failed to create robot simulation interface loader: %s ", + this->dataPtr->node_->get_logger(), "Failed to create robot simulation interface loader: %s ", ex.what()); return; } @@ -377,20 +384,20 @@ void IgnitionROS2ControlPlugin::Configure( this->dataPtr->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); if (!ignitionSystem->initSim( - this->dataPtr->node, + this->dataPtr->node_, enabledJoints, control_hardware[i], _ecm)) { RCLCPP_FATAL( - this->dataPtr->node->get_logger(), "Could not initialize robot simulation interface"); + this->dataPtr->node_->get_logger(), "Could not initialize robot simulation interface"); return; } resource_manager_->import_component(std::move(ignitionSystem)); } // Create the controller manager - RCLCPP_INFO(this->dataPtr->node->get_logger(), "Loading controller_manager"); + RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager"); this->dataPtr->controller_manager_.reset( new controller_manager::ControllerManager( std::move(resource_manager_), @@ -400,7 +407,7 @@ void IgnitionROS2ControlPlugin::Configure( if (!this->dataPtr->controller_manager_->has_parameter("update_rate")) { RCLCPP_ERROR_STREAM( - this->dataPtr->node->get_logger(), + this->dataPtr->node_->get_logger(), "controller manager doesn't have an update_rate parameter"); return; } @@ -425,13 +432,13 @@ void IgnitionROS2ControlPlugin::PreUpdate( // Check the period against the simulation period if (this->dataPtr->control_period_ < _info.dt) { RCLCPP_ERROR_STREAM( - this->dataPtr->node->get_logger(), + this->dataPtr->node_->get_logger(), "Desired controller update period (" << this->dataPtr->control_period_.seconds() << " s) is faster than the gazebo simulation period (" << gazebo_period.seconds() << " s)."); } else if (this->dataPtr->control_period_ > gazebo_period) { RCLCPP_WARN_STREAM( - this->dataPtr->node->get_logger(), + this->dataPtr->node_->get_logger(), " Desired controller update period (" << this->dataPtr->control_period_.seconds() << " s) is slower than the gazebo simulation period (" << gazebo_period.seconds() << " s)."); @@ -471,7 +478,3 @@ IGNITION_ADD_PLUGIN( ignition_ros2_control::IgnitionROS2ControlPlugin::ISystemConfigure, ignition_ros2_control::IgnitionROS2ControlPlugin::ISystemPreUpdate, ignition_ros2_control::IgnitionROS2ControlPlugin::ISystemPostUpdate) - -IGNITION_ADD_PLUGIN_ALIAS( - ignition_ros2_control::IgnitionROS2ControlPlugin, - "ignition_ros2_control::IgnitionROS2ControlPlugin") diff --git a/ignition_ros2_control/src/ignition_system.cpp b/ignition_ros2_control/src/ignition_system.cpp index 5f7843e0..88d45ffc 100644 --- a/ignition_ros2_control/src/ignition_system.cpp +++ b/ignition_ros2_control/src/ignition_system.cpp @@ -40,31 +40,31 @@ struct jointData { - /// \brief vector with the joint's names. + /// \brief Joint's names. std::string name; - /// \brief vector with the current joint position + /// \brief Current joint position double joint_position; - /// \brief vector with the current joint velocity + /// \brief Current joint velocity double joint_velocity; - /// \brief vector with the current joint effort + /// \brief Current joint effort double joint_effort; - /// \brief vector with the current cmd joint position + /// \brief Current cmd joint position double joint_position_cmd; - /// \brief vector with the current cmd joint velocity + /// \brief Current cmd joint velocity double joint_velocity_cmd; - /// \brief vector with the current cmd joint effort + /// \brief Current cmd joint effort double joint_effort_cmd; /// \brief handles to the joints from within Gazebo ignition::gazebo::Entity sim_joint; - /// \brief vector with the control method defined in the URDF for each joint. + /// \brief Control method defined in the URDF for each joint. ignition_ros2_control::IgnitionSystemInterface::ControlMethod joint_control_method; }; @@ -122,6 +122,8 @@ class ignition_ros2_control::IgnitionSystemPrivate /// \brief command interfaces that will be exported to the Resource Manager std::vector command_interfaces_; + /// \brief Entity component manager, ECM shouldn't be accessed outside those + /// methods, otherwise the app will crash ignition::gazebo::EntityComponentManager * ecm; /// \brief Ignition communication node. diff --git a/ignition_ros2_control_demos/examples/example_effort.cpp b/ignition_ros2_control_demos/examples/example_effort.cpp index 579ef941..41733b0c 100644 --- a/ignition_ros2_control_demos/examples/example_effort.cpp +++ b/ignition_ros2_control_demos/examples/example_effort.cpp @@ -19,13 +19,12 @@ #include -std::shared_ptr node; - int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - node = std::make_shared("effort_test_node"); + std::shared_ptr node = + std::make_shared("effort_test_node"); auto publisher = node->create_publisher( "/effort_controllers/commands", 10); diff --git a/ignition_ros2_control_demos/package.xml b/ignition_ros2_control_demos/package.xml index abbead9f..7285e8dc 100644 --- a/ignition_ros2_control_demos/package.xml +++ b/ignition_ros2_control_demos/package.xml @@ -8,9 +8,9 @@ Apache License 2.0 - http://ros.org/wiki/gazebo_ros_control + https://github.com/ignitionrobotics/ign_ros2_control/blob/main/README.md https://github.com/ros-simulation/gazebo_ros2_control/issues - https://github.com/ros-simulation/gazebo_ros2_control + https://github.com/ignitionrobotics/ign_ros2_control Alejandro Hernández From d47b38b8b3a07e7ba8dec0b8519358a3500e98e3 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 3 Dec 2021 11:23:52 +0100 Subject: [PATCH 47/50] CI Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 13 +++++++------ .../ignition_ros2_control/ignition_system.hpp | 3 +++ 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 916359eb..1a78cf8b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -8,6 +8,11 @@ on: jobs: build: runs-on: ubuntu-latest + strategy: + matrix: + version: [citadel, edifice, fortress] + env: + IGNITION_VERSION: ${{ matrix.version }} container: image: ubuntu:20.04 steps: @@ -21,11 +26,9 @@ jobs: cd .. mkdir -p /home/ros2_ws/src cp -r ign_ros2_control /home/ros2_ws/src/ - sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-latest.list' curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - - IGN_DEPS="libignition-gazebo5-dev libignition-plugin-dev libignition-utils1-cli-dev" apt-get update && apt-get upgrade -q -y apt-get update && apt-get install -qq -y \ dirmngr \ @@ -34,13 +37,11 @@ jobs: python3-colcon-ros \ python3-colcon-common-extensions \ python3-rosdep \ - build-essential \ - $IGN_DEPS + build-essential cd /home/ros2_ws/src/ rosdep init rosdep update - rosdep install --from-paths ./ -i -y --rosdistro foxy \ - --ignore-src --skip-keys "ignition-gazebo5" + rosdep install --from-paths ./ -i -y --rosdistro foxy - name: Build project id: build run: | diff --git a/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp b/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp index 19ce9af8..f1876a4f 100644 --- a/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp +++ b/ignition_ros2_control/include/ignition_ros2_control/ignition_system.hpp @@ -64,6 +64,9 @@ class IgnitionSystem : public IgnitionSystemInterface ignition::gazebo::EntityComponentManager & _ecm) override; private: + // Register a sensor (for now just IMUs) + // \param[in] hardware_info hardware information where the data of + // the sensors is extract. void registerSensors( const hardware_interface::HardwareInfo & hardware_info); From a0ab577cc35cc044ac2017c834392b96ce21bf5f Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 3 Dec 2021 11:36:04 +0100 Subject: [PATCH 48/50] remove fortress from CI Signed-off-by: ahcorde --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 1a78cf8b..2842e2b8 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -10,7 +10,7 @@ jobs: runs-on: ubuntu-latest strategy: matrix: - version: [citadel, edifice, fortress] + version: [citadel, edifice] env: IGNITION_VERSION: ${{ matrix.version }} container: From 3341771cd3ef796641e4d8172f16d99f5deddef8 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 3 Dec 2021 11:46:17 +0100 Subject: [PATCH 49/50] make linters happy Signed-off-by: ahcorde --- .../src/ignition_ros2_control_plugin.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index 948bcb3d..dd0ba10d 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -81,11 +81,11 @@ class IgnitionROS2ControlPluginPrivate /// \brief Interface loader std::shared_ptr> - robot_hw_sim_loader_{nullptr}; + robot_hw_sim_loader_{nullptr}; /// \brief Controller manager std::shared_ptr - controller_manager_{nullptr}; + controller_manager_{nullptr}; /// \brief String with the robot description param_name // TODO(ahcorde): Add param in plugin tag @@ -272,7 +272,8 @@ void IgnitionROS2ControlPlugin::Configure( std::string controllerManagerPrefixNodeName = _sdf->Get("controller_manager_prefix_node_name"); - if (!controllerManagerPrefixNodeName.empty()) { + if (!controllerManagerPrefixNodeName.empty()) + { controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; } @@ -344,7 +345,8 @@ void IgnitionROS2ControlPlugin::Configure( if (enabledJoints.size() == 0) { RCLCPP_DEBUG_STREAM( - this->dataPtr->node_->get_logger(), "[Ignition ROS 2 Control] There are no available Joints."); + this->dataPtr->node_->get_logger(), + "[Ignition ROS 2 Control] There are no available Joints."); return; } From 263a19028025a5fbc5ad729505be6caf83b8cee4 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Fri, 3 Dec 2021 11:53:33 +0100 Subject: [PATCH 50/50] make linters happy Signed-off-by: ahcorde --- ignition_ros2_control/src/ignition_ros2_control_plugin.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp index dd0ba10d..1b9019f8 100644 --- a/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp +++ b/ignition_ros2_control/src/ignition_ros2_control_plugin.cpp @@ -272,8 +272,7 @@ void IgnitionROS2ControlPlugin::Configure( std::string controllerManagerPrefixNodeName = _sdf->Get("controller_manager_prefix_node_name"); - if (!controllerManagerPrefixNodeName.empty()) - { + if (!controllerManagerPrefixNodeName.empty()) { controllerManagerNodeName = controllerManagerPrefixNodeName + "_" + controllerManagerNodeName; } @@ -317,8 +316,7 @@ void IgnitionROS2ControlPlugin::Configure( if (!rclcpp::ok()) { rclcpp::init(static_cast(argv.size()), argv.data()); std::string node_name = "ignition_ros_control"; - if (!controllerManagerPrefixNodeName.empty()) - { + if (!controllerManagerPrefixNodeName.empty()) { node_name = controllerManagerPrefixNodeName + "_" + node_name; } this->dataPtr->node_ = rclcpp::Node::make_shared(node_name);