EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

migrate gz quadruped plugin to humble

This commit is contained in:
Huang Zhenbiao 2025-04-19 16:45:11 +08:00
parent c039c4d083
commit 3163e86012
8 changed files with 1164 additions and 270 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
descriptions/unitree/go2_description/urdf/robot.urdf
View File

@ -194,7 +194,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/trunk.dae" scale="1 1 1"/> <mesh filename="../meshes/trunk.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -247,7 +247,7 @@
<visual> <visual>
<origin rpy="3.141592653589793 0 0" xyz="0 0 0"/> <origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/hip.dae" scale="1 1 1"/> <mesh filename="../meshes/hip.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -275,7 +275,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/thigh_mirror.dae" scale="1 1 1"/> <mesh filename="../meshes/thigh_mirror.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -303,7 +303,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/calf.dae" scale="1 1 1"/> <mesh filename="../meshes/calf.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -387,7 +387,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/hip.dae" scale="1 1 1"/> <mesh filename="../meshes/hip.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -415,7 +415,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/thigh.dae" scale="1 1 1"/> <mesh filename="../meshes/thigh.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -443,7 +443,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/calf.dae" scale="1 1 1"/> <mesh filename="../meshes/calf.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -527,7 +527,7 @@
<visual> <visual>
<origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0 0"/> <origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/hip.dae" scale="1 1 1"/> <mesh filename="../meshes/hip.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -555,7 +555,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/thigh_mirror.dae" scale="1 1 1"/> <mesh filename="../meshes/thigh_mirror.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -583,7 +583,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/calf.dae" scale="1 1 1"/> <mesh filename="../meshes/calf.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -667,7 +667,7 @@
<visual> <visual>
<origin rpy="0 3.141592653589793 0" xyz="0 0 0"/> <origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/hip.dae" scale="1 1 1"/> <mesh filename="../meshes/hip.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -695,7 +695,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/thigh.dae" scale="1 1 1"/> <mesh filename="../meshes/thigh.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>
@ -723,7 +723,7 @@
<visual> <visual>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<geometry> <geometry>
<mesh filename="file:///home/tlab-uav/ros2_ws/install/go2_description/share/go2_description/meshes/calf.dae" scale="1 1 1"/> <mesh filename="../meshes/calf.dae" scale="1 1 1"/>
</geometry> </geometry>
<!-- <material name="orange"/> --> <!-- <material name="orange"/> -->
</visual> </visual>

43
hardwares/gz_quadruped_hardware/CMakeLists.txt
View File

