migrate gz quadruped plugin to humble
This commit is contained in:
Huang Zhenbiao
parent
c039c4d083
commit
3163e86012
|
|
@ -194,7 +194,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -247,7 +247,7 @@
|
|||
<visual>
|
||||
<origin rpy="3.141592653589793 0 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -275,7 +275,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -303,7 +303,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -387,7 +387,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -415,7 +415,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -443,7 +443,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -527,7 +527,7 @@
|
|||
<visual>
|
||||
<origin rpy="3.141592653589793 3.141592653589793 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -555,7 +555,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -583,7 +583,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -667,7 +667,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 3.141592653589793 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -695,7 +695,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
@ -723,7 +723,7 @@
|
|||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<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>
|
||||
<!-- <material name="orange"/> -->
|
||||
</visual>
|
||||
|
|
|
|||
|
|
@ -3,18 +3,18 @@ project(gz_quadruped_hardware)
|
|||
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
|
||||
|
||||
# Default to C11
|
||||
if (NOT CMAKE_C_STANDARD)
|
||||
if(NOT CMAKE_C_STANDARD)
|
||||
set(CMAKE_C_STANDARD 11)
|
||||
endif ()
|
||||
endif()
|
||||
# Default to C++17
|
||||
if (NOT CMAKE_CXX_STANDARD)
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif ()
|
||||
endif()
|
||||
|
||||
# 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)
|
||||
endif ()
|
||||
endif()
|
||||
|
||||
# Find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
|
|
@ -25,11 +25,28 @@ find_package(pluginlib REQUIRED)
|
|||
find_package(rclcpp REQUIRED)
|
||||
find_package(yaml_cpp_vendor REQUIRED)
|
||||
|
||||
find_package(gz_sim_vendor REQUIRED)
|
||||
find_package(gz-sim REQUIRED)
|
||||
set(GZ_PLUGIN)
|
||||
set(GZ_SIM)
|
||||
|
||||
find_package(gz_plugin_vendor REQUIRED)
|
||||
find_package(gz-plugin REQUIRED)
|
||||
find_package(gz-sim8 QUIET)
|
||||
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)
|
||||
|
||||
|
|
@ -38,8 +55,8 @@ add_library(${PROJECT_NAME}-system SHARED
|
|||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}-system
|
||||
gz-sim::gz-sim
|
||||
gz-plugin::register
|
||||
${GZ_SIM}
|
||||
${GZ_PLUGIN}
|
||||
)
|
||||
ament_target_dependencies(${PROJECT_NAME}-system
|
||||
ament_index_cpp
|
||||
|
|
@ -62,7 +79,7 @@ ament_target_dependencies(gz_quadruped_plugins
|
|||
rclcpp
|
||||
)
|
||||
target_link_libraries(gz_quadruped_plugins
|
||||
gz-sim::gz-sim
|
||||
${GZ_SIM}
|
||||
)
|
||||
|
||||
## Install
|
||||
|
|
|
|||
|
|
@ -15,18 +15,25 @@
|
|||
|
||||
#include <memory>
|
||||
|
||||
#ifdef GZ_HEADERS
|
||||
#include <gz/sim/System.hh>
|
||||
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.
|
||||
class GazeboSimQuadrupedPluginPrivate;
|
||||
|
||||
class GazeboSimQuadrupedPlugin
|
||||
: public sim::System,
|
||||
public sim::ISystemConfigure,
|
||||
public sim::ISystemPreUpdate,
|
||||
public sim::ISystemPostUpdate {
|
||||
: public sim::System,
|
||||
public sim::ISystemConfigure,
|
||||
public sim::ISystemPreUpdate,
|
||||
public sim::ISystemPostUpdate
|
||||
{
|
||||
public:
|
||||
/// \brief Constructor
|
||||
GazeboSimQuadrupedPlugin();
|
||||
|
|
@ -36,22 +43,22 @@ namespace gz_quadruped_hardware {
|
|||
|
||||
// Documentation inherited
|
||||
void Configure(
|
||||
const sim::Entity &_entity,
|
||||
const std::shared_ptr<const sdf::Element> &_sdf,
|
||||
sim::EntityComponentManager &_ecm,
|
||||
sim::EventManager &_eventMgr) override;
|
||||
const sim::Entity& _entity,
|
||||
const std::shared_ptr<const sdf::Element>& _sdf,
|
||||
sim::EntityComponentManager& _ecm,
|
||||
sim::EventManager& _eventMgr) override;
|
||||
|
||||
// Documentation inherited
|
||||
void PreUpdate(
|
||||
const sim::UpdateInfo &_info,
|
||||
sim::EntityComponentManager &_ecm) override;
|
||||
const sim::UpdateInfo& _info,
|
||||
sim::EntityComponentManager& _ecm) override;
|
||||
|
||||
void PostUpdate(
|
||||
const sim::UpdateInfo &_info,
|
||||
const sim::EntityComponentManager &_ecm) override;
|
||||
const sim::UpdateInfo& _info,
|
||||
const sim::EntityComponentManager& _ecm) override;
|
||||
|
||||
private:
|
||||
/// \brief Private data pointer.
|
||||
std::unique_ptr<GazeboSimQuadrupedPluginPrivate> dataPtr;
|
||||
};
|
||||
} // namespace gz_ros2_control
|
||||
}
|
||||
|
|
|
|||
|
|
@ -13,7 +13,6 @@
|
|||
// limitations under the License.
|
||||
#pragma once
|
||||
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
|
@ -23,7 +22,8 @@
|
|||
#include "rclcpp_lifecycle/state.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;
|
||||
|
||||
// Forward declaration
|
||||
|
|
@ -32,13 +32,14 @@ namespace gz_quadruped_hardware {
|
|||
// These class must inherit `gz_ros2_control::GazeboSimSystemInterface` which implements a
|
||||
// simulated `ros2_control` `hardware_interface::SystemInterface`.
|
||||
|
||||
class GazeboSimSystem final : public GazeboSimSystemInterface {
|
||||
class GazeboSimSystem : public GazeboSimSystemInterface
|
||||
{
|
||||
public:
|
||||
// Documentation Inherited
|
||||
CallbackReturn on_init(const hardware_interface::HardwareInfo &system_info)
|
||||
CallbackReturn on_init(const hardware_interface::HardwareInfo& system_info)
|
||||
override;
|
||||
|
||||
CallbackReturn on_configure(const rclcpp_lifecycle::State &previous_state) override;
|
||||
CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
|
||||
|
||||
// Documentation Inherited
|
||||
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;
|
||||
|
||||
// Documentation Inherited
|
||||
CallbackReturn on_activate(const rclcpp_lifecycle::State &previous_state) override;
|
||||
CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
|
||||
|
||||
// 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
|
||||
hardware_interface::return_type read(
|
||||
const rclcpp::Time &time,
|
||||
const rclcpp::Duration &period) override;
|
||||
const rclcpp::Time& time,
|
||||
const rclcpp::Duration& period) override;
|
||||
|
||||
// Documentation Inherited
|
||||
hardware_interface::return_type write(
|
||||
const rclcpp::Time &time,
|
||||
const rclcpp::Duration &period) override;
|
||||
const rclcpp::Time& time,
|
||||
const rclcpp::Duration& period) override;
|
||||
|
||||
// Documentation Inherited
|
||||
bool initSim(
|
||||
rclcpp::Node::SharedPtr &model_nh,
|
||||
std::map<std::string, sim::Entity> &joints,
|
||||
const hardware_interface::HardwareInfo &hardware_info,
|
||||
sim::EntityComponentManager &_ecm,
|
||||
unsigned int update_rate) override;
|
||||
rclcpp::Node::SharedPtr& model_nh,
|
||||
std::map<std::string, sim::Entity>& joints,
|
||||
const hardware_interface::HardwareInfo& hardware_info,
|
||||
sim::EntityComponentManager& _ecm,
|
||||
int& update_rate) 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);
|
||||
const hardware_interface::HardwareInfo& hardware_info);
|
||||
|
||||
/// \brief Private data class
|
||||
std::unique_ptr<GazeboSimSystemPrivate> dataPtr;
|
||||
};
|
||||
} // namespace gz_ros2_control
|
||||
}
|
||||
|
|
|
|||
|
|
@ -16,15 +16,23 @@
|
|||
#include <map>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#ifdef GZ_HEADERS
|
||||
#include <gz/sim/System.hh>
|
||||
namespace sim = gz::sim;
|
||||
#else
|
||||
#include <ignition/gazebo/System.hh>
|
||||
namespace sim = ignition::gazebo;
|
||||
#endif
|
||||
|
||||
#include <hardware_interface/system_interface.hpp>
|
||||
#include <hardware_interface/types/hardware_interface_type_values.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
|
||||
///
|
||||
/// For example
|
||||
|
|
@ -42,44 +50,53 @@ namespace gz_quadruped_hardware {
|
|||
/// foo & POSITION -> True // Check if position 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>
|
||||
class SafeEnum {
|
||||
template <class ENUM, class UNDERLYING = typename std::underlying_type<ENUM>::type>
|
||||
class SafeEnum
|
||||
{
|
||||
public:
|
||||
SafeEnum()
|
||||
: mFlags(0) {
|
||||
: mFlags(0)
|
||||
{
|
||||
}
|
||||
|
||||
explicit SafeEnum(ENUM singleFlag)
|
||||
: mFlags(singleFlag) {
|
||||
: mFlags(singleFlag)
|
||||
{
|
||||
}
|
||||
|
||||
SafeEnum(const SafeEnum &original)
|
||||
: mFlags(original.mFlags) {
|
||||
SafeEnum(const SafeEnum& original)
|
||||
: mFlags(original.mFlags)
|
||||
{
|
||||
}
|
||||
|
||||
SafeEnum &operator|=(ENUM addValue) {
|
||||
SafeEnum& operator|=(ENUM addValue)
|
||||
{
|
||||
mFlags |= addValue;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SafeEnum operator|(ENUM addValue) {
|
||||
SafeEnum operator|(ENUM addValue)
|
||||
{
|
||||
SafeEnum result(*this);
|
||||
result |= addValue;
|
||||
return result;
|
||||
}
|
||||
|
||||
SafeEnum &operator&=(ENUM maskValue) {
|
||||
SafeEnum& operator&=(ENUM maskValue)
|
||||
{
|
||||
mFlags &= maskValue;
|
||||
return *this;
|
||||
}
|
||||
|
||||
SafeEnum operator&(ENUM maskValue) {
|
||||
SafeEnum operator&(ENUM maskValue)
|
||||
{
|
||||
SafeEnum result(*this);
|
||||
result &= maskValue;
|
||||
return result;
|
||||
}
|
||||
|
||||
SafeEnum operator~() {
|
||||
SafeEnum operator~()
|
||||
{
|
||||
SafeEnum result(*this);
|
||||
result.mFlags = ~result.mFlags;
|
||||
return result;
|
||||
|
|
@ -93,7 +110,8 @@ namespace gz_quadruped_hardware {
|
|||
|
||||
// SystemInterface provides API-level access to read and command joint properties.
|
||||
class GazeboSimSystemInterface
|
||||
: public hardware_interface::SystemInterface {
|
||||
: public hardware_interface::SystemInterface
|
||||
{
|
||||
public:
|
||||
/// \brief Initialize the system interface
|
||||
/// 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] update_rate controller update rate
|
||||
virtual bool initSim(
|
||||
rclcpp::Node::SharedPtr &model_nh,
|
||||
std::map<std::string, sim::Entity> &joints,
|
||||
const hardware_interface::HardwareInfo &hardware_info,
|
||||
sim::EntityComponentManager &_ecm,
|
||||
unsigned int update_rate) = 0;
|
||||
rclcpp::Node::SharedPtr& model_nh,
|
||||
std::map<std::string, sim::Entity>& joints,
|
||||
const hardware_interface::HardwareInfo& hardware_info,
|
||||
sim::EntityComponentManager& _ecm,
|
||||
int& update_rate) = 0;
|
||||
|
||||
// Methods used to control a joint.
|
||||
enum ControlMethod_ {
|
||||
enum ControlMethod_
|
||||
{
|
||||
NONE = 0,
|
||||
POSITION = (1 << 0),
|
||||
VELOCITY = (1 << 1),
|
||||
EFFORT = (1 << 2),
|
||||
};
|
||||
|
||||
typedef SafeEnum<ControlMethod_> ControlMethod;
|
||||
typedef SafeEnum<enum ControlMethod_> ControlMethod;
|
||||
|
||||
protected:
|
||||
rclcpp::Node::SharedPtr nh_;
|
||||
};
|
||||
} // namespace gz_ros2_control
|
||||
}
|
||||
|
|
|
|||
|
|
@ -17,12 +17,13 @@
|
|||
#include <chrono>
|
||||
#include <map>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#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>
|
||||
|
|
@ -30,12 +31,21 @@
|
|||
#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>
|
||||
|
||||
|
|
@ -44,89 +54,15 @@
|
|||
#include "gz_quadruped_hardware/gz_quadruped_plugin.hpp"
|
||||
#include "gz_quadruped_hardware/gz_system.hpp"
|
||||
|
||||
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_;
|
||||
};
|
||||
|
||||
namespace gz_quadruped_hardware
|
||||
{
|
||||
//////////////////////////////////////////////////
|
||||
class GazeboSimQuadrupedPluginPrivate {
|
||||
class GazeboSimQuadrupedPluginPrivate
|
||||
{
|
||||
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
|
||||
|
|
@ -135,8 +71,8 @@ namespace gz_quadruped_hardware {
|
|||
/// \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;
|
||||
const sim::Entity& _entity,
|
||||
sim::EntityComponentManager& _ecm) const;
|
||||
|
||||
/// \brief Entity ID for sensor within Gazebo.
|
||||
sim::Entity entity_;
|
||||
|
|
@ -153,16 +89,27 @@ namespace gz_quadruped_hardware {
|
|||
/// \brief Timing
|
||||
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
|
||||
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(static_cast<int64_t>(0), RCL_ROS_TIME);
|
||||
rclcpp::Time((int64_t)0, RCL_ROS_TIME);
|
||||
|
||||
/// \brief ECM pointer
|
||||
sim::EntityComponentManager *ecm{nullptr};
|
||||
sim::EntityComponentManager* ecm{nullptr};
|
||||
|
||||
/// \brief controller update rate
|
||||
int update_rate;
|
||||
|
|
@ -171,8 +118,9 @@ namespace gz_quadruped_hardware {
|
|||
//////////////////////////////////////////////////
|
||||
std::map<std::string, sim::Entity>
|
||||
GazeboSimQuadrupedPluginPrivate::GetEnabledJoints(
|
||||
const sim::Entity &_entity,
|
||||
sim::EntityComponentManager &_ecm) const {
|
||||
const sim::Entity& _entity,
|
||||
sim::EntityComponentManager& _ecm) const
|
||||
{
|
||||
std::map<std::string, sim::Entity> output;
|
||||
|
||||
std::vector<std::string> enabledJoints;
|
||||
|
|
@ -181,32 +129,37 @@ namespace gz_quadruped_hardware {
|
|||
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) {
|
||||
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: {
|
||||
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: {
|
||||
case sdf::JointType::FIXED:
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
node_->get_logger(),
|
||||
"[gz_quadruped_hardware] 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: {
|
||||
case sdf::JointType::REVOLUTE2:
|
||||
case sdf::JointType::SCREW:
|
||||
case sdf::JointType::BALL:
|
||||
case sdf::JointType::UNIVERSAL:
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
node_->get_logger(),
|
||||
"[gz_quadruped_hardware] Joint [%s] (Entity=%lu)] is of unsupported type."
|
||||
|
|
@ -214,7 +167,8 @@ namespace gz_quadruped_hardware {
|
|||
jointName.c_str(), jointEntity);
|
||||
continue;
|
||||
}
|
||||
default: {
|
||||
default:
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
node_->get_logger(),
|
||||
"[gz_quadruped_hardware] Joint [%s] (Entity=%lu)] is of unknown type",
|
||||
|
|
@ -229,14 +183,81 @@ namespace gz_quadruped_hardware {
|
|||
}
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
GazeboSimQuadrupedPlugin::GazeboSimQuadrupedPlugin()
|
||||
: dataPtr(std::make_unique<GazeboSimQuadrupedPluginPrivate>()) {
|
||||
std::string GazeboSimQuadrupedPluginPrivate::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_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
|
||||
if (!this->dataPtr->controller_manager_) {
|
||||
if (!this->dataPtr->controller_manager_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
this->dataPtr->executor_->remove_node(this->dataPtr->controller_manager_);
|
||||
|
|
@ -246,39 +267,61 @@ namespace gz_quadruped_hardware {
|
|||
|
||||
//////////////////////////////////////////////////
|
||||
void GazeboSimQuadrupedPlugin::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");
|
||||
const sim::Entity& _entity,
|
||||
const std::shared_ptr<const sdf::Element>& _sdf,
|
||||
sim::EntityComponentManager& _ecm,
|
||||
sim::EventManager&)
|
||||
{
|
||||
rclcpp::Logger logger = rclcpp::get_logger("GazeboSimQuadrupedPlugin");
|
||||
// Make sure the controller is attached to a valid model
|
||||
const auto model = sim::Model(_entity);
|
||||
if (!model.Valid(_ecm)) {
|
||||
if (!model.Valid(_ecm))
|
||||
{
|
||||
RCLCPP_ERROR(
|
||||
logger,
|
||||
"[Gazebo ROS 2 Control] 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.",
|
||||
"[gz_quadruped_hardware] Failed to initialize because [%s] (Entity=%lu)] is not a model."
|
||||
"Please make sure that gz_quadruped_hardware is attached to a valid model.",
|
||||
model.Name(_ecm).c_str(), _entity);
|
||||
return;
|
||||
}
|
||||
|
||||
// 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(
|
||||
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;
|
||||
}
|
||||
|
||||
// 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());
|
||||
auto sdfPtr = const_cast<sdf::Element*>(_sdf.get());
|
||||
|
||||
sdf::ElementPtr argument_sdf = sdfPtr->GetElement("parameters");
|
||||
while (argument_sdf) {
|
||||
while (argument_sdf)
|
||||
{
|
||||
std::string argument = argument_sdf->Get<std::string>();
|
||||
arguments.push_back(RCL_PARAM_FILE_FLAG);
|
||||
arguments.push_back(argument);
|
||||
|
|
@ -288,30 +331,39 @@ namespace gz_quadruped_hardware {
|
|||
// Get controller manager node name
|
||||
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>();
|
||||
}
|
||||
|
||||
std::string ns = "/";
|
||||
|
||||
if (sdfPtr->HasElement("ros")) {
|
||||
if (sdfPtr->HasElement("ros"))
|
||||
{
|
||||
sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros");
|
||||
|
||||
// Set namespace if tag is present
|
||||
if (sdfRos->HasElement("namespace")) {
|
||||
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] != '/') {
|
||||
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")) {
|
||||
if (sdfRos->HasElement("remapping"))
|
||||
{
|
||||
sdf::ElementPtr argument_sdf = sdfRos->GetElement("remapping");
|
||||
|
||||
arguments.push_back(RCL_ROS_ARGS_FLAG);
|
||||
while (argument_sdf) {
|
||||
while (argument_sdf)
|
||||
{
|
||||
auto argument = argument_sdf->Get<std::string>();
|
||||
arguments.push_back(RCL_REMAP_FLAG);
|
||||
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
|
||||
if (!rclcpp::ok()) {
|
||||
init(
|
||||
static_cast<int>(argv.size()), argv.data(), rclcpp::InitOptions(),
|
||||
rclcpp::SignalHandlerOptions::None);
|
||||
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_quadruped_control";
|
||||
|
||||
std::string node_name = "gz_quadruped_hardware";
|
||||
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_);
|
||||
|
|
@ -342,8 +390,54 @@ namespace gz_quadruped_hardware {
|
|||
};
|
||||
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(), "[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 << ")].");
|
||||
|
||||
// Get list of enabled joints
|
||||
|
|
@ -351,15 +445,100 @@ namespace gz_quadruped_hardware {
|
|||
_entity,
|
||||
_ecm);
|
||||
|
||||
if (enabledJoints.size() == 0) {
|
||||
if (enabledJoints.size() == 0)
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
RCLCPP_INFO(this->dataPtr->node_->get_logger(), "Loading controller_manager");
|
||||
|
|
@ -374,10 +553,19 @@ namespace gz_quadruped_hardware {
|
|||
std::move(resource_manager_),
|
||||
this->dataPtr->executor_,
|
||||
controllerManagerNodeName,
|
||||
this->dataPtr->node_->get_namespace(), options));
|
||||
this->dataPtr->node_->get_namespace()));
|
||||
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(
|
||||
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
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(
|
||||
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;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////
|
||||
void GazeboSimQuadrupedPlugin::PreUpdate(
|
||||
const sim::UpdateInfo &_info,
|
||||
sim::EntityComponentManager & /*_ecm*/) {
|
||||
if (!this->dataPtr->controller_manager_) {
|
||||
const sim::UpdateInfo& _info,
|
||||
sim::EntityComponentManager& /*_ecm*/)
|
||||
{
|
||||
if (!this->dataPtr->controller_manager_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
static bool warned{false};
|
||||
if (!warned) {
|
||||
if (!warned)
|
||||
{
|
||||
rclcpp::Duration gazebo_period(_info.dt);
|
||||
|
||||
// Check the period against the simulation period
|
||||
if (this->dataPtr->control_period_ < _info.dt) {
|
||||
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) {
|
||||
}
|
||||
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() <<
|
||||
|
|
@ -425,9 +611,9 @@ namespace gz_quadruped_hardware {
|
|||
warned = true;
|
||||
}
|
||||
|
||||
const rclcpp::Time sim_time_ros(std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
_info.simTime).count(), RCL_ROS_TIME);
|
||||
const rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_;
|
||||
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);
|
||||
|
|
@ -435,30 +621,42 @@ namespace gz_quadruped_hardware {
|
|||
|
||||
//////////////////////////////////////////////////
|
||||
void GazeboSimQuadrupedPlugin::PostUpdate(
|
||||
const sim::UpdateInfo &_info,
|
||||
const sim::EntityComponentManager & /*_ecm*/) {
|
||||
if (!this->dataPtr->controller_manager_) {
|
||||
const sim::UpdateInfo& _info,
|
||||
const sim::EntityComponentManager& /*_ecm*/)
|
||||
{
|
||||
if (!this->dataPtr->controller_manager_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
// Get the simulation time and period
|
||||
const rclcpp::Time sim_time_ros(std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
_info.simTime).count(), RCL_ROS_TIME);
|
||||
const rclcpp::Duration sim_period = sim_time_ros - this->dataPtr->last_update_sim_time_ros_;
|
||||
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_) {
|
||||
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<GazeboSimSystemInterface>(
|
||||
this->dataPtr->controller_manager_);
|
||||
std::dynamic_pointer_cast<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_quadruped_hardware
|
||||
|
||||
#ifdef GZ_HEADERS
|
||||
GZ_ADD_PLUGIN(
|
||||
gz_quadruped_hardware::GazeboSimQuadrupedPlugin,
|
||||
gz::sim::System,
|
||||
sim::System,
|
||||
gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemConfigure,
|
||||
gz_quadruped_hardware::GazeboSimQuadrupedPlugin::ISystemPreUpdate,
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -202,7 +202,7 @@ namespace gz_quadruped_hardware
|
|||
std::map<std::string, sim::Entity>& enableJoints,
|
||||
const hardware_interface::HardwareInfo& hardware_info,
|
||||
sim::EntityComponentManager& _ecm,
|
||||
unsigned int update_rate)
|
||||
int& update_rate)
|
||||
{
|
||||
this->dataPtr = std::make_unique<GazeboSimSystemPrivate>();
|
||||
this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time();
|
||||
|
|
@ -272,24 +272,6 @@ namespace gz_quadruped_hardware
|
|||
// Accept this joint and continue configuration
|
||||
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:");
|
||||
|
||||
auto get_initial_value =
|
||||
|
|
@ -536,9 +518,9 @@ namespace gz_quadruped_hardware
|
|||
ftData->name = _name->Data();
|
||||
ftData->sim_ft_sensors_ = _entity;
|
||||
this->dataPtr->state_interfaces_.emplace_back(
|
||||
"foot_force",
|
||||
ftData->name,
|
||||
&ftData->foot_effort);
|
||||
"foot_force",
|
||||
ftData->name,
|
||||
&ftData->foot_effort);
|
||||
this->dataPtr->ft_sensors_.push_back(ftData);
|
||||
return true;
|
||||
});
|
||||
|
|
@ -680,6 +662,60 @@ namespace gz_quadruped_hardware
|
|||
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(
|
||||
const rclcpp::Time& /*time*/,
|
||||
const rclcpp::Duration& /*period*/)
|
||||
|
|
|
|||
Loading…
Reference in New Issue