Forward Controller working

But its not using the Gap as Control Value but the angles

src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt

@@ -15,7 +15,7 @@ find_package(realtime_tools REQUIRED)
 
 include_directories(include)
 
-add_library(${PROJECT_NAME}
+add_library(robotiq_2f_controllers SHARED
   src/robotiq_forward_controller.cpp
 )
 ament_target_dependencies(${PROJECT_NAME}

src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_forward_controller.hpp

@@ -68,6 +68,7 @@ public:
 protected:
     std::string joint_name_;
     std::vector<std::string> command_interface_types_;
+    std::vector<std::string> state_interface_types_;
 
     // communication interface
     bool new_msg_ = false;

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller.cpp

@@ -11,23 +11,43 @@ controller_interface::CallbackReturn RobotiqForwardController::on_init()
 {
     joint_name_ = auto_declare<std::string>("joint", joint_name_);
     command_interface_types_ =
-      auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
+    auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
+    state_interface_types_ =
+    auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
+
+
+    if (joint_name_.empty()) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint_name' parameter.");
+        return CallbackReturn::ERROR;
+    }
+    else{
+        RCLCPP_INFO(get_node()->get_logger(), "Joint name: %s.", joint_name_.c_str());
+    }
 
     return CallbackReturn::SUCCESS;
 }
 
-controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration()
+controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration() const
-  const
 {
-    controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
+    RCLCPP_DEBUG(get_node()->get_logger(), "Starting configuration of command interfaces.");
 
-    conf.names.reserve(command_interface_types_.size());
+    controller_interface::InterfaceConfiguration config;
-    for (const auto & interface_type : command_interface_types_)
+    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
-    {
+
-        conf.names.push_back(joint_name_ + "/" + interface_type);
+    for (const auto& interface_map_entry : command_interface_map_) {
+        const std::string& interface_type = interface_map_entry.first;
+        config.names.push_back(joint_name_ + "/" + interface_type);
     }
+    RCLCPP_DEBUG(get_node()->get_logger(), "Completed configuration of command interfaces.");
 
-    return conf;
+    return config;
+}
+
+
+controller_interface::InterfaceConfiguration RobotiqForwardController::state_interface_configuration() const {
+    controller_interface::InterfaceConfiguration config;
+    config.type = controller_interface::interface_configuration_type::NONE;  // Or whatever is appropriate
+    return config;
 }
 
 controller_interface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State &)
@@ -47,18 +67,23 @@ controller_interface::CallbackReturn RobotiqForwardController::on_configure(cons
 
 controller_interface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State &)
 {
-  // clear out vectors in case of restart
+    RCLCPP_INFO(get_node()->get_logger(), "Available command interfaces:");
-  joint_position_command_interface_.clear();
+    for (const auto& interface : command_interfaces_) {
-  max_velocity_command_interface_.clear();
+        RCLCPP_INFO(get_node()->get_logger(), "Interface Name: %s, Interface Type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str());
-  max_effort_command_interface_.clear();
+    }
 
-  // assign command interfaces
+    // clear out vectors in case of restart
-  for (auto & interface : command_interfaces_)
+    joint_position_command_interface_.clear();
-  {
+    max_velocity_command_interface_.clear();
-    command_interface_map_[interface.get_interface_name()]->push_back(interface);
+    max_effort_command_interface_.clear();
-  }
 
-  return CallbackReturn::SUCCESS;
+    // assign command interfaces
+    for (auto & interface : command_interfaces_)
+    {
+        command_interface_map_[interface.get_interface_name()]->push_back(interface);
+    }
+
+    return CallbackReturn::SUCCESS;
 }
 
 controller_interface::return_type RobotiqForwardController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
@@ -71,6 +96,11 @@ controller_interface::return_type RobotiqForwardController::update(const rclcpp:
         desired_max_effort_ = command_msg_->max_effort;
     }
     // Set the hardware interface commands
+    if (joint_position_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "Command interface is not available.");
+        return controller_interface::return_type::ERROR;
+    }
+
     joint_position_command_interface_[0].get().set_value(desired_position_);
     max_velocity_command_interface_[0].get().set_value(desired_max_velocity_);
     max_effort_command_interface_[0].get().set_value(desired_max_effort_);
@@ -100,4 +130,8 @@ controller_interface::CallbackReturn RobotiqForwardController::on_shutdown(const
 {
   return CallbackReturn::SUCCESS;
 }
-}
+}
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqForwardController, controller_interface::ControllerInterface)
+

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_forward_controller_old.cpp

