fully working example

Author: macmacal

robotiq_hande_driver/hardware/include/robotiq_hande_driver/hande_hardware_interface.hpp

@@ -10,7 +10,7 @@ namespace robotiq_hande_driver {
 namespace HWI = hardware_interface;
 namespace rlccp_lc = rclcpp_lifecycle;
 
-constexpr int HANDE_LEFT_FINGER_JOINT_ID = 0;
+constexpr int LEFT_FINGER_JOINT_ID = 0;
 
 class RobotiqHandeHardwareInterface : public HWI::SystemInterface
 {
@@ -32,25 +32,15 @@ class RobotiqHandeHardwareInterface : public HWI::SystemInterface
     HWI::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override;
     HWI::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
-    /// Get the logger of the SystemInterface.
-    /**
-     * \return logger of the SystemInterface.
-     */
     rclcpp::Logger get_logger() const { return *logger_; }
 
-    /// Get the clock of the SystemInterface.
-    /**
-     * \return clock of the SystemInterface.
-     */
-    rclcpp::Clock::SharedPtr get_clock() const { return clock_; }
-
 private:
     //TODO: composition of the modbus communication
     std::shared_ptr<rclcpp::Logger> logger_;
-    rclcpp::Clock::SharedPtr clock_; //TODO: Remove after tests
 
     std::string tty_port_;
     double state_position_;
+    double state_velocity_;
     double cmd_position_;
 };
 

robotiq_hande_driver/hardware/src/hande_hardware_interface.cpp

@@ -13,13 +13,13 @@ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_init(const HWI::HardwareIn
         return HWI::CallbackReturn::ERROR;
     }
 
-    state_position_ = 0.0025;
-    cmd_position_ = 0.0025;
+    state_position_ = 0.025;
+    state_velocity_ = 0.0;
+    cmd_position_ = 0.025;
     tty_port_ = info_.hardware_parameters["tty"];
 
     logger_ = std::make_shared<rclcpp::Logger>(
     rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.RobotiqHandeHardwareInterface"));
-    clock_ = std::make_shared<rclcpp::Clock>(rclcpp::Clock());
 
     //TODO: Set parameters for the modbus communication
 
@@ -43,7 +43,9 @@ std::vector<HWI::StateInterface> RobotiqHandeHardwareInterface::export_state_int
     std::vector<HWI::StateInterface> state_interfaces;
 
     state_interfaces.emplace_back(hardware_interface::StateInterface(
-        info_.joints[HANDE_LEFT_FINGER_JOINT_ID].name, hardware_interface::HW_IF_POSITION, &state_position_));
+        info_.joints[LEFT_FINGER_JOINT_ID].name, hardware_interface::HW_IF_POSITION, &state_position_));
+    state_interfaces.emplace_back(hardware_interface::StateInterface(
+        info_.joints[LEFT_FINGER_JOINT_ID].name, hardware_interface::HW_IF_VELOCITY, &state_velocity_));
 
     RCLCPP_INFO(get_logger(), "export_state_interfaces()");
     return state_interfaces;
@@ -53,7 +55,7 @@ std::vector<HWI::CommandInterface> RobotiqHandeHardwareInterface::export_command
     std::vector<hardware_interface::CommandInterface> command_interfaces;
 
     command_interfaces.emplace_back(hardware_interface::CommandInterface(
-        info_.joints[HANDE_LEFT_FINGER_JOINT_ID].name, hardware_interface::HW_IF_POSITION, &cmd_position_));
+        info_.joints[LEFT_FINGER_JOINT_ID].name, hardware_interface::HW_IF_POSITION, &cmd_position_));
 
     RCLCPP_INFO(get_logger(), "export_command_interfaces()");
     return command_interfaces;
@@ -85,30 +87,12 @@ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_error(const rlccp_lc::Stat
 
 HWI::return_type RobotiqHandeHardwareInterface::read(const rclcpp::Time& /*time*/,
                                                      const rclcpp::Duration& /*period*/) {
-    std::stringstream ss;
-    ss << "Reading states:";
-
-    //TODO: auto data = driver_->receive_data();
-    auto data = true;
-    if (data) {
-        state_position_ = 0.015;
-    }
-
-    ss << std::fixed << std::setprecision(2) << std::endl
-       << "\t Position: " << state_position_ << " m for joint '" << info_.joints[HANDE_LEFT_FINGER_JOINT_ID].name << "'";
-    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str());
+    //TODO: data = driver_->receive_data();
     return hardware_interface::return_type::OK;
 }
 HWI::return_type RobotiqHandeHardwareInterface::write(const rclcpp::Time& /*time*/,
                                                       const rclcpp::Duration& /*period*/) {
-    std::stringstream ss;
-    ss << "Writing commands:";
-
     //TODO: driver_->send_data(cmd_position_);
-    ss << std::fixed << std::setprecision(2) << std::endl
-        << "\t Target position:" << cmd_position_ << " m for joint '" << info_.joints[HANDE_LEFT_FINGER_JOINT_ID].name << "'";
-
-    RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 500, "%s", ss.str().c_str());
     return hardware_interface::return_type::OK;
 }