@ -3,18 +3,18 @@ project(gz_quadruped_hardware)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON) set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
# Default to C11 # Default to C11
if (NOT CMAKE_C_STANDARD) if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 11) set(CMAKE_C_STANDARD 11)
endif () endif()
# Default to C++17 # Default to C++17
if (NOT CMAKE_CXX_STANDARD) if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD 17)
endif () endif()
# Compiler options # Compiler options
if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic) add_compile_options(-Wall -Wextra -Wpedantic)
endif () endif()
# Find dependencies # Find dependencies
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
@ -25,11 +25,28 @@ find_package(pluginlib REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
find_package(yaml_cpp_vendor REQUIRED) find_package(yaml_cpp_vendor REQUIRED)
find_package(gz_sim_vendor REQUIRED) set(GZ_PLUGIN)
find_package(gz-sim REQUIRED) set(GZ_SIM)
find_package(gz_plugin_vendor REQUIRED) find_package(gz-sim8 QUIET)
find_package(gz-plugin REQUIRED) if(NOT gz-sim8_FOUND)
message(STATUS "Compiling against Gazebo fortress")
find_package(ignition-gazebo6 REQUIRED)
set(GZ_SIM_VER ${ignition-gazebo6_VERSION_MAJOR})
find_package(ignition-plugin1 REQUIRED)
set(GZ_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
set(GZ_PLUGIN ignition-plugin${GZ_PLUGIN_VER}::register)
set(GZ_SIM ignition-gazebo${GZ_SIM_VER}::core)
else ()
message(STATUS "Compiling against Gazebo harmonic")
find_package(gz-sim8 REQUIRED)
set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR})
find_package(gz-plugin2 REQUIRED)
set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR})
set(GZ_PLUGIN gz-plugin${GZ_PLUGIN_VER}::register)
set(GZ_SIM gz-sim${GZ_SIM_VER}::core)
add_definitions(-DGZ_HEADERS)
endif()
include_directories(include) include_directories(include)
@ -38,8 +55,8 @@ add_library(${PROJECT_NAME}-system SHARED
) )
target_link_libraries(${PROJECT_NAME}-system target_link_libraries(${PROJECT_NAME}-system
gz-sim::gz-sim ${GZ_SIM}
gz-plugin::register ${GZ_PLUGIN}
) )
ament_target_dependencies(${PROJECT_NAME}-system ament_target_dependencies(${PROJECT_NAME}-system
ament_index_cpp ament_index_cpp
@ -62,7 +79,7 @@ ament_target_dependencies(gz_quadruped_plugins
rclcpp rclcpp
) )
target_link_libraries(gz_quadruped_plugins target_link_libraries(gz_quadruped_plugins
gz-sim::gz-sim ${GZ_SIM}
) )
## Install ## Install

35
hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_quadruped_plugin.hpp
View File

@ -15,18 +15,25 @@
#include <memory> #include <memory>
#ifdef GZ_HEADERS
#include <gz/sim/System.hh> #include <gz/sim/System.hh>
namespace sim = gz::sim; namespace sim = gz::sim;
#else
#include <ignition/gazebo/System.hh>
namespace sim = ignition::gazebo;
#endif
namespace gz_quadruped_hardware { namespace gz_quadruped_hardware
{
// Forward declarations. // Forward declarations.
class GazeboSimQuadrupedPluginPrivate; class GazeboSimQuadrupedPluginPrivate;
class GazeboSimQuadrupedPlugin class GazeboSimQuadrupedPlugin
: public sim::System, : public sim::System,
public sim::ISystemConfigure, public sim::ISystemConfigure,
public sim::ISystemPreUpdate, public sim::ISystemPreUpdate,
public sim::ISystemPostUpdate { public sim::ISystemPostUpdate
{
public: public:
/// \brief Constructor /// \brief Constructor
GazeboSimQuadrupedPlugin(); GazeboSimQuadrupedPlugin();
@ -36,22 +43,22 @@ namespace gz_quadruped_hardware {
// Documentation inherited // Documentation inherited
void Configure( void Configure(
const sim::Entity &_entity, const sim::Entity& _entity,
const std::shared_ptr<const sdf::Element> &_sdf, const std::shared_ptr<const sdf::Element>& _sdf,
sim::EntityComponentManager &_ecm, sim::EntityComponentManager& _ecm,
sim::EventManager &_eventMgr) override; sim::EventManager& _eventMgr) override;
// Documentation inherited // Documentation inherited
void PreUpdate( void PreUpdate(
const sim::UpdateInfo &_info, const sim::UpdateInfo& _info,
sim::EntityComponentManager &_ecm) override; sim::EntityComponentManager& _ecm) override;
void PostUpdate( void PostUpdate(
const sim::UpdateInfo &_info, const sim::UpdateInfo& _info,
const sim::EntityComponentManager &_ecm) override; const sim::EntityComponentManager& _ecm) override;
private: private:
/// \brief Private data pointer. /// \brief Private data pointer.
std::unique_ptr<GazeboSimQuadrupedPluginPrivate> dataPtr; std::unique_ptr<GazeboSimQuadrupedPluginPrivate> dataPtr;
}; };
} // namespace gz_ros2_control }

42
hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system.hpp
View File

@ -13,7 +13,6 @@
// limitations under the License. // limitations under the License.
#pragma once #pragma once
#include <map> #include <map>
#include <memory> #include <memory>
#include <string> #include <string>
@ -23,7 +22,8 @@
#include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/state.hpp"
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
namespace gz_quadruped_hardware { namespace gz_quadruped_hardware
{
using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
// Forward declaration // Forward declaration
@ -32,13 +32,14 @@ namespace gz_quadruped_hardware {
// These class must inherit `gz_ros2_control::GazeboSimSystemInterface` which implements a // These class must inherit `gz_ros2_control::GazeboSimSystemInterface` which implements a
// simulated `ros2_control` `hardware_interface::SystemInterface`. // simulated `ros2_control` `hardware_interface::SystemInterface`.
class GazeboSimSystem final : public GazeboSimSystemInterface { class GazeboSimSystem : public GazeboSimSystemInterface
{
public: public:
// Documentation Inherited // Documentation Inherited
CallbackReturn on_init(const hardware_interface::HardwareInfo &system_info) CallbackReturn on_init(const hardware_interface::HardwareInfo& system_info)
override; override;
CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override; CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
// Documentation Inherited // Documentation Inherited
std::vector<hardware_interface::StateInterface> export_state_interfaces() override; std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
@ -47,37 +48,42 @@ namespace gz_quadruped_hardware {
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override; std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
// Documentation Inherited // Documentation Inherited
CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override; CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
// Documentation Inherited // Documentation Inherited
CallbackReturn on_deactivate(const rclcpp_lifecycle::State &previous_state) override; CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
// Documentation Inherited
hardware_interface::return_type perform_command_mode_switch(
const std::vector<std::string>& start_interfaces,
const std::vector<std::string>& stop_interfaces) override;
// Documentation Inherited // Documentation Inherited
hardware_interface::return_type read( hardware_interface::return_type read(
const rclcpp::Time &time, const rclcpp::Time& time,
const rclcpp::Duration &period) override; const rclcpp::Duration& period) override;
// Documentation Inherited // Documentation Inherited
hardware_interface::return_type write( hardware_interface::return_type write(
const rclcpp::Time &time, const rclcpp::Time& time,
const rclcpp::Duration &period) override; const rclcpp::Duration& period) override;
// Documentation Inherited // Documentation Inherited
bool initSim( bool initSim(
rclcpp::Node::SharedPtr &model_nh, rclcpp::Node::SharedPtr& model_nh,
std::map<std::string, sim::Entity> &joints, std::map<std::string, sim::Entity>& joints,
const hardware_interface::HardwareInfo &hardware_info, const hardware_interface::HardwareInfo& hardware_info,
sim::EntityComponentManager &_ecm, sim::EntityComponentManager& _ecm,
unsigned int update_rate) override; int& update_rate) override;
private: private:
// Register a sensor (for now just IMUs) // Register a sensor (for now just IMUs)
// \param[in] hardware_info hardware information where the data of // \param[in] hardware_info hardware information where the data of
// the sensors is extract. // the sensors is extract.
void registerSensors( void registerSensors(
const hardware_interface::HardwareInfo &hardware_info); const hardware_interface::HardwareInfo& hardware_info);
/// \brief Private data class /// \brief Private data class
std::unique_ptr<GazeboSimSystemPrivate> dataPtr; std::unique_ptr<GazeboSimSystemPrivate> dataPtr;
}; };
} // namespace gz_ros2_control }

61
hardwares/gz_quadruped_hardware/include/gz_quadruped_hardware/gz_system_interface.hpp
View File

@ -16,15 +16,23 @@
#include <map> #include <map>
#include <memory> #include <memory>
#include <string> #include <string>
#include <vector>
#ifdef GZ_HEADERS
#include <gz/sim/System.hh> #include <gz/sim/System.hh>
namespace sim = gz::sim; namespace sim = gz::sim;
#else
#include <ignition/gazebo/System.hh>
namespace sim = ignition::gazebo;
#endif
#include <hardware_interface/system_interface.hpp> #include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <rclcpp/rclcpp.hpp> #include <rclcpp/rclcpp.hpp>
namespace gz_quadruped_hardware { namespace gz_quadruped_hardware
{
/// \brief This class allows us to handle flags easily, instead of using strings /// \brief This class allows us to handle flags easily, instead of using strings
/// ///
/// For example /// For example
@ -42,44 +50,53 @@ namespace gz_quadruped_hardware {
/// foo & POSITION -> True // Check if position is active in the flag /// foo & POSITION -> True // Check if position is active in the flag
/// foo & VELOCITY -> False // Check if velocity is active in the flag /// foo & VELOCITY -> False // Check if velocity is active in the flag
template<class ENUM, class UNDERLYING = typename std::underlying_type<ENUM>::type> template <class ENUM, class UNDERLYING = typename std::underlying_type<ENUM>::type>
class SafeEnum { class SafeEnum
{
public: public:
SafeEnum() SafeEnum()
: mFlags(0) { : mFlags(0)
{
} }
explicit SafeEnum(ENUM singleFlag) explicit SafeEnum(ENUM singleFlag)
: mFlags(singleFlag) { : mFlags(singleFlag)
{
} }
SafeEnum(const SafeEnum &original) SafeEnum(const SafeEnum& original)
: mFlags(original.mFlags) { : mFlags(original.mFlags)
{
} }
SafeEnum &operator|=(ENUM addValue) { SafeEnum& operator|=(ENUM addValue)
{
mFlags |= addValue; mFlags |= addValue;
return *this; return *this;
} }
SafeEnum operator|(ENUM addValue) { SafeEnum operator|(ENUM addValue)
{
SafeEnum result(*this); SafeEnum result(*this);
result |= addValue; result |= addValue;
return result; return result;
} }
SafeEnum &operator&=(ENUM maskValue) { SafeEnum& operator&=(ENUM maskValue)
{
mFlags &= maskValue; mFlags &= maskValue;
return *this; return *this;
} }
SafeEnum operator&(ENUM maskValue) { SafeEnum operator&(ENUM maskValue)
{
SafeEnum result(*this); SafeEnum result(*this);
result &= maskValue; result &= maskValue;
return result; return result;
} }
SafeEnum operator~() { SafeEnum operator~()
{
SafeEnum result(*this); SafeEnum result(*this);
result.mFlags = ~result.mFlags; result.mFlags = ~result.mFlags;
return result; return result;
@ -93,7 +110,8 @@ namespace gz_quadruped_hardware {
// SystemInterface provides API-level access to read and command joint properties. // SystemInterface provides API-level access to read and command joint properties.
class GazeboSimSystemInterface class GazeboSimSystemInterface
: public hardware_interface::SystemInterface { : public hardware_interface::SystemInterface
{
public: public:
/// \brief Initialize the system interface /// \brief Initialize the system interface
/// param[in] model_nh Pointer to the ros2 node /// param[in] model_nh Pointer to the ros2 node
@ -103,23 +121,24 @@ namespace gz_quadruped_hardware {
/// param[in] _ecm Entity-component manager. /// param[in] _ecm Entity-component manager.
/// param[in] update_rate controller update rate /// param[in] update_rate controller update rate
virtual bool initSim( virtual bool initSim(
rclcpp::Node::SharedPtr &model_nh, rclcpp::Node::SharedPtr& model_nh,
std::map<std::string, sim::Entity> &joints, std::map<std::string, sim::Entity>& joints,
const hardware_interface::HardwareInfo &hardware_info, const hardware_interface::HardwareInfo& hardware_info,
sim::EntityComponentManager &_ecm, sim::EntityComponentManager& _ecm,
unsigned int update_rate) = 0; int& update_rate) = 0;
// Methods used to control a joint. // Methods used to control a joint.
enum ControlMethod_ { enum ControlMethod_
{
NONE = 0, NONE = 0,
POSITION = (1 << 0), POSITION = (1 << 0),
VELOCITY = (1 << 1), VELOCITY = (1 << 1),
EFFORT = (1 << 2), EFFORT = (1 << 2),
}; };
typedef SafeEnum<ControlMethod_> ControlMethod; typedef SafeEnum<enum ControlMethod_> ControlMethod;
protected: protected:
rclcpp::Node::SharedPtr nh_; rclcpp::Node::SharedPtr nh_;
}; };
} // namespace gz_ros2_control }

536
hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp
View File

@ -17,12 +17,13 @@
#include <chrono> #include <chrono>
#include <map> #include <map>
#include <memory> #include <memory>
#include <mutex> #include <regex>
#include <string> #include <string>
#include <thread> #include <thread>
#include <utility> #include <utility>
#include <vector> #include <vector>
#ifdef GZ_HEADERS
#include <gz/sim/components/Joint.hh> #include <gz/sim/components/Joint.hh>
#include <gz/sim/components/JointType.hh> #include <gz/sim/components/JointType.hh>
#include <gz/sim/components/Name.hh> #include <gz/sim/components/Name.hh>
@ -30,12 +31,21 @@
#include <gz/sim/components/World.hh> #include <gz/sim/components/World.hh>
#include <gz/sim/Model.hh> #include <gz/sim/Model.hh>
#include <gz/plugin/Register.hh> #include <gz/plugin/Register.hh>
#else
#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/World.hh>
#include <ignition/gazebo/Model.hh>
#include <ignition/plugin/Register.hh>
#endif
#include <controller_manager/controller_manager.hpp> #include <controller_manager/controller_manager.hpp>
#include <hardware_interface/resource_manager.hpp> #include <hardware_interface/resource_manager.hpp>
#include <hardware_interface/component_parser.hpp> #include <hardware_interface/component_parser.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <pluginlib/class_loader.hpp> #include <pluginlib/class_loader.hpp>
@ -44,89 +54,15 @@
#include "gz_quadruped_hardware/gz_quadruped_plugin.hpp" #include "gz_quadruped_hardware/gz_quadruped_plugin.hpp"
#include "gz_quadruped_hardware/gz_system.hpp" #include "gz_quadruped_hardware/gz_system.hpp"
namespace gz_quadruped_hardware { namespace gz_quadruped_hardware
class GZResourceManager : public hardware_interface::ResourceManager { {
public:
GZResourceManager(
rclcpp::Node::SharedPtr &node,
sim::EntityComponentManager &ecm,
std::map<std::string, sim::Entity> enabledJoints)
: ResourceManager(
node->get_node_clock_interface(), node->get_node_logging_interface()),
gz_system_loader_("gz_quadruped_hardware", "gz_quadruped_hardware::GazeboSimSystemInterface"),
logger_(node->get_logger().get_child("GZResourceManager")) {
node_ = node;
ecm_ = &ecm;
enabledJoints_ = enabledJoints;
}
GZResourceManager(const GZResourceManager &) = delete;
// Called from Controller Manager when robot description is initialized from callback
bool load_and_initialize_components(
const std::string &urdf,
unsigned int update_rate) override {
components_are_loaded_and_initialized_ = true;
const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf);
for (const auto &individual_hardware_info: hardware_info) {
std::string robot_hw_sim_type_str_ = individual_hardware_info.hardware_plugin_name;
RCLCPP_DEBUG(
logger_, "Load hardware interface %s ...",
robot_hw_sim_type_str_.c_str());
// Load hardware
std::unique_ptr<GazeboSimSystemInterface> gzSimSystem;
std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_);
try {
gzSimSystem = std::unique_ptr<GazeboSimSystemInterface>(
gz_system_loader_.createUnmanagedInstance(robot_hw_sim_type_str_));
} catch (pluginlib::PluginlibException &ex) {
RCLCPP_ERROR(
logger_,
"The plugin failed to load for some reason. Error: %s\n",
ex.what());
continue;
}
// initialize simulation requirements
if (!gzSimSystem->initSim(
node_,
enabledJoints_,
individual_hardware_info,
*ecm_,
update_rate)) {
RCLCPP_FATAL(
logger_, "Could not initialize robot simulation interface");
components_are_loaded_and_initialized_ = false;
break;
}
RCLCPP_DEBUG(
logger_, "Initialized robot simulation interface %s!",
robot_hw_sim_type_str_.c_str());
// initialize hardware
import_component(std::move(gzSimSystem), individual_hardware_info);
}
return components_are_loaded_and_initialized_;
}
private:
std::shared_ptr<rclcpp::Node> node_;
sim::EntityComponentManager *ecm_;
std::map<std::string, sim::Entity> enabledJoints_;
/// \brief Interface loader
pluginlib::ClassLoader<GazeboSimSystemInterface> gz_system_loader_;
rclcpp::Logger logger_;
};
////////////////////////////////////////////////// //////////////////////////////////////////////////
class GazeboSimQuadrupedPluginPrivate { class GazeboSimQuadrupedPluginPrivate
{
public: public:
/// \brief Get the URDF XML from the parameter server
std::string getURDF() const;
/// \brief Get a list of enabled, unique, 1-axis joints of the model. If no /// \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 /// joint names are specified in the plugin configuration, all valid 1-axis
/// joints are returned /// joints are returned
@ -135,8 +71,8 @@ namespace gz_quadruped_hardware {
/// \param[in] _ecm Gazebo Entity Component Manager /// \param[in] _ecm Gazebo Entity Component Manager
/// \return List of entities containing all enabled joints /// \return List of entities containing all enabled joints
std::map<std::string, sim::Entity> GetEnabledJoints( std::map<std::string, sim::Entity> GetEnabledJoints(
const sim::Entity &_entity, const sim::Entity& _entity,
sim::EntityComponentManager &_ecm) const; sim::EntityComponentManager& _ecm) const;
/// \brief Entity ID for sensor within Gazebo. /// \brief Entity ID for sensor within Gazebo.
sim::Entity entity_; sim::Entity entity_;
@ -153,16 +89,27 @@ namespace gz_quadruped_hardware {
/// \brief Timing /// \brief Timing
rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); rclcpp::Duration control_period_ = rclcpp::Duration(1, 0);
/// \brief Interface loader
std::shared_ptr<pluginlib::ClassLoader<
GazeboSimSystemInterface>>
robot_hw_sim_loader_{nullptr};
/// \brief Controller manager /// \brief Controller manager
std::shared_ptr<controller_manager::ControllerManager> std::shared_ptr<controller_manager::ControllerManager>
controller_manager_{nullptr}; controller_manager_{nullptr};
/// \brief String with the robot description param_name
std::string robot_description_ = "robot_description";
/// \brief String with the name of the node that contains the robot_description
std::string robot_description_node_ = "robot_state_publisher";
/// \brief Last time the update method was called /// \brief Last time the update method was called
rclcpp::Time last_update_sim_time_ros_ = rclcpp::Time last_update_sim_time_ros_ =
rclcpp::Time(static_cast<int64_t>(0), RCL_ROS_TIME); rclcpp::Time((int64_t)0, RCL_ROS_TIME);
/// \brief ECM pointer /// \brief ECM pointer
sim::EntityComponentManager *ecm{nullptr}; sim::EntityComponentManager* ecm{nullptr};
/// \brief controller update rate /// \brief controller update rate
int update_rate; int update_rate;
@ -171,8 +118,9 @@ namespace gz_quadruped_hardware {
////////////////////////////////////////////////// //////////////////////////////////////////////////
std::map<std::string, sim::Entity> std::map<std::string, sim::Entity>
GazeboSimQuadrupedPluginPrivate::GetEnabledJoints( GazeboSimQuadrupedPluginPrivate::GetEnabledJoints(
const sim::Entity &_entity, const sim::Entity& _entity,
sim::EntityComponentManager &_ecm) const { sim::EntityComponentManager& _ecm) const
{
std::map<std::string, sim::Entity> output; std::map<std::string, sim::Entity> output;
std::vector<std::string> enabledJoints; std::vector<std::string> enabledJoints;
@ -181,32 +129,37 @@ namespace gz_quadruped_hardware {
auto jointEntities = _ecm.ChildrenByComponents(_entity, sim::components::Joint()); auto jointEntities = _ecm.ChildrenByComponents(_entity, sim::components::Joint());
// Iterate over all joints and verify whether they can be enabled or not // Iterate over all joints and verify whether they can be enabled or not
for (const auto &jointEntity: jointEntities) { for (const auto& jointEntity : jointEntities)
{
const auto jointName = _ecm.Component<sim::components::Name>( const auto jointName = _ecm.Component<sim::components::Name>(
jointEntity)->Data(); jointEntity)->Data();
// Make sure the joint type is supported, i.e. it has exactly one // Make sure the joint type is supported, i.e. it has exactly one
// actuated axis // actuated axis
const auto *jointType = _ecm.Component<sim::components::JointType>(jointEntity); const auto* jointType = _ecm.Component<sim::components::JointType>(jointEntity);
switch (jointType->Data()) { switch (jointType->Data())
case sdf::JointType::PRISMATIC: {
case sdf::JointType::REVOLUTE: case sdf::JointType::PRISMATIC:
case sdf::JointType::CONTINUOUS: case sdf::JointType::REVOLUTE:
case sdf::JointType::GEARBOX: { case sdf::JointType::CONTINUOUS:
case sdf::JointType::GEARBOX:
{
// Supported joint type // Supported joint type
break; break;
} }
case sdf::JointType::FIXED: { case sdf::JointType::FIXED:
{
RCLCPP_INFO( RCLCPP_INFO(
node_->get_logger(), node_->get_logger(),
"[gz_quadruped_hardware] Fixed joint [%s] (Entity=%lu)] is skipped", "[gz_quadruped_hardware] Fixed joint [%s] (Entity=%lu)] is skipped",
jointName.c_str(), jointEntity); jointName.c_str(), jointEntity);
continue; continue;
} }
case sdf::JointType::REVOLUTE2: case sdf::JointType::REVOLUTE2:
case sdf::JointType::SCREW: case sdf::JointType::SCREW:
case sdf::JointType::BALL: case sdf::JointType::BALL:
case sdf::JointType::UNIVERSAL: { case sdf::JointType::UNIVERSAL:
{
RCLCPP_WARN( RCLCPP_WARN(
node_->get_logger(), node_->get_logger(),
"[gz_quadruped_hardware] Joint [%s] (Entity=%lu)] is of unsupported type." "[gz_quadruped_hardware] Joint [%s] (Entity=%lu)] is of unsupported type."
@ -214,7 +167,8 @@ namespace gz_quadruped_hardware {
jointName.c_str(), jointEntity); jointName.c_str(), jointEntity);
continue; continue;
} }
default: { default:
{
RCLCPP_WARN( RCLCPP_WARN(
node_->get_logger(), node_->get_logger(),
"[gz_quadruped_hardware] Joint [%s] (Entity=%lu)] is of unknown type", "[gz_quadruped_hardware] Joint [%s] (Entity=%lu)] is of unknown type",
@ -229,14 +183,81 @@ namespace gz_quadruped_hardware {
} }
////////////////////////////////////////////////// //////////////////////////////////////////////////
GazeboSimQuadrupedPlugin::GazeboSimQuadrupedPlugin() std::string GazeboSimQuadrupedPluginPrivate::getURDF() const
: dataPtr(std::make_unique<GazeboSimQuadrupedPluginPrivate>()) { {
std::string urdf_string;
using namespace std::chrono_literals;
auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(
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(),
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<rclcpp::Parameter> 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(), "gz_quadruped_hardware plugin is waiting for model"
" URDF in parameter [%s] on the ROS param server.",
this->robot_description_.c_str());
}
std::this_thread::sleep_for(std::chrono::microseconds(100000));
}
RCLCPP_INFO(node_->get_logger(), "Received URDF from param server");
return urdf_string;
} }
////////////////////////////////////////////////// //////////////////////////////////////////////////
GazeboSimQuadrupedPlugin::~GazeboSimQuadrupedPlugin() { GazeboSimQuadrupedPlugin::GazeboSimQuadrupedPlugin()
: dataPtr(std::make_unique<GazeboSimQuadrupedPluginPrivate>())
{
}
//////////////////////////////////////////////////
GazeboSimQuadrupedPlugin::~GazeboSimQuadrupedPlugin()
{
// Stop controller manager thread // Stop controller manager thread
if (!this->dataPtr->controller_manager_) { if (!this->dataPtr->controller_manager_)
{
return; return;
} }
this->dataPtr->executor_->remove_node(this->dataPtr->controller_manager_); this->dataPtr->executor_->remove_node(this->dataPtr->controller_manager_);
@ -246,39 +267,61 @@ namespace gz_quadruped_hardware {
////////////////////////////////////////////////// //////////////////////////////////////////////////
void GazeboSimQuadrupedPlugin::Configure( void GazeboSimQuadrupedPlugin::Configure(
const sim::Entity &_entity, const sim::Entity& _entity,
const std::shared_ptr<const sdf::Element> &_sdf, const std::shared_ptr<const sdf::Element>& _sdf,
sim::EntityComponentManager &_ecm, sim::EntityComponentManager& _ecm,
sim::EventManager &) { sim::EventManager&)
rclcpp::Logger logger = rclcpp::get_logger("GazeboSimROS2ControlPlugin"); {
rclcpp::Logger logger = rclcpp::get_logger("GazeboSimQuadrupedPlugin");
// Make sure the controller is attached to a valid model // Make sure the controller is attached to a valid model
const auto model = sim::Model(_entity); const auto model = sim::Model(_entity);
if (!model.Valid(_ecm)) { if (!model.Valid(_ecm))
{
RCLCPP_ERROR( RCLCPP_ERROR(
logger, logger,
"[Gazebo ROS 2 Control] Failed to initialize because [%s] (Entity=%lu)] is not a model." "[gz_quadruped_hardware] Failed to initialize because [%s] (Entity=%lu)] is not a model."
"Please make sure that Gazebo ROS 2 Control is attached to a valid model.", "Please make sure that gz_quadruped_hardware is attached to a valid model.",
model.Name(_ecm).c_str(), _entity); model.Name(_ecm).c_str(), _entity);
return; return;
} }
// Get params from SDF // Get params from SDF
auto param_file_name = _sdf->Get<std::string>("parameters"); std::string paramFileName = _sdf->Get<std::string>("parameters");
if (param_file_name.empty()) { if (paramFileName.empty())
{
RCLCPP_ERROR( RCLCPP_ERROR(
logger, logger,
"Gazebo quadruped ros2 control found an empty parameters file. Failed to initialize."); "gz_quadruped_hardware found an empty parameters file. Failed to initialize.");
return; return;
} }
// Get params from SDF // Get params from SDF
std::string robot_param_node = _sdf->Get<std::string>("robot_param_node");
if (!robot_param_node.empty())
{
this->dataPtr->robot_description_node_ = robot_param_node;
}
RCLCPP_INFO(
logger,
"robot_param_node is %s", this->dataPtr->robot_description_node_.c_str());
std::string robot_description = _sdf->Get<std::string>("robot_param");
if (!robot_description.empty())
{
this->dataPtr->robot_description_ = robot_description;
}
RCLCPP_INFO(
logger,
"robot_param_node is %s", this->dataPtr->robot_description_.c_str());
std::vector<std::string> arguments = {"--ros-args"}; std::vector<std::string> arguments = {"--ros-args"};
auto sdfPtr = const_cast<sdf::Element *>(_sdf.get()); auto sdfPtr = const_cast<sdf::Element*>(_sdf.get());
sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters"); sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters");
while (argument_sdf) { while (argument_sdf)
{
std::string argument = argument_sdf->Get<std::string>(); std::string argument = argument_sdf->Get<std::string>();
arguments.push_back(RCL_PARAM_FILE_FLAG); arguments.push_back(RCL_PARAM_FILE_FLAG);
arguments.push_back(argument); arguments.push_back(argument);
@ -288,30 +331,39 @@ namespace gz_quadruped_hardware {
// Get controller manager node name // Get controller manager node name
std::string controllerManagerNodeName{"controller_manager"}; std::string controllerManagerNodeName{"controller_manager"};
if (sdfPtr->HasElement("controller_manager_name")) { if (sdfPtr->HasElement("controller_manager_name"))
{
controllerManagerNodeName = sdfPtr->GetElement("controller_manager_name")->Get<std::string>(); controllerManagerNodeName = sdfPtr->GetElement("controller_manager_name")->Get<std::string>();
} }
std::string ns = "/"; std::string ns = "/";
if (sdfPtr->HasElement("ros"))
if (sdfPtr->HasElement("ros")) { {
sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros");
// Set namespace if tag is present // Set namespace if tag is present
if (sdfRos->HasElement("namespace")) { if (sdfRos->HasElement("namespace"))
{
ns = sdfRos->GetElement("namespace")->Get<std::string>(); ns = sdfRos->GetElement("namespace")->Get<std::string>();
// prevent exception: namespace must be absolute, it must lead with a '/' // prevent exception: namespace must be absolute, it must lead with a '/'
if (ns.empty() || ns[0] != '/') { if (ns.empty() || ns[0] != '/')
{
ns = '/' + ns; ns = '/' + ns;
} }
if (ns.length() > 1)
{
this->dataPtr->robot_description_node_ = ns + "/" + this->dataPtr->robot_description_node_;
}
} }
// Get list of remapping rules from SDF // Get list of remapping rules from SDF
if (sdfRos->HasElement("remapping")) { if (sdfRos->HasElement("remapping"))
{
sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping"); sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping");
arguments.push_back(RCL_ROS_ARGS_FLAG); arguments.push_back(RCL_ROS_ARGS_FLAG);
while (argument_sdf) { while (argument_sdf)
{
auto argument = argument_sdf->Get<std::string>(); auto argument = argument_sdf->Get<std::string>();
arguments.push_back(RCL_REMAP_FLAG); arguments.push_back(RCL_REMAP_FLAG);
arguments.push_back(argument); arguments.push_back(argument);
@ -320,19 +372,15 @@ namespace gz_quadruped_hardware {
} }
} }
std::vector<const char *> argv;
for (const auto &arg: arguments) {
argv.push_back(arg.data());
}
// Create a default context, if not already // Create a default context, if not already
if (!rclcpp::ok()) { if (!rclcpp::ok())
init( {
static_cast<int>(argv.size()), argv.data(), rclcpp::InitOptions(), RCLCPP_DEBUG_STREAM(logger, "Create default context");
rclcpp::SignalHandlerOptions::None); std::vector<const char*> argv;
rclcpp::init(static_cast<int>(argv.size()), argv.data());
} }
std::string node_name = "gz_quadruped_control"; std::string node_name = "gz_quadruped_hardware";
this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns); this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns);
this->dataPtr->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>(); this->dataPtr->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
this->dataPtr->executor_->add_node(this->dataPtr->node_); this->dataPtr->executor_->add_node(this->dataPtr->node_);
@ -342,8 +390,54 @@ namespace gz_quadruped_hardware {
}; };
this->dataPtr->thread_executor_spin_ = std::thread(spin); this->dataPtr->thread_executor_spin_ = std::thread(spin);
RCLCPP_DEBUG_STREAM(logger, "Create node " << node_name);
// Read urdf from ros parameter server
const auto urdf_string = this->dataPtr->getURDF();
if (urdf_string.empty())
{
RCLCPP_ERROR_STREAM(this->dataPtr->node_->get_logger(), "An empty URDF was passed. Exiting.");
return;
}
// set the robot description as argument to propagate it among controller manager and controllers
// Remove all comments via regex pattern to match XML comments, including newlines
const std::regex comment_pattern(R"(<!--[\s\S]*?-->)");
const auto rb_arg = std::string("robot_description:=") + std::regex_replace(
urdf_string,
comment_pattern, "");
arguments.push_back(RCL_PARAM_FLAG);
arguments.push_back(rb_arg);
std::vector<const char*> argv;
for (const auto& arg : arguments)
{
argv.push_back(reinterpret_cast<const char*>(arg.data()));
}
// set the arguments into rcl context
rcl_arguments_t rcl_args = rcl_get_zero_initialized_arguments();
rcl_ret_t rcl_ret = rcl_parse_arguments(
static_cast<int>(argv.size()),
argv.data(), rcl_get_default_allocator(), &rcl_args);
auto rcl_context =
this->dataPtr->node_->get_node_base_interface()->get_context()->get_rcl_context();
rcl_context->global_arguments = rcl_args;
if (rcl_ret != RCL_RET_OK)
{
RCLCPP_ERROR_STREAM(
this->dataPtr->node_->get_logger(), "Argument parser error: " << rcl_get_error_string().str);
rcl_reset_error();
return;
}
if (rcl_arguments_get_param_files_count(&rcl_args) < 1)
{
RCLCPP_ERROR(this->dataPtr->node_->get_logger(), "Failed to parse input yaml file(s)");
return;
}
RCLCPP_DEBUG_STREAM( RCLCPP_DEBUG_STREAM(
this->dataPtr->node_->get_logger(), "[Gazebo Quadruped ROS2 Control] Setting up controller for [" << this->dataPtr->node_->get_logger(), "[gz_quadruped_hardware] Setting up controller for [" <<
model.Name(_ecm) << "] (Entity=" << _entity << ")]."); model.Name(_ecm) << "] (Entity=" << _entity << ")].");
// Get list of enabled joints // Get list of enabled joints
@ -351,15 +445,100 @@ namespace gz_quadruped_hardware {
_entity, _entity,
_ecm); _ecm);
if (enabledJoints.size() == 0) { if (enabledJoints.size() == 0)
{
RCLCPP_DEBUG_STREAM( RCLCPP_DEBUG_STREAM(
this->dataPtr->node_->get_logger(), this->dataPtr->node_->get_logger(),
"[Gazebo Quadruped ROS2 Control] There are no available Joints."); "[gz_quadruped_hardware] 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::vector<hardware_interface::HardwareInfo> control_hardware_info;
try
{
control_hardware_info = 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 gz_quadruped_hardware plugin, plugin not active : " << ex.what());
return; return;
} }
std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ = std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
std::make_unique<GZResourceManager>(this->dataPtr->node_, _ecm, enabledJoints); std::make_unique<hardware_interface::ResourceManager>();
try
{
resource_manager_->load_urdf(urdf_string, false, false);
}
catch (...)
{
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Error initializing URDF to resource manager!");
}
try
{
this->dataPtr->robot_hw_sim_loader_.reset(
new pluginlib::ClassLoader<GazeboSimSystemInterface>(
"gz_quadruped_hardware",
"gz_quadruped_hardware::GazeboSimSystemInterface"));
}
catch (pluginlib::LibraryLoadException& ex)
{
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Failed to create robot simulation interface loader: %s ",
ex.what());
return;
}
for (unsigned int i = 0; i < control_hardware_info.size(); ++i)
{
std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_class_type;
std::unique_ptr<GazeboSimSystemInterface> gzSimSystem;
RCLCPP_DEBUG(
this->dataPtr->node_->get_logger(), "Load hardware interface %s ...",
robot_hw_sim_type_str_.c_str());
try
{
gzSimSystem = std::unique_ptr<GazeboSimSystemInterface>(
this->dataPtr->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_));
}
catch (pluginlib::PluginlibException& ex)
{
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(),
"The plugin failed to load for some reason. Error: %s\n",
ex.what());
continue;
}
if (!gzSimSystem->initSim(
this->dataPtr->node_,
enabledJoints,
control_hardware_info[i],
_ecm,
this->dataPtr->update_rate))
{
RCLCPP_FATAL(
this->dataPtr->node_->get_logger(), "Could not initialize robot simulation interface");
return;
}
RCLCPP_DEBUG(
this->dataPtr->node_->get_logger(), "Initialized robot simulation interface %s!",
robot_hw_sim_type_str_.c_str());
resource_manager_->import_component(std::move(gzSimSystem), control_hardware_info[i]);
rclcpp_lifecycle::State state(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
hardware_interface::lifecycle_state_names::ACTIVE);
resource_manager_->set_component_state(control_hardware_info[i].name, state);
}
// Create the controller manager // Create the controller manager
RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager"); RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager");
@ -374,10 +553,19 @@ namespace gz_quadruped_hardware {
std::move(resource_manager_), std::move(resource_manager_),
this->dataPtr->executor_, this->dataPtr->executor_,
controllerManagerNodeName, controllerManagerNodeName,
this->dataPtr->node_->get_namespace(), options)); this->dataPtr->node_->get_namespace()));
this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_); this->dataPtr->executor_->add_node(this->dataPtr->controller_manager_);
this->dataPtr->update_rate = this->dataPtr->controller_manager_->get_update_rate(); 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;
}
this->dataPtr->update_rate =
this->dataPtr->controller_manager_->get_parameter("update_rate").as_int();
this->dataPtr->control_period_ = rclcpp::Duration( this->dataPtr->control_period_ = rclcpp::Duration(
std::chrono::duration_cast<std::chrono::nanoseconds>( std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / static_cast<double>(this->dataPtr->update_rate)))); std::chrono::duration<double>(1.0 / static_cast<double>(this->dataPtr->update_rate))));
@ -386,36 +574,34 @@ namespace gz_quadruped_hardware {
this->dataPtr->controller_manager_->set_parameter( this->dataPtr->controller_manager_->set_parameter(
rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true))); rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true)));
// Wait for CM to receive robot description from the topic and then initialize Resource Manager
while (!this->dataPtr->controller_manager_->is_resource_manager_initialized()) {
RCLCPP_WARN(
this->dataPtr->node_->get_logger(),
"Waiting RM to load and initialize hardware...");
std::this_thread::sleep_for(std::chrono::microseconds(2000000));
}
this->dataPtr->entity_ = _entity; this->dataPtr->entity_ = _entity;
} }
////////////////////////////////////////////////// //////////////////////////////////////////////////
void GazeboSimQuadrupedPlugin::PreUpdate( void GazeboSimQuadrupedPlugin::PreUpdate(
const sim::UpdateInfo &_info, const sim::UpdateInfo& _info,
sim::EntityComponentManager & /*_ecm*/) { sim::EntityComponentManager& /*_ecm*/)
if (!this->dataPtr->controller_manager_) { {
if (!this->dataPtr->controller_manager_)
{
return; return;
} }
static bool warned{false}; static bool warned{false};
if (!warned) { if (!warned)
{
rclcpp::Duration gazebo_period(_info.dt); rclcpp::Duration gazebo_period(_info.dt);
// Check the period against the simulation period // Check the period against the simulation period
if (this->dataPtr->control_period_ < _info.dt) { if (this->dataPtr->control_period_ < _info.dt)
{
RCLCPP_ERROR_STREAM( RCLCPP_ERROR_STREAM(
this->dataPtr->node_->get_logger(), this->dataPtr->node_->get_logger(),
"Desired controller update period (" << this->dataPtr->control_period_.seconds() << "Desired controller update period (" << this->dataPtr->control_period_.seconds() <<
" s) is faster than the gazebo simulation period (" << " s) is faster than the gazebo simulation period (" <<
gazebo_period.seconds() << " s)."); gazebo_period.seconds() << " s).");
} else if (this->dataPtr->control_period_ > gazebo_period) { }
else if (this->dataPtr->control_period_ > gazebo_period)
{
RCLCPP_WARN_STREAM( RCLCPP_WARN_STREAM(
this->dataPtr->node_->get_logger(), this->dataPtr->node_->get_logger(),
" Desired controller update period (" << this->dataPtr->control_period_.seconds() << " Desired controller update period (" << this->dataPtr->control_period_.seconds() <<
@ -425,9 +611,9 @@ namespace gz_quadruped_hardware {
warned = true; warned = true;
} }
const rclcpp::Time sim_time_ros(std::chrono::duration_cast<std::chrono::nanoseconds>( rclcpp::Time sim_time_ros(std::chrono::duration_cast<std::chrono::nanoseconds>(
_info.simTime).count(), RCL_ROS_TIME); _info.simTime).count(), RCL_ROS_TIME);
const rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_;
// Always set commands on joints, otherwise at low control frequencies the joints tremble // Always set commands on joints, otherwise at low control frequencies the joints tremble
// as they are updated at a fraction of gazebo sim time // as they are updated at a fraction of gazebo sim time
this->dataPtr->controller_manager_->write(sim_time_ros, sim_period); this->dataPtr->controller_manager_->write(sim_time_ros, sim_period);
@ -435,30 +621,42 @@ namespace gz_quadruped_hardware {
////////////////////////////////////////////////// //////////////////////////////////////////////////
void GazeboSimQuadrupedPlugin::PostUpdate( void GazeboSimQuadrupedPlugin::PostUpdate(
const sim::UpdateInfo &_info, const sim::UpdateInfo& _info,
const sim::EntityComponentManager & /*_ecm*/) { const sim::EntityComponentManager& /*_ecm*/)
if (!this->dataPtr->controller_manager_) { {
if (!this->dataPtr->controller_manager_)
{
return; return;
} }
// Get the simulation time and period // Get the simulation time and period
const rclcpp::Time sim_time_ros(std::chrono::duration_cast<std::chrono::nanoseconds>( rclcpp::Time sim_time_ros(std::chrono::duration_cast<std::chrono::nanoseconds>(
_info.simTime).count(), RCL_ROS_TIME); _info.simTime).count(), RCL_ROS_TIME);
const rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_; rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_;
if (sim_period >= this->dataPtr->control_period_) { if (sim_period >= this->dataPtr->control_period_)
{
this->dataPtr->last_update_sim_time_ros_ = sim_time_ros; this->dataPtr->last_update_sim_time_ros_ = sim_time_ros;
auto gz_controller_manager = auto gz_controller_manager =
std::dynamic_pointer_cast<GazeboSimSystemInterface>( std::dynamic_pointer_cast<GazeboSimSystemInterface>(
this->dataPtr->controller_manager_); this->dataPtr->controller_manager_);
this->dataPtr->controller_manager_->read(sim_time_ros, sim_period); this->dataPtr->controller_manager_->read(sim_time_ros, sim_period);
this->dataPtr->controller_manager_->update(sim_time_ros, sim_period); this->dataPtr->controller_manager_->update(sim_time_ros, sim_period);
} }
} }
} // namespace gz_quadruped_hardware } // namespace gz_quadruped_hardware
#ifdef GZ_HEADERS
GZ_ADD_PLUGIN( GZ_ADD_PLUGIN(
gz_quadruped_hardware::GazeboSimQuadrupedPlugin, gz_quadruped_hardware::GazeboSimQuadrupedPlugin,
gz::sim::System, sim::System,
gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemConfigure, gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemConfigure,
gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPreUpdate, gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPreUpdate,
gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPostUpdate) gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPostUpdate)
#else
IGNITION_ADD_PLUGIN(
gz_quadruped_hardware::GazeboSimQuadrupedPlugin,
sim::System,
gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemConfigure,
gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPreUpdate,
gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPostUpdate)
#endif

611
hardwares/gz_quadruped_hardware/src/gz_ros2_control_plugin.cpp Normal file
View File

@ -0,0 +1,611 @@
// 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 <unistd.h>
#include <chrono>
#include <map>
#include <memory>
#include <regex>
#include <string>
#include <thread>
#include <utility>
#include <vector>
#ifdef GZ_HEADERS
#include <gz/sim/components/Joint.hh>
#include <gz/sim/components/JointType.hh>
#include <gz/sim/components/Name.hh>
#include <gz/sim/components/ParentEntity.hh>
#include <gz/sim/components/World.hh>
#include <gz/sim/Model.hh>
#include <gz/plugin/Register.hh>
#else
#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/World.hh>
#include <ignition/gazebo/Model.hh>
#include <ignition/plugin/Register.hh>
#endif
#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 <pluginlib/class_loader.hpp>
#include <rclcpp/rclcpp.hpp>
#include "gz_ros2_control/gz_ros2_control_plugin.hpp"
#include "gz_ros2_control/gz_system.hpp"
namespace gz_quadruped_hardware
{
//////////////////////////////////////////////////
class GazeboSimROS2ControlPluginPrivate
{
public:
/// \brief Get the URDF XML from the parameter server
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 Gazebo Entity Component Manager
/// \return List of entities containing all enabled joints
std::map<std::string, sim::Entity> GetEnabledJoints(
const sim::Entity & _entity,
sim::EntityComponentManager & _ecm) const;
/// \brief Entity ID for sensor within Gazebo.
sim::Entity entity_;
/// \brief Node Handles
std::shared_ptr<rclcpp::Node> node_{nullptr};
/// \brief Thread where the executor will spin
std::thread thread_executor_spin_;
/// \brief Executor to spin the controller
rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_;
/// \brief Timing
rclcpp::Duration control_period_ = rclcpp::Duration(1, 0);
/// \brief Interface loader
std::shared_ptr<pluginlib::ClassLoader<
gz_ros2_control::GazeboSimSystemInterface>>
robot_hw_sim_loader_{nullptr};
/// \brief Controller manager
std::shared_ptr<controller_manager::ControllerManager>
controller_manager_{nullptr};
/// \brief String with the robot description param_name
std::string robot_description_ = "robot_description";
/// \brief String with the name of the node that contains the robot_description
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((int64_t)0, RCL_ROS_TIME);
/// \brief ECM pointer
sim::EntityComponentManager * ecm{nullptr};
/// \brief controller update rate
int update_rate;
};
//////////////////////////////////////////////////
std::map<std::string, sim::Entity>
GazeboSimROS2ControlPluginPrivate::GetEnabledJoints(
const sim::Entity & _entity,
sim::EntityComponentManager & _ecm) const
{
std::map<std::string, sim::Entity> output;
std::vector<std::string> enabledJoints;
// Get all available joints
auto jointEntities = _ecm.ChildrenByComponents(_entity, sim::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<sim::components::Name>(
jointEntity)->Data();
// Make sure the joint type is supported, i.e. it has exactly one
// actuated axis
const auto * jointType = _ecm.Component<sim::components::JointType>(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(),
"[gz_ros2_control] Fixed joint [%s] (Entity=%lu)] 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(),
"[gz_ros2_control] Joint [%s] (Entity=%lu)] is of unsupported type."
" Only joints with a single axis are supported.",
jointName.c_str(), jointEntity);
continue;
}
default:
{
RCLCPP_WARN(
node_->get_logger(),
"[gz_ros2_control] Joint [%s] (Entity=%lu)] is of unknown type",
jointName.c_str(), jointEntity);
continue;
}
}
output[jointName] = jointEntity;
}
return output;
}
//////////////////////////////////////////////////
std::string GazeboSimROS2ControlPluginPrivate::getURDF() const
{
std::string urdf_string;
using namespace std::chrono_literals;
auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(
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(),
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<rclcpp::Parameter> 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(), "gz_ros2_control plugin is waiting for model"
" URDF in parameter [%s] on the ROS param server.",
this->robot_description_.c_str());
}
std::this_thread::sleep_for(std::chrono::microseconds(100000));
}
RCLCPP_INFO(node_->get_logger(), "Received URDF from param server");
return urdf_string;
}
//////////////////////////////////////////////////
GazeboSimROS2ControlPlugin::GazeboSimROS2ControlPlugin()
: dataPtr(std::make_unique<GazeboSimROS2ControlPluginPrivate>())
{
}
//////////////////////////////////////////////////
GazeboSimROS2ControlPlugin::~GazeboSimROS2ControlPlugin()
{
// Stop controller manager thread
if (!this->dataPtr->controller_manager_) {
return;
}
this->dataPtr->executor_->remove_node(this->dataPtr->controller_manager_);
this->dataPtr->executor_->cancel();
this->dataPtr->thread_executor_spin_.join();
}
//////////////////////////////////////////////////
void GazeboSimROS2ControlPlugin::Configure(
const sim::Entity & _entity,
const std::shared_ptr<const sdf::Element> & _sdf,
sim::EntityComponentManager & _ecm,
sim::EventManager &)
{
rclcpp::Logger logger = rclcpp::get_logger("GazeboSimROS2ControlPlugin");
// Make sure the controller is attached to a valid model
const auto model = sim::Model(_entity);
if (!model.Valid(_ecm)) {
RCLCPP_ERROR(
logger,
"[gz_ros2_control] Failed to initialize because [%s] (Entity=%lu)] is not a model."
"Please make sure that gz_ros2_control is attached to a valid model.",
model.Name(_ecm).c_str(), _entity);
return;
}
// Get params from SDF
std::string paramFileName = _sdf->Get<std::string>("parameters");
if (paramFileName.empty()) {
RCLCPP_ERROR(
logger,
"gz_ros2_control found an empty parameters file. Failed to initialize.");
return;
}
// Get params from SDF
std::string robot_param_node = _sdf->Get<std::string>("robot_param_node");
if (!robot_param_node.empty()) {
this->dataPtr->robot_description_node_ = robot_param_node;
}
RCLCPP_INFO(
logger,
"robot_param_node is %s", this->dataPtr->robot_description_node_.c_str());
std::string robot_description = _sdf->Get<std::string>("robot_param");
if (!robot_description.empty()) {
this->dataPtr->robot_description_ = robot_description;
}
RCLCPP_INFO(
logger,
"robot_param_node is %s", this->dataPtr->robot_description_.c_str());
std::vector<std::string> arguments = {"--ros-args"};
auto sdfPtr = const_cast<sdf::Element *>(_sdf.get());
sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters");
while (argument_sdf) {
std::string argument = argument_sdf->Get<std::string>();
arguments.push_back(RCL_PARAM_FILE_FLAG);
arguments.push_back(argument);
argument_sdf = argument_sdf->GetNextElement("parameters");
}
// Get controller manager node name
std::string controllerManagerNodeName{"controller_manager"};
if (sdfPtr->HasElement("controller_manager_name")) {
controllerManagerNodeName = sdfPtr->GetElement("controller_manager_name")->Get<std::string>();
}
std::string ns = "/";
if (sdfPtr->HasElement("ros")) {
sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros");
// Set namespace if tag is present
if (sdfRos->HasElement("namespace")) {
ns = sdfRos->GetElement("namespace")->Get<std::string>();
// prevent exception: namespace must be absolute, it must lead with a '/'
if (ns.empty() || ns[0] != '/') {
ns = '/' + ns;
}
if (ns.length() > 1) {
this->dataPtr->robot_description_node_ = ns + "/" + this->dataPtr->robot_description_node_;
}
}
// 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<std::string>();
arguments.push_back(RCL_REMAP_FLAG);
arguments.push_back(argument);
argument_sdf = argument_sdf->GetNextElement("remapping");
}
}
}
// Create a default context, if not already
if (!rclcpp::ok()) {
RCLCPP_DEBUG_STREAM(logger, "Create default context");
std::vector<const char *> argv;
rclcpp::init(static_cast<int>(argv.size()), argv.data());
}
std::string node_name = "gz_ros2_control";
this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns);
this->dataPtr->executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
this->dataPtr->executor_->add_node(this->dataPtr->node_);
auto spin = [this]()
{
this->dataPtr->executor_->spin();
};
this->dataPtr->thread_executor_spin_ = std::thread(spin);
RCLCPP_DEBUG_STREAM(logger, "Create node " << node_name);
// Read urdf from ros parameter server
const auto urdf_string = this->dataPtr->getURDF();
if (urdf_string.empty()) {
RCLCPP_ERROR_STREAM(this->dataPtr->node_->get_logger(), "An empty URDF was passed. Exiting.");
return;
}
// set the robot description as argument to propagate it among controller manager and controllers
// Remove all comments via regex pattern to match XML comments, including newlines
const std::regex comment_pattern(R"(<!--[\s\S]*?-->)");
const auto rb_arg = std::string("robot_description:=") + std::regex_replace(
urdf_string,
comment_pattern, "");
arguments.push_back(RCL_PARAM_FLAG);
arguments.push_back(rb_arg);
std::vector<const char *> argv;
for (const auto & arg : arguments) {
argv.push_back(reinterpret_cast<const char *>(arg.data()));
}
// set the arguments into rcl context
rcl_arguments_t rcl_args = rcl_get_zero_initialized_arguments();
rcl_ret_t rcl_ret = rcl_parse_arguments(
static_cast<int>(argv.size()),
argv.data(), rcl_get_default_allocator(), &rcl_args);
auto rcl_context =
this->dataPtr->node_->get_node_base_interface()->get_context()->get_rcl_context();
rcl_context->global_arguments = rcl_args;
if (rcl_ret != RCL_RET_OK) {
RCLCPP_ERROR_STREAM(
this->dataPtr->node_->get_logger(), "Argument parser error: " << rcl_get_error_string().str);
rcl_reset_error();
return;
}
if (rcl_arguments_get_param_files_count(&rcl_args) < 1) {
RCLCPP_ERROR(this->dataPtr->node_->get_logger(), "Failed to parse input yaml file(s)");
return;
}
RCLCPP_DEBUG_STREAM(
this->dataPtr->node_->get_logger(), "[gz_ros2_control] Setting up controller for [" <<
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(),
"[gz_ros2_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::vector<hardware_interface::HardwareInfo> control_hardware_info;
try {
control_hardware_info = 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 gz_ros2_control plugin, plugin not active : " << ex.what());
return;
}
std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ =
std::make_unique<hardware_interface::ResourceManager>();
try {
resource_manager_->load_urdf(urdf_string, false, false);
} catch (...) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Error initializing URDF to resource manager!");
}
try {
this->dataPtr->robot_hw_sim_loader_.reset(
new pluginlib::ClassLoader<gz_ros2_control::GazeboSimSystemInterface>(
"gz_ros2_control",
"gz_ros2_control::GazeboSimSystemInterface"));
} catch (pluginlib::LibraryLoadException & ex) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(), "Failed to create robot simulation interface loader: %s ",
ex.what());
return;
}
for (unsigned int i = 0; i < control_hardware_info.size(); ++i) {
std::string robot_hw_sim_type_str_ = control_hardware_info[i].hardware_class_type;
std::unique_ptr<gz_ros2_control::GazeboSimSystemInterface> gzSimSystem;
RCLCPP_DEBUG(
this->dataPtr->node_->get_logger(), "Load hardware interface %s ...",
robot_hw_sim_type_str_.c_str());
try {
gzSimSystem = std::unique_ptr<gz_ros2_control::GazeboSimSystemInterface>(
this->dataPtr->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_));
} catch (pluginlib::PluginlibException & ex) {
RCLCPP_ERROR(
this->dataPtr->node_->get_logger(),
"The plugin failed to load for some reason. Error: %s\n",
ex.what());
continue;
}
if (!gzSimSystem->initSim(
this->dataPtr->node_,
enabledJoints,
control_hardware_info[i],
_ecm,
this->dataPtr->update_rate))
{
RCLCPP_FATAL(
this->dataPtr->node_->get_logger(), "Could not initialize robot simulation interface");
return;
}
RCLCPP_DEBUG(
this->dataPtr->node_->get_logger(), "Initialized robot simulation interface %s!",
robot_hw_sim_type_str_.c_str());
resource_manager_->import_component(std::move(gzSimSystem), control_hardware_info[i]);
rclcpp_lifecycle::State state(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
hardware_interface::lifecycle_state_names::ACTIVE);
resource_manager_->set_component_state(control_hardware_info[i].name, state);
}
// 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_,
controllerManagerNodeName,
this->dataPtr->node_->get_namespace()));
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;
}
this->dataPtr->update_rate =
this->dataPtr->controller_manager_->get_parameter("update_rate").as_int();
this->dataPtr->control_period_ = rclcpp::Duration(
std::chrono::duration_cast<std::chrono::nanoseconds>(
std::chrono::duration<double>(1.0 / static_cast<double>(this->dataPtr->update_rate))));
// Force setting of use_sim_time parameter
this->dataPtr->controller_manager_->set_parameter(
rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true)));
this->dataPtr->entity_ = _entity;
}
//////////////////////////////////////////////////
void GazeboSimROS2ControlPlugin::PreUpdate(
const sim::UpdateInfo & _info,
sim::EntityComponentManager & /*_ecm*/)
{
if (!this->dataPtr->controller_manager_) {
return;
}
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;
}
rclcpp::Time sim_time_ros(std::chrono::duration_cast<std::chrono::nanoseconds>(
_info.simTime).count(), RCL_ROS_TIME);
rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_;
// 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(sim_time_ros, sim_period);
}
//////////////////////////////////////////////////
void GazeboSimROS2ControlPlugin::PostUpdate(
const sim::UpdateInfo & _info,
const sim::EntityComponentManager & /*_ecm*/)
{
if (!this->dataPtr->controller_manager_) {
return;
}
// Get the simulation time and period
rclcpp::Time sim_time_ros(std::chrono::duration_cast<std::chrono::nanoseconds>(
_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_) {
this->dataPtr->last_update_sim_time_ros_ = sim_time_ros;
auto gz_controller_manager =
std::dynamic_pointer_cast<gz_ros2_control::GazeboSimSystemInterface>(
this->dataPtr->controller_manager_);
this->dataPtr->controller_manager_->read(sim_time_ros, sim_period);
this->dataPtr->controller_manager_->update(sim_time_ros, sim_period);
}
}
} // namespace gz_ros2_control
#ifdef GZ_HEADERS
GZ_ADD_PLUGIN(
gz_ros2_control::GazeboSimROS2ControlPlugin,
sim::System,
gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemConfigure,
gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPreUpdate,
gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPostUpdate)
GZ_ADD_PLUGIN_ALIAS(
gz_ros2_control::GazeboSimROS2ControlPlugin,
"ign_ros2_control::IgnitionROS2ControlPlugin")
#else
IGNITION_ADD_PLUGIN(
gz_ros2_control::GazeboSimROS2ControlPlugin,
sim::System,
gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemConfigure,
gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPreUpdate,
gz_ros2_control::GazeboSimROS2ControlPlugin::ISystemPostUpdate)
IGNITION_ADD_PLUGIN_ALIAS(
gz_ros2_control::GazeboSimROS2ControlPlugin,
"ign_ros2_control::IgnitionROS2ControlPlugin")
#endif

80
hardwares/gz_quadruped_hardware/src/gz_system.cpp
View File

@ -202,7 +202,7 @@ namespace gz_quadruped_hardware
std::map<std::string, sim::Entity>& enableJoints, std::map<std::string, sim::Entity>& enableJoints,
const hardware_interface::HardwareInfo& hardware_info, const hardware_interface::HardwareInfo& hardware_info,
sim::EntityComponentManager& _ecm, sim::EntityComponentManager& _ecm,
unsigned int update_rate) int& update_rate)
{ {
this->dataPtr = std::make_unique<GazeboSimSystemPrivate>(); this->dataPtr = std::make_unique<GazeboSimSystemPrivate>();
this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time();
@ -272,24 +272,6 @@ namespace gz_quadruped_hardware
// Accept this joint and continue configuration // Accept this joint and continue configuration
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name); RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name);
// check if joint is mimicked
auto it = std::find_if(
hardware_info.mimic_joints.begin(),
hardware_info.mimic_joints.end(),
[j](const hardware_interface::MimicJoint& mj)
{
return mj.joint_index == j;
});
if (it != hardware_info.mimic_joints.end())
{
RCLCPP_INFO_STREAM(
this->nh_->get_logger(),
"Joint '" << joint_name << "'is mimicking joint '" <<
this->dataPtr->joints_[it->mimicked_joint_index].name <<
"' with multiplier: " << it->multiplier << " and offset: " << it->offset);
}
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");
auto get_initial_value = auto get_initial_value =
@ -536,9 +518,9 @@ namespace gz_quadruped_hardware
ftData->name = _name->Data(); ftData->name = _name->Data();
ftData->sim_ft_sensors_ = _entity; ftData->sim_ft_sensors_ = _entity;
this->dataPtr->state_interfaces_.emplace_back( this->dataPtr->state_interfaces_.emplace_back(
"foot_force", "foot_force",
ftData->name, ftData->name,
&ftData->foot_effort); &ftData->foot_effort);
this->dataPtr->ft_sensors_.push_back(ftData); this->dataPtr->ft_sensors_.push_back(ftData);
return true; return true;
}); });
@ -680,6 +662,60 @@ namespace gz_quadruped_hardware
return hardware_interface::return_type::OK; return hardware_interface::return_type::OK;
} }
hardware_interface::return_type
GazeboSimSystem::perform_command_mode_switch(
const std::vector<std::string>& start_interfaces,
const std::vector<std::string>& stop_interfaces)
{
for (unsigned int j = 0; j < this->dataPtr->joints_.size(); j++)
{
for (const std::string& interface_name : stop_interfaces)
{
// Clear joint control method bits corresponding to stop interfaces
if (interface_name == (this->dataPtr->joints_[j].name + "/" +
hardware_interface::HW_IF_POSITION))
{
this->dataPtr->joints_[j].joint_control_method &=
static_cast<ControlMethod_>(VELOCITY & EFFORT);
}
else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT
hardware_interface::HW_IF_VELOCITY))
{
this->dataPtr->joints_[j].joint_control_method &=
static_cast<ControlMethod_>(POSITION & EFFORT);
}
else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT
hardware_interface::HW_IF_EFFORT))
{
this->dataPtr->joints_[j].joint_control_method &=
static_cast<ControlMethod_>(POSITION & VELOCITY);
}
}
// Set joint control method bits corresponding to start interfaces
for (const std::string& interface_name : start_interfaces)
{
if (interface_name == (this->dataPtr->joints_[j].name + "/" +
hardware_interface::HW_IF_POSITION))
{
this->dataPtr->joints_[j].joint_control_method |= POSITION;
}
else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT
hardware_interface::HW_IF_VELOCITY))
{
this->dataPtr->joints_[j].joint_control_method |= VELOCITY;
}
else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT
hardware_interface::HW_IF_EFFORT))
{
this->dataPtr->joints_[j].joint_control_method |= EFFORT;
}
}
}
return hardware_interface::return_type::OK;
}
hardware_interface::return_type GazeboSimSystem::write( hardware_interface::return_type GazeboSimSystem::write(
const rclcpp::Time& /*time*/, const rclcpp::Time& /*time*/,
const rclcpp::Duration& /*period*/) const rclcpp::Duration& /*period*/)