From 08899075fc95a0ff9d14decbd8f4c1c5dbc17dd7 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 14 Aug 2023 19:33:56 +0300 Subject: [PATCH 1/9] sensor-interface --- ign_ros2_control/CMakeLists.txt | 1 + ign_ros2_control/ign_hardware_plugins.xml | 9 + .../ign_ros2_control/ign_fts_sensor.hpp | 65 +++++++ .../ign_ros2_control/ign_sensor_interface.hpp | 31 +++ ign_ros2_control/src/ign_fts_sensor.cpp | 184 ++++++++++++++++++ 5 files changed, 290 insertions(+) create mode 100644 ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp create mode 100644 ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp create mode 100644 ign_ros2_control/src/ign_fts_sensor.cpp diff --git a/ign_ros2_control/CMakeLists.txt b/ign_ros2_control/CMakeLists.txt index b400ae48..3d48c77f 100644 --- a/ign_ros2_control/CMakeLists.txt +++ b/ign_ros2_control/CMakeLists.txt @@ -72,6 +72,7 @@ ament_target_dependencies(${PROJECT_NAME}-system add_library(ign_hardware_plugins SHARED src/ign_system.cpp + src/ign_fts_sensor.cpp ) ament_target_dependencies(ign_hardware_plugins rclcpp_lifecycle diff --git a/ign_ros2_control/ign_hardware_plugins.xml b/ign_ros2_control/ign_hardware_plugins.xml index 3d245541..250d9ecf 100644 --- a/ign_ros2_control/ign_hardware_plugins.xml +++ b/ign_ros2_control/ign_hardware_plugins.xml @@ -7,4 +7,13 @@ GazeboPositionJoint + + + + GazeboSensorInterface + + diff --git a/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp b/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp new file mode 100644 index 00000000..5096a280 --- /dev/null +++ b/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp @@ -0,0 +1,65 @@ +#ifndef IGN_ROS2_CONTROL__IGN_FTS_INTERFACE_HPP_ +#define IGN_ROS2_CONTROL__IGN_FTS_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include + +#include "ign_ros2_control/ign_sensor_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include +#include +#include + +#include + + + +namespace ign_ros2_control +{ + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + class IgnitionFtsPrivate; + + class IgnitionFts : public IgnitionSensorInterface + { + public: + CallbackReturn on_init(const hardware_interface::HardwareInfo & system_info) + override; + + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) + override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) + override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) + override; + + std::vector export_state_interfaces() + override; + + hardware_interface::return_type read( + const rclcpp::Time & time, + const rclcpp::Duration & period + ) override; + + bool InitSensorInterface( + const hardware_interface::HardwareInfo & hardware_info, + ignition::gazebo::EntityComponentManager & _ecm, + int & update_rate + ) override; + + private: + // void registerSensors( + // const hardware_interface::HardwareInfo & hardware_info + // ); + std::unique_ptr dataPtr; + }; +} // namespace ign_ros2_control + +#endif // IGN_ROS2_CONTROL__IGN_FTS_INTERFACE_HPP_ \ No newline at end of file diff --git a/ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp b/ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp new file mode 100644 index 00000000..650e2f6e --- /dev/null +++ b/ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp @@ -0,0 +1,31 @@ +// #ifndef IGN_ROS2_CONTROL__IGN_SENSOR_INTERFACE_HPP_ +// #define IGN_ROS2_CONTROL__IGN_SENSOR_INTERFACE_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include +#include + + +#include + +namespace ign_ros2_control +{ + class IgnitionSensorInterface + : public hardware_interface::SensorInterface + { + public: + virtual bool InitSensorInterface( + const hardware_interface::HardwareInfo & hardware_info, + ignition::gazebo::EntityComponentManager & _ecm, + int & update_rate) = 0; + protected: + rclcpp::Node::SharedPtr nh_; + }; +} \ No newline at end of file diff --git a/ign_ros2_control/src/ign_fts_sensor.cpp b/ign_ros2_control/src/ign_fts_sensor.cpp new file mode 100644 index 00000000..558642a7 --- /dev/null +++ b/ign_ros2_control/src/ign_fts_sensor.cpp @@ -0,0 +1,184 @@ +#include "ign_ros2_control/ign_fts_sensor.hpp" +#include + + +class FtsData +{ +public: + std::string name{}; + std::string topicName{}; + ignition::gazebo::Entity sim_fts_sensors_ = ignition::gazebo::kNullEntity; + std::array fts_sensor_data_; + void OnFts(const ignition::msgs::Wrench & _msg); +}; +void FtsData::OnFts(const ignition::msgs::Wrench & _msg) +{ + this->fts_sensor_data_[0] = _msg.force().x(); + this->fts_sensor_data_[1] = _msg.force().y(); + this->fts_sensor_data_[2] = _msg.force().z(); + this->fts_sensor_data_[3] = _msg.torque().x(); + this->fts_sensor_data_[4] = _msg.torque().y(); + this->fts_sensor_data_[5] = _msg.torque().z(); +} + +class ign_ros2_control::IgnitionFtsPrivate +{ +public: + IgnitionFtsPrivate() = default; + ~IgnitionFtsPrivate() = default; + std::vector state_interfaces_; + rclcpp::Time last_update_sim_time_ros_; + + std::vector> fts_; + + ignition::gazebo::EntityComponentManager * ecm; + + int * update_rate; + + ignition::transport::Node node; +}; + +namespace ign_ros2_control +{ +bool IgnitionFts::InitSensorInterface( + const hardware_interface::HardwareInfo & hardware_info, + ignition::gazebo::EntityComponentManager & _ecm, + int & update_rate) + { + this->dataPtr = std::make_unique(); + this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); + this->dataPtr->ecm = &_ecm; + this->dataPtr->update_rate = &update_rate; + size_t n_sensors = hardware_info.sensors.size(); + std::vector sensor_components_; + + for (unsigned int j = 0; j < n_sensors; j++) + { + hardware_interface::ComponentInfo component = hardware_info.sensors[j]; + sensor_components_.push_back(component); + } + + this->dataPtr->ecm->Each( + [&](const ignition::gazebo::Entity & _entity, + const ignition::gazebo::components::ForceTorque *, + const ignition::gazebo::components::Name * _name) -> bool + { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data()); + auto sensorTopicComp = this->dataPtr->ecm->Component< + ignition::gazebo::components::SensorTopic>(_entity); + if(sensorTopicComp) + { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data()); + } + + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); + + auto ftsData = std::make_shared(); + + ftsData->name = _name->Data(); + ftsData->sim_fts_sensors_ = _entity; + + hardware_interface::ComponentInfo component; + + for( auto & comp : sensor_components_) + { + if (comp.name == _name->Data()) + { + component = comp; + } + } + + static const std::map interface_name_map = { + {"force.x", 0}, + {"force.y", 1}, + {"force.z", 2}, + {"torque.x", 3}, + {"torque.y", 4}, + {"torque.z", 5}, + }; + + for (const auto & state_interface : component.state_interfaces) + { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t" << state_interface.name); + + size_t data_index = interface_name_map.at(state_interface.name); + this->dataPtr->state_interfaces_.emplace_back( + ftsData->name, + state_interface.name, + &ftsData->fts_sensor_data_[data_index] + ); + } + this->dataPtr->fts_.push_back(ftsData); + return true; + } + ); + } + +hardware_interface::return_type IgnitionFts::read( + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) + { + for (unsigned int i = 0; i < this->dataPtr->fts_.size(); ++i) + { + if (this->dataPtr->fts_[i]->topicName.empty()) + { + auto sensorTopicComp = this->dataPtr->ecm->Component< + ignition::gazebo::components::SensorTopic>(this->dataPtr->fts_[i]->sim_fts_sensors_); + if (sensorTopicComp) + { + this->dataPtr->fts_[i]->topicName = sensorTopicComp->Data(); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "FTS " << this->dataPtr->fts_[i]->name << + " has a topic name: " << sensorTopicComp->Data()); + this->dataPtr->node.Subscribe( + this->dataPtr->fts_[i]->topicName, &FtsData::OnFts, + this->dataPtr->fts_[i].get()); + } + } + } + return hardware_interface::return_type::OK; + } + + +CallbackReturn +IgnitionFts::on_init(const hardware_interface::HardwareInfo & system_info) +{ + RCLCPP_WARN(this->nh_->get_logger(), "On init..."); + if (hardware_interface::SensorInterface::on_init(system_info) != CallbackReturn::SUCCESS) { + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +CallbackReturn IgnitionFts::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + RCLCPP_INFO( + this->nh_->get_logger(), "System Successfully configured!"); + + return CallbackReturn::SUCCESS; +} + +std::vector +IgnitionFts::export_state_interfaces() +{ + return std::move(this->dataPtr->state_interfaces_); +} + +CallbackReturn IgnitionFts::on_activate(const rclcpp_lifecycle::State & previous_state) +{ + return CallbackReturn::SUCCESS; + return hardware_interface::SensorInterface::on_activate(previous_state); +} + +CallbackReturn IgnitionFts::on_deactivate(const rclcpp_lifecycle::State & previous_state) +{ + return CallbackReturn::SUCCESS; + return hardware_interface::SensorInterface::on_deactivate(previous_state); +} +} + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS( + ign_ros2_control::IgnitionFts, ign_ros2_control::IgnitionSensorInterface) From fa15dc77bead5f6c90c03542f015f8970bb8f2fb Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Wed, 16 Aug 2023 15:45:10 +0300 Subject: [PATCH 2/9] add sensor interface to ign_ros2_control plugin --- .../src/ign_ros2_control_plugin.cpp | 59 +++++++++++++++---- 1 file changed, 48 insertions(+), 11 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index a52772e8..826cb6e8 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -39,6 +39,7 @@ #include "ign_ros2_control/ign_ros2_control_plugin.hpp" #include "ign_ros2_control/ign_system.hpp" +#include "ign_ros2_control/ign_sensor_interface.hpp" namespace ign_ros2_control { @@ -78,11 +79,16 @@ class IgnitionROS2ControlPluginPrivate /// \brief Timing rclcpp::Duration control_period_ = rclcpp::Duration(1, 0); - /// \brief Interface loader + /// \brief System interface loader std::shared_ptr> robot_hw_sim_loader_{nullptr}; + /// \brief Sensor interface loader + std::shared_ptr> + robot_sensor_sim_loader_{nullptr}; + /// \brief Controller manager std::shared_ptr controller_manager_{nullptr}; @@ -386,24 +392,55 @@ void IgnitionROS2ControlPlugin::Configure( return; } + try { + this->dataPtr->robot_sensor_sim_loader_.reset( + new pluginlib::ClassLoader( + "ign_ros2_control", + "ign_ros2_control::IgnitionSensorInterface")); + } + catch(pluginlib::LibraryLoadException & ex) { + RCLCPP_ERROR( + this->dataPtr->node_->get_logger(), "Failed to create robot sensor simulation interface loader: %s ", + ex.what()); + } + + 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; - auto ignitionSystem = std::unique_ptr( + + if (control_hardware_info[i].type == "system") + { + auto ignitionSystem = std::unique_ptr( this->dataPtr->robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); - if (!ignitionSystem->initSim( - this->dataPtr->node_, - enabledJoints, + if (!ignitionSystem->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; + } + resource_manager_->import_component(std::move(ignitionSystem), control_hardware_info[i]); + } else if (control_hardware_info[i].type == "sensor") + { + auto ignitionSensor = std::unique_ptr( + this->dataPtr->robot_sensor_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); + + if (!ignitionSensor->InitSensorInterface( control_hardware_info[i], _ecm, this->dataPtr->update_rate)) - { - RCLCPP_FATAL( - this->dataPtr->node_->get_logger(), "Could not initialize robot simulation interface"); - return; + { + RCLCPP_FATAL( + this->dataPtr->node_->get_logger(), "Could not initialize robot sensor simulation interface"); + return; + } } - - resource_manager_->import_component(std::move(ignitionSystem), control_hardware_info[i]); + rclcpp_lifecycle::State state( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, From c776da9f7904c33ef41a6e72db7720c5a0a1afb3 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Thu, 17 Aug 2023 14:03:17 +0300 Subject: [PATCH 3/9] force-torque-sensor class loader complete --- .../ign_ros2_control/ign_fts_sensor.hpp | 25 +++++++++++++---- .../ign_ros2_control/ign_sensor_interface.hpp | 1 + ign_ros2_control/src/ign_fts_sensor.cpp | 28 ++++--------------- .../src/ign_ros2_control_plugin.cpp | 13 +++++---- 4 files changed, 34 insertions(+), 33 deletions(-) diff --git a/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp b/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp index 5096a280..a3f51611 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp +++ b/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp @@ -21,6 +21,24 @@ namespace ign_ros2_control { + class FtsData + { + public: + std::string name{}; + std::string topicName{}; + ignition::gazebo::Entity sim_fts_sensors_ = ignition::gazebo::kNullEntity; + std::array fts_sensor_data_; + void OnFts(const ignition::msgs::Wrench & _msg); + }; + void FtsData::OnFts(const ignition::msgs::Wrench & _msg) + { + this->fts_sensor_data_[0] = _msg.force().x(); + this->fts_sensor_data_[1] = _msg.force().y(); + this->fts_sensor_data_[2] = _msg.force().z(); + this->fts_sensor_data_[3] = _msg.torque().x(); + this->fts_sensor_data_[4] = _msg.torque().y(); + this->fts_sensor_data_[5] = _msg.torque().z(); + } using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; class IgnitionFtsPrivate; @@ -49,15 +67,12 @@ namespace ign_ros2_control ) override; bool InitSensorInterface( + rclcpp::Node::SharedPtr & model_nh, const hardware_interface::HardwareInfo & hardware_info, ignition::gazebo::EntityComponentManager & _ecm, - int & update_rate - ) override; + int & update_rate) override; private: - // void registerSensors( - // const hardware_interface::HardwareInfo & hardware_info - // ); std::unique_ptr dataPtr; }; } // namespace ign_ros2_control diff --git a/ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp b/ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp index 650e2f6e..98b7d867 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp +++ b/ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp @@ -22,6 +22,7 @@ namespace ign_ros2_control { public: virtual bool InitSensorInterface( + rclcpp::Node::SharedPtr & model_nh, const hardware_interface::HardwareInfo & hardware_info, ignition::gazebo::EntityComponentManager & _ecm, int & update_rate) = 0; diff --git a/ign_ros2_control/src/ign_fts_sensor.cpp b/ign_ros2_control/src/ign_fts_sensor.cpp index 558642a7..8155de67 100644 --- a/ign_ros2_control/src/ign_fts_sensor.cpp +++ b/ign_ros2_control/src/ign_fts_sensor.cpp @@ -1,26 +1,6 @@ #include "ign_ros2_control/ign_fts_sensor.hpp" #include - -class FtsData -{ -public: - std::string name{}; - std::string topicName{}; - ignition::gazebo::Entity sim_fts_sensors_ = ignition::gazebo::kNullEntity; - std::array fts_sensor_data_; - void OnFts(const ignition::msgs::Wrench & _msg); -}; -void FtsData::OnFts(const ignition::msgs::Wrench & _msg) -{ - this->fts_sensor_data_[0] = _msg.force().x(); - this->fts_sensor_data_[1] = _msg.force().y(); - this->fts_sensor_data_[2] = _msg.force().z(); - this->fts_sensor_data_[3] = _msg.torque().x(); - this->fts_sensor_data_[4] = _msg.torque().y(); - this->fts_sensor_data_[5] = _msg.torque().z(); -} - class ign_ros2_control::IgnitionFtsPrivate { public: @@ -41,6 +21,7 @@ class ign_ros2_control::IgnitionFtsPrivate namespace ign_ros2_control { bool IgnitionFts::InitSensorInterface( + rclcpp::Node::SharedPtr & model_nh, const hardware_interface::HardwareInfo & hardware_info, ignition::gazebo::EntityComponentManager & _ecm, int & update_rate) @@ -49,6 +30,7 @@ bool IgnitionFts::InitSensorInterface( this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); this->dataPtr->ecm = &_ecm; this->dataPtr->update_rate = &update_rate; + this->nh_ = model_nh; size_t n_sensors = hardware_info.sensors.size(); std::vector sensor_components_; @@ -58,6 +40,8 @@ bool IgnitionFts::InitSensorInterface( sensor_components_.push_back(component); } + RCLCPP_WARN(this->nh_->get_logger(), "Found number of sensors: %ld", sensor_components_.size()); + this->dataPtr->ecm->Each( [&](const ignition::gazebo::Entity & _entity, @@ -111,8 +95,8 @@ bool IgnitionFts::InitSensorInterface( } this->dataPtr->fts_.push_back(ftsData); return true; - } - ); + }); + return true; } hardware_interface::return_type IgnitionFts::read( diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 826cb6e8..42af762c 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -420,17 +420,18 @@ void IgnitionROS2ControlPlugin::Configure( _ecm, this->dataPtr->update_rate)) { - RCLCPP_FATAL( - this->dataPtr->node_->get_logger(), "Could not initialize robot simulation interface"); - return; + RCLCPP_FATAL( + this->dataPtr->node_->get_logger(), "Could not initialize robot system simulation interface"); + return; } resource_manager_->import_component(std::move(ignitionSystem), control_hardware_info[i]); - } else if (control_hardware_info[i].type == "sensor") + } + else if (control_hardware_info[i].type == "sensor") { auto ignitionSensor = std::unique_ptr( this->dataPtr->robot_sensor_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); - if (!ignitionSensor->InitSensorInterface( + this->dataPtr->node_, control_hardware_info[i], _ecm, this->dataPtr->update_rate)) @@ -439,8 +440,8 @@ void IgnitionROS2ControlPlugin::Configure( this->dataPtr->node_->get_logger(), "Could not initialize robot sensor simulation interface"); return; } + resource_manager_->import_component(std::move(ignitionSensor), control_hardware_info[i]); } - rclcpp_lifecycle::State state( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, From 1bbb58495441db83b6c0ff4317a6aabf77cfdf09 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Thu, 17 Aug 2023 14:12:54 +0300 Subject: [PATCH 4/9] clear extra debug info --- ign_ros2_control/src/ign_fts_sensor.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/ign_ros2_control/src/ign_fts_sensor.cpp b/ign_ros2_control/src/ign_fts_sensor.cpp index 8155de67..247dec69 100644 --- a/ign_ros2_control/src/ign_fts_sensor.cpp +++ b/ign_ros2_control/src/ign_fts_sensor.cpp @@ -39,9 +39,6 @@ bool IgnitionFts::InitSensorInterface( hardware_interface::ComponentInfo component = hardware_info.sensors[j]; sensor_components_.push_back(component); } - - RCLCPP_WARN(this->nh_->get_logger(), "Found number of sensors: %ld", sensor_components_.size()); - this->dataPtr->ecm->Each( [&](const ignition::gazebo::Entity & _entity, From e158961f740e85c478352559a51fd32ace581bb4 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Tue, 22 Aug 2023 10:22:07 +0300 Subject: [PATCH 5/9] add comments --- .../include/ign_ros2_control/ign_fts_sensor.hpp | 9 ++++++++- .../ign_ros2_control/ign_sensor_interface.hpp | 13 ++++++++++--- ign_ros2_control/src/ign_ros2_control_plugin.cpp | 5 +++-- 3 files changed, 21 insertions(+), 6 deletions(-) diff --git a/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp b/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp index a3f51611..7d56e387 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp +++ b/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp @@ -24,10 +24,15 @@ namespace ign_ros2_control class FtsData { public: + /// \brief fts name. std::string name{}; + /// \brief fts topic name. std::string topicName{}; + /// \brief handles to the fts from within Gazebo ignition::gazebo::Entity sim_fts_sensors_ = ignition::gazebo::kNullEntity; + /// @brief Force Torque Sensor data storage std::array fts_sensor_data_; + /// \brief callback to get the FTS topic values void OnFts(const ignition::msgs::Wrench & _msg); }; void FtsData::OnFts(const ignition::msgs::Wrench & _msg) @@ -39,8 +44,9 @@ namespace ign_ros2_control this->fts_sensor_data_[4] = _msg.torque().y(); this->fts_sensor_data_[5] = _msg.torque().z(); } + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; - + // Forward declaration class IgnitionFtsPrivate; class IgnitionFts : public IgnitionSensorInterface @@ -73,6 +79,7 @@ namespace ign_ros2_control int & update_rate) override; private: + /// \brief Private data pointer. std::unique_ptr dataPtr; }; } // namespace ign_ros2_control diff --git a/ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp b/ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp index 98b7d867..18a3ce5d 100644 --- a/ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp +++ b/ign_ros2_control/include/ign_ros2_control/ign_sensor_interface.hpp @@ -1,5 +1,5 @@ -// #ifndef IGN_ROS2_CONTROL__IGN_SENSOR_INTERFACE_HPP_ -// #define IGN_ROS2_CONTROL__IGN_SENSOR_INTERFACE_HPP_ +#ifndef IGN_ROS2_CONTROL__IGN_SENSOR_INTERFACE_HPP_ +#define IGN_ROS2_CONTROL__IGN_SENSOR_INTERFACE_HPP_ #include #include @@ -21,6 +21,11 @@ namespace ign_ros2_control : public hardware_interface::SensorInterface { public: + /// \brief Initialize the sensor interface + /// param[in] model_nh Pointer to the ros2 node + /// param[in] hardware_info structure with data from URDF. + /// param[in] _ecm Entity-component manager. + /// param[in] update_rate controller update rate virtual bool InitSensorInterface( rclcpp::Node::SharedPtr & model_nh, const hardware_interface::HardwareInfo & hardware_info, @@ -29,4 +34,6 @@ namespace ign_ros2_control protected: rclcpp::Node::SharedPtr nh_; }; -} \ No newline at end of file +} // namespace ign_ros2_control + +#endif // IGN_ROS2_CONTROL__IGN_SENSOR_INTERFACE_HPP_ \ No newline at end of file diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 42af762c..c45847e7 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -387,7 +387,7 @@ void IgnitionROS2ControlPlugin::Configure( "ign_ros2_control::IgnitionSystemInterface")); } catch (pluginlib::LibraryLoadException & ex) { RCLCPP_ERROR( - this->dataPtr->node_->get_logger(), "Failed to create robot simulation interface loader: %s ", + this->dataPtr->node_->get_logger(), "Failed to create robot system simulation interface loader: %s ", ex.what()); return; } @@ -407,7 +407,7 @@ void IgnitionROS2ControlPlugin::Configure( 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; - + // Load system interface if (control_hardware_info[i].type == "system") { auto ignitionSystem = std::unique_ptr( @@ -426,6 +426,7 @@ void IgnitionROS2ControlPlugin::Configure( } resource_manager_->import_component(std::move(ignitionSystem), control_hardware_info[i]); } + // Load sensor interface else if (control_hardware_info[i].type == "sensor") { auto ignitionSensor = std::unique_ptr( From cbf27fde3245d0ff5407799266432fb9e27e9994 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Wed, 23 Aug 2023 15:56:13 +0300 Subject: [PATCH 6/9] update log info --- ign_ros2_control/src/ign_fts_sensor.cpp | 38 ++++++++++++------------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/ign_ros2_control/src/ign_fts_sensor.cpp b/ign_ros2_control/src/ign_fts_sensor.cpp index 247dec69..425b5349 100644 --- a/ign_ros2_control/src/ign_fts_sensor.cpp +++ b/ign_ros2_control/src/ign_fts_sensor.cpp @@ -97,29 +97,29 @@ bool IgnitionFts::InitSensorInterface( } hardware_interface::return_type IgnitionFts::read( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, + const rclcpp::Duration & /*period*/) + { + for (unsigned int i = 0; i < this->dataPtr->fts_.size(); ++i) { - for (unsigned int i = 0; i < this->dataPtr->fts_.size(); ++i) + if (this->dataPtr->fts_[i]->topicName.empty()) + { + auto sensorTopicComp = this->dataPtr->ecm->Component< + ignition::gazebo::components::SensorTopic>(this->dataPtr->fts_[i]->sim_fts_sensors_); + if (sensorTopicComp) { - if (this->dataPtr->fts_[i]->topicName.empty()) - { - auto sensorTopicComp = this->dataPtr->ecm->Component< - ignition::gazebo::components::SensorTopic>(this->dataPtr->fts_[i]->sim_fts_sensors_); - if (sensorTopicComp) - { - this->dataPtr->fts_[i]->topicName = sensorTopicComp->Data(); - RCLCPP_INFO_STREAM( - this->nh_->get_logger(), "FTS " << this->dataPtr->fts_[i]->name << - " has a topic name: " << sensorTopicComp->Data()); - this->dataPtr->node.Subscribe( - this->dataPtr->fts_[i]->topicName, &FtsData::OnFts, - this->dataPtr->fts_[i].get()); - } - } + this->dataPtr->fts_[i]->topicName = sensorTopicComp->Data(); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "FTS with name: " << this->dataPtr->fts_[i]->name << + " has a topic name: " << sensorTopicComp->Data()); + this->dataPtr->node.Subscribe( + this->dataPtr->fts_[i]->topicName, &FtsData::OnFts, + this->dataPtr->fts_[i].get()); } - return hardware_interface::return_type::OK; + } } + return hardware_interface::return_type::OK; + } CallbackReturn From 8a3c8a782e2f6b7abae06f40f3b2542064451d66 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Thu, 24 Aug 2023 12:13:54 +0300 Subject: [PATCH 7/9] change filenames --- ign_ros2_control/CMakeLists.txt | 2 +- .../ign_ros2_control/{ign_fts_sensor.hpp => ign_ft_sensor.hpp} | 0 ign_ros2_control/src/{ign_fts_sensor.cpp => ign_ft_sensor.cpp} | 0 3 files changed, 1 insertion(+), 1 deletion(-) rename ign_ros2_control/include/ign_ros2_control/{ign_fts_sensor.hpp => ign_ft_sensor.hpp} (100%) rename ign_ros2_control/src/{ign_fts_sensor.cpp => ign_ft_sensor.cpp} (100%) diff --git a/ign_ros2_control/CMakeLists.txt b/ign_ros2_control/CMakeLists.txt index 3d48c77f..cc82df79 100644 --- a/ign_ros2_control/CMakeLists.txt +++ b/ign_ros2_control/CMakeLists.txt @@ -72,7 +72,7 @@ ament_target_dependencies(${PROJECT_NAME}-system add_library(ign_hardware_plugins SHARED src/ign_system.cpp - src/ign_fts_sensor.cpp + src/ign_ft_sensor.cpp ) ament_target_dependencies(ign_hardware_plugins rclcpp_lifecycle diff --git a/ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp b/ign_ros2_control/include/ign_ros2_control/ign_ft_sensor.hpp similarity index 100% rename from ign_ros2_control/include/ign_ros2_control/ign_fts_sensor.hpp rename to ign_ros2_control/include/ign_ros2_control/ign_ft_sensor.hpp diff --git a/ign_ros2_control/src/ign_fts_sensor.cpp b/ign_ros2_control/src/ign_ft_sensor.cpp similarity index 100% rename from ign_ros2_control/src/ign_fts_sensor.cpp rename to ign_ros2_control/src/ign_ft_sensor.cpp From ecedb107c794902ef08053e97731067ebff9d33b Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Thu, 24 Aug 2023 12:17:19 +0300 Subject: [PATCH 8/9] rename header --- ign_ros2_control/src/ign_ft_sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ign_ros2_control/src/ign_ft_sensor.cpp b/ign_ros2_control/src/ign_ft_sensor.cpp index 425b5349..7670f6d0 100644 --- a/ign_ros2_control/src/ign_ft_sensor.cpp +++ b/ign_ros2_control/src/ign_ft_sensor.cpp @@ -1,4 +1,4 @@ -#include "ign_ros2_control/ign_fts_sensor.hpp" +#include "ign_ros2_control/ign_ft_sensor.hpp" #include class ign_ros2_control::IgnitionFtsPrivate From 407d88f57b0ce4583c5e457a29e202ad7260eb3f Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Thu, 24 Aug 2023 13:04:44 +0300 Subject: [PATCH 9/9] fix conflicts after merge --- .../src/ign_ros2_control_plugin.cpp | 85 +++++++++++++------ 1 file changed, 58 insertions(+), 27 deletions(-) diff --git a/ign_ros2_control/src/ign_ros2_control_plugin.cpp b/ign_ros2_control/src/ign_ros2_control_plugin.cpp index 315230ca..cbc241a8 100644 --- a/ign_ros2_control/src/ign_ros2_control_plugin.cpp +++ b/ign_ros2_control/src/ign_ros2_control_plugin.cpp @@ -87,7 +87,7 @@ class IgnitionROS2ControlPluginPrivate /// \brief Sensor interface loader std::shared_ptr> - robot_sensor_sim_loader_{nullptr}; + robot_hw_sensor_sim_loader_{nullptr}; /// \brief Controller manager std::shared_ptr @@ -393,7 +393,7 @@ void IgnitionROS2ControlPlugin::Configure( } try { - this->dataPtr->robot_sensor_sim_loader_.reset( + this->dataPtr->robot_hw_sensor_sim_loader_.reset( new pluginlib::ClassLoader( "ign_ros2_control", "ign_ros2_control::IgnitionSensorInterface")); @@ -407,37 +407,68 @@ void IgnitionROS2ControlPlugin::Configure( 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 ignitionSystem; RCLCPP_DEBUG( this->dataPtr->node_->get_logger(), "Load hardware interface %s ...", robot_hw_sim_type_str_.c_str()); + if (control_hardware_info[i].type == "system") + { + std::unique_ptr ignitionSystem; + try { + ignitionSystem = std::unique_ptr( + 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 (!ignitionSystem->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 system simulation interface"); + return; + } + RCLCPP_DEBUG( + this->dataPtr->node_->get_logger(), "Initialized robot system simulation interface %s!", + robot_hw_sim_type_str_.c_str()); - try { - ignitionSystem = std::unique_ptr( - 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 (!ignitionSystem->initSim( - this->dataPtr->node_, - enabledJoints, - control_hardware_info[i], - _ecm, - this->dataPtr->update_rate)) + resource_manager_->import_component(std::move(ignitionSystem), control_hardware_info[i]); + } else if (control_hardware_info[i].type == "sensor") { - 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()); + std::unique_ptr ignitionSensor; + try { + ignitionSensor = std::unique_ptr( + this->dataPtr->robot_hw_sensor_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 (!ignitionSensor->InitSensorInterface( + this->dataPtr->node_, + control_hardware_info[i], + _ecm, + this->dataPtr->update_rate)) + { + RCLCPP_FATAL( + this->dataPtr->node_->get_logger(), "Could not initialize robot sensor simulation interface"); + return; + } + RCLCPP_DEBUG( + this->dataPtr->node_->get_logger(), "Initialized robot sensor simulation interface %s!", + robot_hw_sim_type_str_.c_str()); - resource_manager_->import_component(std::move(ignitionSystem), control_hardware_info[i]); + resource_manager_->import_component(std::move(ignitionSensor), control_hardware_info[i]); + } + rclcpp_lifecycle::State state( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,