@@ -1,82 +0,0 @@
-#include "robotiq_2f_controllers/robotiq_forward_controller.hpp"
-#include <pluginlib/class_list_macros.hpp>
-
-namespace robotiq_2f_controllers
-{
-RobotiqForwardController::RobotiqForwardController()
-{
-
-}
-
-controller_interface::InterfaceConfiguration RobotiqForwardController::command_interface_configuration() const
-{
-    controller_interface::InterfaceConfiguration config;
-    config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
-    config.names.emplace_back(joint_name_ + hardware_interface::HW_IF_POSITION);
-    config.names.emplace_back(joint_name_ + "/set_gripper_max_effort");
-    config.names.emplace_back(joint_name_ + "/set_gripper_max_velocity");
-    return config;
-}
-
-controller_interface::InterfaceConfiguration RobotiqForwardController::state_interface_configuration() const
-{
-    controller_interface::InterfaceConfiguration config;
-    config.type = controller_interface::interface_configuration_type::NONE;
-    return config;
-}
-
-void RobotiqForwardController::command_callback(const robotiq_2f_msgs::msg::ForwardCommand::SharedPtr msg)
-{
-    desired_position_ = msg->position;
-    desired_max_velocity_ = msg->max_velocity;
-    desired_max_effort_ = msg->max_effort;
-}
-
-controller_interface::return_type RobotiqForwardController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
-{
-    // Set the hardware interface commands
-    position_command_.set_value(desired_position_);
-    max_effort_command_.set_value(desired_max_effort_);
-    max_velocity_command_.set_value(desired_max_velocity_);
-
-    return controller_interface::return_type::OK;
-}
-
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_init()
-{
-    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
-}
-
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
-{
-    joint_name_ = get_node()->declare_parameter<std::string>("joint_name", "finger_joint");
-
-    auto command_interfaces = get_node()->get_node_base_interface();
-    auto position_handle = robot_hardware.get_handle(joint_name_ + hardware_interface::HW_IF_POSITION);
-
-
-    position_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_[0].);
-    max_effort_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_[0].);
-    max_velocity_command_ = hardware_interface::LoanedCommandInterface(command_interfaces_(joint_name_ + "/set_gripper_max_velocity"));
-
-
-    command_subscriber_ = get_node()->create_subscription<robotiq_2f_msgs::msg::ForwardCommand>(
-        "~/command", 10, std::bind(&RobotiqForwardController::command_callback, this, std::placeholders::_1));
-    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
-}
-
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/)
-{
-    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
-}
-
-rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqForwardController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/)
-{
-    for (size_t i = 0; i < command_interfaces_.size(); ++i) {
-        command_interfaces_[i].set_value(std::numeric_limits<double>::quiet_NaN());
-    }
-    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
-}
-}  // namespace robotiq_2f_controllers
-
-PLUGINLIB_EXPORT_CLASS(robotiq_2f_controllers::RobotiqForwardController, controller_interface::ControllerInterface)

src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml

@@ -1,7 +1,7 @@
 robotiq_2f_gripper:
   # Physical limits
-  min_position: 0.0     # Meters (fully closed)
+  min_position: 0.0     # radiant (fully open)
-  max_position: 0.14    # Meters (fully open)
+  max_position: 0.695    # radiant (fully open)
   min_speed: 0.02       # Meters per second
   max_speed: 0.15       # Meters per second
   min_force: 20.0       # Newtons

src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml

@@ -8,7 +8,7 @@ controller_manager:
     fake_gripper_controller:
       type: joint_trajectory_controller/JointTrajectoryController
     forward_gripper_position_controller:
-      type: position_controllers/JointGroupPositionController
+      type: robotiq_2f_controllers/RobotiqForwardController
     robotiq_activation_controller:
       type: robotiq_controllers/RobotiqActivationController
 
@@ -21,8 +21,7 @@ robotiq_gripper_controller:
 
 forward_gripper_position_controller:
   ros__parameters:
-    joints:
+    joint: finger_joint
-      - finger_joint
 
 fake_gripper_controller:
   ros__parameters:

src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py

@@ -122,9 +122,9 @@ def generate_launch_description():
         controller_spawner_inactive_names = ["forward_gripper_position_controller",
                                              "fake_gripper_controller"]
     else:
-        controller_spawner_names = ["fake_gripper_controller"]
+        controller_spawner_names = ["forward_gripper_position_controller"]
-        controller_spawner_inactive_names = ["forward_gripper_position_controller",
+        controller_spawner_inactive_names = ["robotiq_gripper_controller",
-                                             "robotiq_gripper_controller"]
+                                             "fake_gripper_controller"]
 
     controller_spawners = [controller_spawner(name) for name in controller_spawner_names] + [
         controller_spawner(name, active=False) for name in controller_spawner_inactive_names

src/robotiq_2f/robotiq_2f_description/package.xml

@@ -17,9 +17,12 @@
   <exec_depend>urdf</exec_depend>
   <exec_depend>xacro</exec_depend>
 
+  <exec_depend>robotiq_2f_msgs</exec_depend>
+
   <exec_depend>ros2_control</exec_depend>
   <exec_depend>ros2_controllers</exec_depend>
 
+  <exec_depend>robotiq_2f_controllers</exec_depend>
   <exec_depend>gripper_controllers</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
 

src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp

@@ -311,9 +311,9 @@ std::vector<hardware_interface::CommandInterface> RobotiqGripperHardwareInterfac
     gripper_force_ = gripper_force_multiplier;
 
     command_interfaces.emplace_back(
-        hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_velocity", &gripper_speed_));
+        hardware_interface::CommandInterface(info_.joints[0].name, "max_velocity", &gripper_speed_));
     command_interfaces.emplace_back(
-        hardware_interface::CommandInterface(info_.joints[0].name, "set_gripper_max_effort", &gripper_force_));
+        hardware_interface::CommandInterface(info_.joints[0].name, "max_effort", &gripper_force_));
     command_interfaces.emplace_back(
         hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_));
     command_interfaces.emplace_back(