Robotiq Gripper Command tested on sim gripper

adapted the launch file to work properly

src/robotiq_2f/README.md

@@ -0,0 +1,60 @@
+**robotiq_2f Repository** 
+
+***Overview***
+
+This repository provides a ROS2 integration for controlling the Robotiq 2F-85 and 2F-140 grippers. It combines elements from:
+
+* **ros2_robotiq_gripper:** [https://github.com/PickNikRobotics/ros2_robotiq_gripper/tree/main](https://github.com/PickNikRobotics/ros2_robotiq_gripper/tree/main)
+* **robotiq_2f_urcap_adapter:** [https://github.com/fzi-forschungszentrum-informatik/robotiq_2f_urcap_adapter/tree/main](https://github.com/fzi-forschungszentrum-informatik/robotiq_2f_urcap_adapter/tree/main)
+
+It leverages the low-level socket communication provided by the robotiq_2f_urcap_adapter to create a new ROS2-compatible hardware interface.
+
+***Features***
+
+* **robotiq_2f_controllers:**
+    * ForwardController
+    * GripperCommand
+    * RobotiqActivationController (inherited by ros2_robotiq_gripper) 
+* **robotiq_2f_description:**
+    * Configuration files for 85mm and 140mm grippers
+    * Controller configuration files
+    * Launch files for RViz visualization and controller activation
+    * Meshes for visualization and collision detection
+    * Modified URDF files
+* **robotiq_2f_interface:**
+    * ROS2 hardware interface 
+    * Robotiq2fSocketAdapter 
+    * MockRobotiq2fSocketAdapter
+* **robotiq_2f_msgs:**
+    * GripperCommand.action
+    * ForwardCommand.msg
+
+***Socket Communication***
+
+Socket communication details: [https://dof.robotiq.com/discussion/2420/control-robotiq-gripper-mounted-on-ur-robot-via-socket-communication-python](https://dof.robotiq.com/discussion/2420/control-robotiq-gripper-mounted-on-ur-robot-via-socket-communication-python)
+
+***Robotiq Resources***
+
+* **Robotiq UR Cap:** [https://robotiq.com/products/2f85-140-adaptive-robot-gripper?ref=nav_product_new_button](https://robotiq.com/products/2f85-140-adaptive-robot-gripper?ref=nav_product_new_button)
+* **Quick Start Guide:** [https://blog.robotiq.com/hubfs/Support%20Documents/QSG/Quick_start_2Finger_e-Series_nocropmarks_EN.pdf](https://blog.robotiq.com/hubfs/Support%20Documents/QSG/Quick_start_2Finger_e-Series_nocropmarks_EN.pdf)
+
+***Parameters***
+
+* `robot_ip`: IP address of the robot controller
+* `robot_port`: Port for robot controller communication
+* `use_fake_hardware`: Start gripper with fake hardware mirroring command to its states.
+* `use_forward_controller`: Use forward controller instead of gripper command controller(action controller).
+* `prefix`: In case multiple grippers are used on the same UR Cap.
+* `description_file`: The type of the Gripper (85/140) is decided here.
+
+**robotiq_2f_gripper (config file)**
+
+* `min_position`: 0.0 (fully closed)
+* `max_position`: 0.14 (fully open)
+* `max_angle`: 0.695 (radians, closed)
+* `min_speed`: 0.02 
+* `max_speed`: 0.15 
+* `min_force`: 20.0 
+* `max_force`: 235.0 
+
+

src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt

@@ -12,12 +12,14 @@ find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(robotiq_2f_msgs REQUIRED)
 find_package(realtime_tools REQUIRED)
+find_package(std_srvs REQUIRED)
 
 include_directories(include)
 
 add_library(robotiq_2f_controllers SHARED
   src/robotiq_forward_controller.cpp
   src/robotiq_gripper_command_controller.cpp
+  src/robotiq_activation_controller.cpp
 )
 ament_target_dependencies(${PROJECT_NAME}
   rclcpp
@@ -26,6 +28,7 @@ ament_target_dependencies(${PROJECT_NAME}
   pluginlib
   robotiq_2f_msgs
   realtime_tools
+  std_srvs
 )
 pluginlib_export_plugin_description_file(controller_interface controller_plugin.xml)
 

src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml

@@ -9,4 +9,9 @@
             Gripper Command controller for Robotiq 2F Gripper.
         </description>
     </class>
+    <class name="robotiq_2f_controllers/RobotiqActivationController" type="robotiq_controllers::RobotiqActivationController" base_class_type="controller_interface::ControllerInterface">
+        <description>
+            This controller provides an interface to (re-)activate the Robotiq gripper.
+        </description>
+    </class>
 </library>

src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp

@@ -10,6 +10,8 @@
 #include <queue>
 #include <mutex>
 
+#include <stdexcept> // For std::runtime_error
+
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/qos.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
@@ -78,9 +80,11 @@ protected:
     std::vector<std::string> state_interface_types_;
 
     // Checker Methods
+    bool command_interface_checker_();
+    bool is_valid_goal(std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal);
     bool is_stalled(double current_position);
-    bool is_goal_valid(std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal);
     bool is_stopped();
+
     // Action Interface
     std::queue<std::shared_ptr<GripperGoalHandle>> goal_queue_;
     std::shared_ptr<GripperGoalHandle> current_goal_handle_;
@@ -93,6 +97,8 @@ protected:
     void process_next_goal();
     void execute_goal(const std::shared_ptr<GripperGoalHandle> goal_handle);
 
+    void publish_feedback();
+
     // Error Related
     bool stop_motion(double current_position);
 
@@ -111,22 +117,24 @@ protected:
       {"max_effort", &max_effort_command_interface_}};
 
     // State Interfaces
-    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
     joint_position_state_interface_;
 
-    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> *>
+    std::unordered_map<std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>> *>
     state_interface_map_ = {
       {"position", &joint_position_state_interface_}};
 
     // Addtional State variables
+    double max_position_ = 0.0;
     double target_position_ = -1.0;
+    double current_position_ = -1.0;
     double last_position_ = -1.0;
     int stall_counter_ = 0;
     bool cancel_requested_ = false;
 
     // Const
-    const int MAX_STALL_COUNT = 5;
+    const int MAX_STALL_COUNT = 200;
-    const double POSITION_TOLERANCE = 0.01;
+    const double POSITION_TOLERANCE = 0.0001;
     const double MIN_SPEED = 0.0;
     const double MIN_EFFORT = 0.0;
 

src/robotiq_2f/robotiq_2f_controllers/package.xml

@@ -15,6 +15,7 @@
   <depend>pluginlib</depend>
   <depend>robotiq_2f_msgs</depend>
   <depend>realtime_tools</depend>
+  <depend>std_srvs</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_activation_controller.cpp

@@ -26,7 +26,7 @@
 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 // POSSIBILITY OF SUCH DAMAGE.
 
-#include "robotiq_controllers/robotiq_activation_controller.hpp"
+#include "robotiq_2f_controllers/robotiq_activation_controller.hpp"
 
 namespace robotiq_controllers
 {

src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp

@@ -10,6 +10,7 @@ RobotiqGripperCommandController::RobotiqGripperCommandController() : controller_
 controller_interface::CallbackReturn RobotiqGripperCommandController::on_init()
 {
     joint_name_ = auto_declare<std::string>("joint", joint_name_);
+    max_position_ = auto_declare<double>("max_gap", max_position_);
     command_interface_types_ =
     auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
     state_interface_types_ =
@@ -17,7 +18,7 @@ controller_interface::CallbackReturn RobotiqGripperCommandController::on_init()
 
 
     if (joint_name_.empty()) {
-        RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint_name' parameter.");
+        RCLCPP_ERROR(get_node()->get_logger(), "Missing 'joint' parameter.");
         return CallbackReturn::ERROR;
     }
     else{
@@ -61,19 +62,14 @@ controller_interface::InterfaceConfiguration RobotiqGripperCommandController::st
 
 controller_interface::CallbackReturn RobotiqGripperCommandController::on_configure(const rclcpp_lifecycle::State &)
 {
-    this->action_server_ = rclcpp_action::create_server<robotiq_2f_msgs::action::GripperCommand>(
+    action_server_ = rclcpp_action::create_server<robotiq_2f_msgs::action::GripperCommand>(
-        get_node()->get_node_base_interface(),
+        get_node(),
-        get_node()->get_node_clock_interface(),
-        get_node()->get_node_logging_interface(),
-        get_node()->get_node_waitables_interface(),
         "~/gripper_command",
         [this](const std::array<unsigned char, 16>& uuid, std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal) {
             return this->handle_goal(uuid, goal);
         },
         [this](std::shared_ptr<GripperGoalHandle> goal_handle) {return this->handle_cancel(goal_handle);},
-        [this](std::shared_ptr<GripperGoalHandle> goal_handle) {this->handle_accepted(goal_handle);},
+        [this](std::shared_ptr<GripperGoalHandle> goal_handle) {this->handle_accepted(goal_handle);}
-        rcl_action_server_get_default_options(),
-        get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive)
     );
     return CallbackReturn::SUCCESS;
 }
@@ -89,6 +85,7 @@ controller_interface::CallbackReturn RobotiqGripperCommandController::on_activat
     joint_position_command_interface_.clear();
     max_velocity_command_interface_.clear();
     max_effort_command_interface_.clear();
+    joint_position_state_interface_.clear();
 
     // assign command interfaces
     for (auto & interface : command_interfaces_)
@@ -96,6 +93,19 @@ controller_interface::CallbackReturn RobotiqGripperCommandController::on_activat
         command_interface_map_[interface.get_interface_name()]->push_back(interface);
     }
 
+    // assign state interface
+        for (auto & interface : state_interfaces_)
+    {
+        state_interface_map_[interface.get_interface_name()]->push_back(interface);
+    }
+
+    RCLCPP_INFO(get_node()->get_logger(), "Checking if command interfaces are initialized corretly...");
+    if (!command_interface_checker_())
+    {
+        return CallbackReturn::ERROR;
+    }
+    RCLCPP_INFO(get_node()->get_logger(), "Command interfaces correctly initialized!");
+
     return CallbackReturn::SUCCESS;
 }
 
@@ -105,12 +115,24 @@ controller_interface::return_type RobotiqGripperCommandController::update(const
     if (current_goal_handle_) {
 
         // Retrieve the current position of the gripper
-        double current_position = joint_position_state_interface_[0].get().get_value();
+        try
+        {
+            if (joint_position_state_interface_.empty())
+            {
+                throw std::runtime_error("State interface is not correctly initialized");
+            }
+
+            current_position_ = joint_position_state_interface_[0].get().get_value();
+        } catch (const std::exception& e) {
+            RCLCPP_FATAL(get_node()->get_logger(), "Failed to access state interface: %s", e.what());
+            return controller_interface::return_type::ERROR;
+        }
+
 
         // Check if the gripper is stalled
-        if (is_stalled(current_position)) {
+        if (is_stalled(current_position_)) {
             auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
-            result->final_position = current_position;
+            result->final_position = current_position_;
             result->reached_goal = false;
             result->stalled = true;
             current_goal_handle_->abort(result);
@@ -121,7 +143,7 @@ controller_interface::return_type RobotiqGripperCommandController::update(const
         // Check if the gripper has stopped after a cancel request
         if (cancel_requested_ && is_stopped()) {
         auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
-        result->final_position = current_position;
+        result->final_position = current_position_;
         result->reached_goal = false;
         result->stalled = false;
         current_goal_handle_->canceled(result);  // Notify that the goal was canceled successfully
@@ -133,14 +155,12 @@ controller_interface::return_type RobotiqGripperCommandController::update(const
         }
 
         // Publish feedback about the current position
-        auto feedback = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Feedback>();
+        publish_feedback();
-        feedback->current_position = current_position;
-        current_goal_handle_->publish_feedback(feedback);
 
         // Check if the target position is reached within a tolerance
-        if (std::abs(current_position - target_position_) < POSITION_TOLERANCE) {
+        if (std::abs(current_position_ - target_position_) < POSITION_TOLERANCE) {
             auto result = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Result>();
-            result->final_position = current_position;
+            result->final_position = current_position_;
             result->reached_goal = true;
             result->stalled = false;
             current_goal_handle_->succeed(result);
@@ -156,8 +176,12 @@ controller_interface::return_type RobotiqGripperCommandController::update(const
 
 controller_interface::CallbackReturn RobotiqGripperCommandController::on_deactivate(const rclcpp_lifecycle::State & )
 {
-  release_interfaces();
+    RCLCPP_INFO(get_node()->get_logger(), "Deactivating and releasing interfaces.");
-
+    release_interfaces();  // Ensure this function properly cleans up resources
+    joint_position_command_interface_.clear();
+    max_velocity_command_interface_.clear();
+    max_effort_command_interface_.clear();
+    joint_position_state_interface_.clear();
     return CallbackReturn::SUCCESS;
 }
 
@@ -192,10 +216,16 @@ controller_interface::CallbackReturn RobotiqGripperCommandController::on_shutdow
 
 // Action Handling
 rclcpp_action::GoalResponse RobotiqGripperCommandController::handle_goal(const rclcpp_action::GoalUUID & goal_uuid,
-     std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> /*goal*/)
+     std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal)
 {
     (void)goal_uuid;  // Suppress unused variable warning
-        RCLCPP_INFO(get_node()->get_logger(), "Received goal request");
+    RCLCPP_INFO(get_node()->get_logger(), "Received goal request with position: %f", goal->desired_position);
+
+    if (!is_valid_goal(goal)) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Goal is invalid");
+        return rclcpp_action::GoalResponse::REJECT;
+    }
+
     return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER;
 }
 
@@ -212,37 +242,115 @@ rclcpp_action::CancelResponse RobotiqGripperCommandController::handle_cancel(con
 
 void RobotiqGripperCommandController::handle_accepted(const std::shared_ptr<GripperGoalHandle> goal_handle)
 {
+
     goal_queue_.push(goal_handle);
     if (!current_goal_handle_) {  // Only start a new goal if one isn't already active
         process_next_goal();
     }
 }
 
+void RobotiqGripperCommandController::publish_feedback()
+{
+    if (!current_goal_handle_) {
+        RCLCPP_ERROR(get_node()->get_logger(), "Goal handle is null, cannot publish feedback.");
+        return;
+    }
+
+    if (current_goal_handle_->is_canceling()) {
+        RCLCPP_WARN(get_node()->get_logger(), "Goal is being canceled, not publishing feedback.");
+        return;
+    }
+
+    auto feedback = std::make_shared<robotiq_2f_msgs::action::GripperCommand::Feedback>();
+    feedback->current_position = current_position_;
+
+    try {
+        current_goal_handle_->publish_feedback(feedback);
+    } catch (const std::exception& e) {
+        RCLCPP_FATAL(get_node()->get_logger(), "Failed to publish feedback: %s", e.what());
+    }
+}
+
 void RobotiqGripperCommandController::process_next_goal()
 {
     std::lock_guard<std::mutex> lock(mutex_);
     if (goal_queue_.empty()) {
         current_goal_handle_.reset();
+        RCLCPP_DEBUG(get_node()->get_logger(), "No more goals to process.");
         return;
     }
     current_goal_handle_ = goal_queue_.front();
     goal_queue_.pop();
+    current_goal_handle_->execute();
 
+    if (current_goal_handle_) {
+        auto goal = current_goal_handle_->get_goal();
+        if (!goal) {
+            RCLCPP_ERROR(get_node()->get_logger(), "Received a null goal in queue, skipping...");
+            process_next_goal();  // Recursively handle the next goal
+        }
+        else {
             try {
                 auto goal = current_goal_handle_->get_goal();
                 target_position_ = goal->desired_position;
                 double desired_max_velocity = goal->max_velocity;
                 double desired_max_effort = goal->max_effort;
 
+                if (!command_interface_checker_())
+                {
+                    throw std::runtime_error("At least one interface is not initialized!");
+                }
+
                 joint_position_command_interface_[0].get().set_value(target_position_);
                 max_velocity_command_interface_[0].get().set_value(desired_max_velocity);
                 max_effort_command_interface_[0].get().set_value(desired_max_effort);
-    } catch (const std::exception& e) {
+            }
+            catch (const std::exception& e) {
                 RCLCPP_ERROR(get_node()->get_logger(), "Exception in process_next_goal: %s", e.what());
             }
+        }
+    }
 }
 
 // Utility
+bool RobotiqGripperCommandController::command_interface_checker_()
+{
+    if (joint_position_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No position command interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Position command interfaces have been successfully initialized.");
+    }
+
+    if (max_velocity_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No velocity command interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Velocity command interfaces have been successfully initialized.");
+    }
+
+    if (max_effort_command_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No effort command interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Effort command interfaces have been successfully initialized.");
+    }
+
+    if (joint_position_state_interface_.empty()) {
+        RCLCPP_FATAL(get_node()->get_logger(), "No position state interfaces have been configured.");
+        return false;
+    } else {
+        RCLCPP_DEBUG(get_node()->get_logger(), "Position stateinterfaces have been successfully initialized.");
+    }
+
+    return true;
+}
+
+bool RobotiqGripperCommandController::is_valid_goal(std::shared_ptr<const robotiq_2f_msgs::action::GripperCommand::Goal> goal) {
+    // Add your validation logic here
+    return goal->desired_position >= 0 && goal->desired_position <= max_position_;
+}
+
 bool RobotiqGripperCommandController::is_stalled(double current_position)
 {
     std::lock_guard<std::mutex> lock(mutex_);
@@ -253,7 +361,7 @@ bool RobotiqGripperCommandController::is_stalled(double current_position)
     }
     last_position_ = current_position;
 
-return stall_counter_ > MAX_STALL_COUNT;  // Consider it stalled if no movement for too many cycles
+    return stall_counter_ > MAX_STALL_COUNT;
 }
 
 bool RobotiqGripperCommandController::stop_motion(double current_position)

src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml

@@ -5,12 +5,19 @@ controller_manager:
       type: joint_state_broadcaster/JointStateBroadcaster
     forward_gripper_position_controller:
       type: robotiq_2f_controllers/ForwardController
+    robotiq_gripper_controller:
+      type: robotiq_2f_controllers/GripperCommand
     robotiq_activation_controller:
-      type: robotiq_controllers/RobotiqActivationController
+      type: robotiq_2f_controllers/RobotiqActivationController
 
 forward_gripper_position_controller:
   ros__parameters:
-    joint: gripper_gap
+    joint: $(var prefix)gripper_gap
+
+robotiq_gripper_controller:
+  ros__parameters:
+    joint: $(var prefix)gripper_gap
+    max_gap: 0.14
 
 robotiq_activation_controller:
   ros__parameters:

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

@@ -1,95 +1,137 @@
-import launch
+from launch_ros.actions import Node
-from launch.substitutions import (
+from launch_ros.parameter_descriptions import ParameterFile, ParameterValue
-    Command,
+from launch_ros.substitutions import FindPackageShare
-    FindExecutable,
+
-    LaunchConfiguration,
+from launch import LaunchDescription
-    PathJoinSubstitution,
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
-)
 from launch.conditions import IfCondition
-import launch_ros
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
-import os
+
 
 def generate_launch_description():
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_package",
+            default_value="robotiq_2f_description",
+            description="Description package with gripper URDF/XACRO files.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "description_file",
+            default_value="robotiq_2f_140.urdf.xacro",
+            description="URDF/XACRO description file with the robot gripper.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "prefix",
+            default_value="",
+            description="prefix of the joint names, useful for \
+        multi-robot setup. If changed, also joint names in the controllers' configuration \
+        have to be updated.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "use_fake_hardware",
+            default_value="false",
+            description="Start gripper with fake hardware mirroring command to its states.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_ip", description="IP address by which the robot can be reached."
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "robot_port",
+            default_value="63352",
+            description="Remote port for UR Cap.",
+        )
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("launch_rviz", default_value="false", description="Launch RViz?")
+    )
+    declared_arguments.append(
+        DeclareLaunchArgument("use_forward_controller",
+                              default_value="false",
+                              description="Use forward controller?")
+    )
+    return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
 
-    use_fake_hardware = 'true'
-    use_fake_hardware_bool = use_fake_hardware == 'true'
 
-    description_pkg_share = launch_ros.substitutions.FindPackageShare(
+def launch_setup(context, *args, **kwargs):
-        package="robotiq_description"
-    ).find("robotiq_2f_description")
-    default_model_path = os.path.join(
-        description_pkg_share, "urdf", "robotiq_2f_140.urdf.xacro"
-    )
-    default_rviz_config_path = os.path.join(
-        description_pkg_share, "rviz", "view_urdf.rviz"
-    )
 
-    args = []
+    robot_ip = LaunchConfiguration("robot_ip")
-    args.append(
+    description_package = LaunchConfiguration("description_package")
-        launch.actions.DeclareLaunchArgument(
+    description_file = LaunchConfiguration("description_file")
-            name="model",
+    prefix = LaunchConfiguration("prefix")
-            default_value=default_model_path,
+    use_fake_hardware = LaunchConfiguration("use_fake_hardware")
-            description="Absolute path to gripper URDF file",
+    robot_port = LaunchConfiguration("robot_port")
-        )
+    launch_rviz = LaunchConfiguration("launch_rviz")
-    )
+    use_forward_controller = LaunchConfiguration("use_forward_controller")
-    args.append(
-        launch.actions.DeclareLaunchArgument(
-            name="rvizconfig",
-            default_value=default_rviz_config_path,
-            description="Absolute path to rviz config file",
-        )
-    )
-    args.append(
-        launch.actions.DeclareLaunchArgument(
-            name="launch_rviz", default_value="true", description="Launch RViz?"
-        )
-    )
 
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
-            LaunchConfiguration("model"),
+            PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
+            " ",
+            "prefix:=",
+            prefix,
+            " ",
+            "use_fake_hardware:=",
+            use_fake_hardware,
+            " ",
+            "robot_ip:=",
+            robot_ip,
+            " ",
+            "robot_port:=",
+            robot_port,
             " ",
-            "use_fake_hardware:=true",
         ]
     )
-    robot_description_param = {
+
-        "robot_description": launch_ros.parameter_descriptions.ParameterValue(
+    robot_description = {"robot_description": robot_description_content}
-            robot_description_content, value_type=str
-        )
-    }
 
     controllers_file = "robotiq_controllers.yaml"
     initial_joint_controllers = PathJoinSubstitution(
-        [description_pkg_share, "config", controllers_file]
+        [FindPackageShare(description_package), "config", controllers_file]
     )
 
-    control_node = launch_ros.actions.Node(
+    control_node = Node(
         package="controller_manager",
         executable="ros2_control_node",
         parameters=[
-            robot_description_param,
+            robot_description,
-            initial_joint_controllers,
+            ParameterFile(initial_joint_controllers, allow_substs=True),
         ],
     )
 
-    robot_state_publisher_node = launch_ros.actions.Node(
+    robot_state_publisher_node = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
-        parameters=[robot_description_param],
+        parameters=[robot_description],
     )
 
-    rviz_node = launch_ros.actions.Node(
+    rviz_config_file = PathJoinSubstitution(
+        [FindPackageShare(description_package), "rviz", "view_robot.rviz"]
+    )
+
+
+    rviz_node = Node(
         package="rviz2",
         executable="rviz2",
         name="rviz2",
         output="log",
-        arguments=["-d", LaunchConfiguration("rvizconfig")],
+        arguments=["-d", rviz_config_file],
-        condition=IfCondition(LaunchConfiguration("launch_rviz")),
+        condition=IfCondition(launch_rviz),
     )
 
-    joint_state_broadcaster_spawner = launch_ros.actions.Node(
+    joint_state_broadcaster_spawner = Node(
         package="controller_manager",
         executable="spawner",
         arguments=[
@@ -99,7 +141,7 @@ def generate_launch_description():
         ],
     )
 
-    robotiq_activation_controller_spawner = launch_ros.actions.Node(
+    robotiq_activation_controller_spawner = Node(
         package="controller_manager",
         executable="spawner",
         arguments=["robotiq_activation_controller", "-c", "/controller_manager"],
@@ -107,7 +149,7 @@ def generate_launch_description():
 
     def controller_spawner(name, active=True):
         inactive_flags = ["--inactive"] if not active else []
-        return launch_ros.actions.Node(
+        return Node(
             package="controller_manager",
             executable="spawner",
             arguments=[
@@ -117,12 +159,12 @@ def generate_launch_description():
                       ] + inactive_flags,
         )
 
-    if not use_fake_hardware_bool:
+    if use_forward_controller.perform(context) == 'false':
-        controller_spawner_names = []
+        controller_spawner_names = ['robotiq_gripper_controller']
         controller_spawner_inactive_names = ["forward_gripper_position_controller"]
     else:
         controller_spawner_names = ["forward_gripper_position_controller"]
-        controller_spawner_inactive_names = []
+        controller_spawner_inactive_names = ['robotiq_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
@@ -136,4 +178,4 @@ def generate_launch_description():
         rviz_node,
     ] + controller_spawners
 
-    return launch.LaunchDescription(args + nodes)
+    return nodes

src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz

@@ -4,9 +4,11 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
         - /Status1
-      Splitter Ratio: 0.6264705657958984
+        - /RobotModel1
-    Tree Height: 555
+      Splitter Ratio: 0.5
+    Tree Height: 719
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -56,7 +58,7 @@ Visualization Manager:
         Durability Policy: Volatile
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: robot_description
+        Value: /robot_description
       Enabled: true
       Links:
         All Links Enabled: true
@@ -64,78 +66,70 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        arm_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         dummy_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        end_effector_link:
+          Value: true
-          Alpha: 1
+        robotiq_140_base_link:
-          Show Axes: false
-          Show Trail: false
-        forearm_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        gripper_base_link:
+        robotiq_140_left_inner_finger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        left_finger_dist_link:
+        robotiq_140_left_inner_finger_pad:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        left_finger_prox_link:
+        robotiq_140_left_inner_knuckle:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        lower_wrist_link:
+        robotiq_140_left_outer_finger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        right_finger_dist_link:
+        robotiq_140_left_outer_knuckle:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        right_finger_prox_link:
+        robotiq_140_right_inner_finger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        shoulder_link:
+        robotiq_140_right_inner_finger_pad:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        table:
+        robotiq_140_right_inner_knuckle:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        tool_frame:
+        robotiq_140_right_outer_finger:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        upper_wrist_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
+        robotiq_140_right_outer_knuckle:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        robotiq_base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         world:
           Alpha: 1
           Show Axes: false
@@ -194,33 +188,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 2.1567115783691406
+      Distance: 1.321521282196045
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.09681572020053864
+        X: 0
-        Y: -0.10843408107757568
+        Y: 0
-        Z: 0.1451336145401001
+        Z: 0
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.785398006439209
+      Pitch: 0.30539846420288086
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 0.785398006439209
+      Yaw: 1.455397367477417
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 846
+  Height: 1016
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006501000000000000073a000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004c90000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -229,6 +223,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1200
+  Width: 1850
-  X: 1989
+  X: 1990
-  Y: 261
+  Y: 27

src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro

@@ -437,7 +437,7 @@
         </joint>
 
         <!-- Pseudo Link and Joint for gripper gap -->
-        <link name="dummy_link">
+        <link name="${prefix}dummy_link">
             <visual>
                 <geometry>
                     <box size="0.001 0.001 0.001"/>  <!-- Very small, effectively invisible -->
@@ -455,7 +455,7 @@
         </joint>
 
         <joint name="gripper_gap" type="prismatic">
-            <parent link="dummy_link"/>
+            <parent link="${prefix}dummy_link"/>
             <child link="${prefix}robotiq_140_right_inner_finger_pad"/>
             <origin xyz="0 0 0" rpy="0 0 0"/>  <!-- Set origin appropriately -->
             <axis xyz="0 1 0"/>  <!-- Axis of movement; adjust according to actual alignment -->

src/robotiq_2f/robotiq_controllers/CHANGELOG.rst

@@ -1,9 +0,0 @@
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-Changelog for package robotiq_controllers
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-
-0.0.1 (2023-07-17)
-------------------
-* Initial ROS 2 release of robotiq_controllers
-  * This package is not supported by Robotiq but is being maintained by PickNik Robotics
-* Contributors: Alex Moriarty, Cory Crean

src/robotiq_2f/robotiq_controllers/CMakeLists.txt

@@ -1,84 +0,0 @@
-cmake_minimum_required(VERSION 3.8)
-project(robotiq_controllers)
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(controller_interface REQUIRED)
-find_package(std_srvs REQUIRED)
-
-set(THIS_PACKAGE_INCLUDE_DEPENDS
-  controller_interface
-  std_srvs
-)
-
-include_directories(include)
-
-add_library(${PROJECT_NAME} SHARED
-  src/robotiq_activation_controller.cpp
-)
-
-target_include_directories(${PROJECT_NAME} PRIVATE
-  include
-)
-
-ament_target_dependencies(${PROJECT_NAME}
-  ${THIS_PACKAGE_INCLUDE_DEPENDS}
-)
-
-pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
-
-# # INSTALL
-install(
-  TARGETS ${PROJECT_NAME}
-  EXPORT export_${PROJECT_NAME}
-  ARCHIVE DESTINATION lib/${PROJECT_NAME}
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-)
-install(
-  DIRECTORY include/
-  DESTINATION include
-)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-
-  # the following line skips the linter which checks for copyrights
-  # comment the line when a copyright and license is added to all source files
-  set(ament_cmake_copyright_FOUND TRUE)
-
-  # the following line skips cpplint (only works in a git repo)
-  # comment the line when this package is in a git repo and when
-  # a copyright and license is added to all source files
-  set(ament_cmake_cpplint_FOUND TRUE)
-
-  # the following skips uncrustify
-  # ament_uncrustify and ament_clang_format cannot both be satisfied
-  set(ament_cmake_uncrustify_FOUND TRUE)
-
-  # the following skips xmllint
-  # ament_xmllint requires network and can timeout if on throttled networks
-  set(ament_cmake_xmllint_FOUND TRUE)
-
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-# # EXPORTS
-ament_export_include_directories(
-  include
-)
-ament_export_libraries(
-  ${PROJECT_NAME}
-)
-ament_export_targets(
-  export_${PROJECT_NAME}
-)
-ament_export_dependencies(
-  ${THIS_PACKAGE_INCLUDE_DEPENDS}
-)
-
-ament_package()

src/robotiq_2f/robotiq_controllers/controller_plugins.xml

@@ -1,7 +0,0 @@
-<library path="robotiq_controllers">
-    <class name="robotiq_controllers/RobotiqActivationController" type="robotiq_controllers::RobotiqActivationController" base_class_type="controller_interface::ControllerInterface">
-        <description>
-            This controller provides an interface to (re-)activate the Robotiq gripper.
-        </description>
-    </class>
-</library>

src/robotiq_2f/robotiq_controllers/package.xml

@@ -1,23 +0,0 @@
-<?xml version="1.0"?>
-<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
-<package format="3">
-  <name>robotiq_controllers</name>
-  <version>0.0.1</version>
-  <description>Controllers for the Robotiq gripper.</description>
-  <maintainer email="alex.moriarty@picknik.ai">Alex Moriarty</maintainer>
-  <maintainer email="marq.rasmussen@picknik.ai">Marq Rasmussen</maintainer>
-  <license>BSD 3-Clause</license>
-  <author>Cory Crean</author>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <depend>controller_interface</depend>
-  <depend>std_srvs</depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>