diff --git a/src/robotiq_2f/README.md b/src/robotiq_2f/README.md index e69de29..4e6b3ca 100644 --- a/src/robotiq_2f/README.md +++ b/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 + + diff --git a/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt b/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt index b0731c8..98a2cae 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt +++ b/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) diff --git a/src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml b/src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml index dad6fbc..dd8ad5f 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml +++ b/src/robotiq_2f/robotiq_2f_controllers/controller_plugin.xml @@ -9,4 +9,9 @@ Gripper Command controller for Robotiq 2F Gripper. + + + This controller provides an interface to (re-)activate the Robotiq gripper. + + \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp b/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_activation_controller.hpp similarity index 100% rename from src/robotiq_2f/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp rename to src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_activation_controller.hpp diff --git a/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp b/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp index 1c30876..fa2cd87 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp +++ b/src/robotiq_2f/robotiq_2f_controllers/include/robotiq_2f_controllers/robotiq_gripper_command_controller.hpp @@ -10,6 +10,8 @@ #include #include +#include // For std::runtime_error + #include #include #include @@ -78,9 +80,11 @@ protected: std::vector state_interface_types_; // Checker Methods + bool command_interface_checker_(); + bool is_valid_goal(std::shared_ptr goal); bool is_stalled(double current_position); - bool is_goal_valid(std::shared_ptr goal); bool is_stopped(); + // Action Interface std::queue> goal_queue_; std::shared_ptr current_goal_handle_; @@ -93,6 +97,8 @@ protected: void process_next_goal(); void execute_goal(const std::shared_ptr 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::vector> joint_position_state_interface_; - std::unordered_map> *> + std::unordered_map> *> 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 double POSITION_TOLERANCE = 0.01; + const int MAX_STALL_COUNT = 200; + const double POSITION_TOLERANCE = 0.0001; const double MIN_SPEED = 0.0; const double MIN_EFFORT = 0.0; diff --git a/src/robotiq_2f/robotiq_2f_controllers/package.xml b/src/robotiq_2f/robotiq_2f_controllers/package.xml index fd14a7f..2aac29e 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/package.xml +++ b/src/robotiq_2f/robotiq_2f_controllers/package.xml @@ -15,6 +15,7 @@ pluginlib robotiq_2f_msgs realtime_tools + std_srvs ament_lint_auto ament_lint_common diff --git a/src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp b/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_activation_controller.cpp similarity index 98% rename from src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp rename to src/robotiq_2f/robotiq_2f_controllers/src/robotiq_activation_controller.cpp index aa63d21..32daf96 100644 --- a/src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp +++ b/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 { diff --git a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp b/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp index 6d01417..7fc4c2c 100644 --- a/src/robotiq_2f/robotiq_2f_controllers/src/robotiq_gripper_command_controller.cpp +++ b/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("joint", joint_name_); + max_position_ = auto_declare("max_gap", max_position_); command_interface_types_ = auto_declare>("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( - get_node()->get_node_base_interface(), - get_node()->get_node_clock_interface(), - get_node()->get_node_logging_interface(), - get_node()->get_node_waitables_interface(), + action_server_ = rclcpp_action::create_server( + get_node(), "~/gripper_command", [this](const std::array& uuid, std::shared_ptr goal) { return this->handle_goal(uuid, goal); }, [this](std::shared_ptr goal_handle) {return this->handle_cancel(goal_handle);}, - [this](std::shared_ptr goal_handle) {this->handle_accepted(goal_handle);}, - rcl_action_server_get_default_options(), - get_node()->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive) + [this](std::shared_ptr goal_handle) {this->handle_accepted(goal_handle);} ); 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(); - 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(); - 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(); - feedback->current_position = current_position; - current_goal_handle_->publish_feedback(feedback); + publish_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(); - result->final_position = current_position; + result->final_position = current_position_; result->reached_goal = true; result->stalled = false; current_goal_handle_->succeed(result); @@ -156,9 +176,13 @@ controller_interface::return_type RobotiqGripperCommandController::update(const controller_interface::CallbackReturn RobotiqGripperCommandController::on_deactivate(const rclcpp_lifecycle::State & ) { - release_interfaces(); - - return CallbackReturn::SUCCESS; + 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; } controller_interface::CallbackReturn RobotiqGripperCommandController::on_cleanup(const rclcpp_lifecycle::State & ) @@ -192,11 +216,17 @@ controller_interface::CallbackReturn RobotiqGripperCommandController::on_shutdow // Action Handling rclcpp_action::GoalResponse RobotiqGripperCommandController::handle_goal(const rclcpp_action::GoalUUID & goal_uuid, - std::shared_ptr /*goal*/) + std::shared_ptr goal) { - (void)goal_uuid; // Suppress unused variable warning - RCLCPP_INFO(get_node()->get_logger(), "Received goal request"); - return rclcpp_action::GoalResponse::ACCEPT_AND_DEFER; + (void)goal_uuid; // Suppress unused variable warning + 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; } rclcpp_action::CancelResponse RobotiqGripperCommandController::handle_cancel(const std::shared_ptr goal_handle) @@ -212,37 +242,115 @@ rclcpp_action::CancelResponse RobotiqGripperCommandController::handle_cancel(con void RobotiqGripperCommandController::handle_accepted(const std::shared_ptr 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(); + 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 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(); - try { + if (current_goal_handle_) { 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 (!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; - 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) { - RCLCPP_ERROR(get_node()->get_logger(), "Exception in process_next_goal: %s", e.what()); + 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) { + 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 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 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) diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml index 7b5ead7..e3bb139 100644 --- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml +++ b/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: diff --git a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py index 46f9a1a..027d6ad 100644 --- a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py +++ b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py @@ -1,95 +1,137 @@ -import launch -from launch.substitutions import ( - Command, - FindExecutable, - LaunchConfiguration, - PathJoinSubstitution, -) +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterFile, ParameterValue +from launch_ros.substitutions import FindPackageShare + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition -import launch_ros -import os +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + def generate_launch_description(): - - use_fake_hardware = 'true' - use_fake_hardware_bool = use_fake_hardware == 'true' - - description_pkg_share = launch_ros.substitutions.FindPackageShare( - 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 = [] - args.append( - launch.actions.DeclareLaunchArgument( - name="model", - default_value=default_model_path, - description="Absolute path to gripper URDF file", + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "description_package", + default_value="robotiq_2f_description", + description="Description package with gripper URDF/XACRO files.", ) ) - args.append( - launch.actions.DeclareLaunchArgument( - name="rvizconfig", - default_value=default_rviz_config_path, - description="Absolute path to rviz config file", + declared_arguments.append( + DeclareLaunchArgument( + "description_file", + default_value="robotiq_2f_140.urdf.xacro", + description="URDF/XACRO description file with the robot gripper.", ) ) - args.append( - launch.actions.DeclareLaunchArgument( - name="launch_rviz", default_value="true", description="Launch RViz?" + 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)]) + + +def launch_setup(context, *args, **kwargs): + + robot_ip = LaunchConfiguration("robot_ip") + description_package = LaunchConfiguration("description_package") + description_file = LaunchConfiguration("description_file") + prefix = LaunchConfiguration("prefix") + use_fake_hardware = LaunchConfiguration("use_fake_hardware") + robot_port = LaunchConfiguration("robot_port") + launch_rviz = LaunchConfiguration("launch_rviz") + use_forward_controller = LaunchConfiguration("use_forward_controller") 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_content, value_type=str - ) - } + + robot_description = {"robot_description": robot_description_content} 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, - initial_joint_controllers, + robot_description, + 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")], - condition=IfCondition(LaunchConfiguration("launch_rviz")), + arguments=["-d", rviz_config_file], + 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: - controller_spawner_names = [] + if use_forward_controller.perform(context) == 'false': + 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) \ No newline at end of file + return nodes \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz b/src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz index c23ac44..4da7e00 100644 --- a/src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz +++ b/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 - Tree Height: 555 + - /RobotModel1 + 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: - Alpha: 1 - Show Axes: false - Show Trail: false - forearm_link: + Value: true + robotiq_140_base_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: - Alpha: 1 - Show Axes: false - Show Trail: false - upper_wrist_link: + robotiq_140_right_outer_finger: 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 - Y: -0.10843408107757568 - Z: 0.1451336145401001 + X: 0 + Y: 0 + 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: 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 - X: 1989 - Y: 261 + Width: 1850 + X: 1990 + Y: 27 diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro index 9195619..fac5c64 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro @@ -437,7 +437,7 @@ - + @@ -455,7 +455,7 @@ - + diff --git a/src/robotiq_2f/robotiq_controllers/CHANGELOG.rst b/src/robotiq_2f/robotiq_controllers/CHANGELOG.rst deleted file mode 100644 index 467d0a3..0000000 --- a/src/robotiq_2f/robotiq_controllers/CHANGELOG.rst +++ /dev/null @@ -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 diff --git a/src/robotiq_2f/robotiq_controllers/CMakeLists.txt b/src/robotiq_2f/robotiq_controllers/CMakeLists.txt deleted file mode 100644 index c4334bc..0000000 --- a/src/robotiq_2f/robotiq_controllers/CMakeLists.txt +++ /dev/null @@ -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() diff --git a/src/robotiq_2f/robotiq_controllers/controller_plugins.xml b/src/robotiq_2f/robotiq_controllers/controller_plugins.xml deleted file mode 100644 index 9c5c8f5..0000000 --- a/src/robotiq_2f/robotiq_controllers/controller_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - This controller provides an interface to (re-)activate the Robotiq gripper. - - - diff --git a/src/robotiq_2f/robotiq_controllers/package.xml b/src/robotiq_2f/robotiq_controllers/package.xml deleted file mode 100644 index 562b784..0000000 --- a/src/robotiq_2f/robotiq_controllers/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - robotiq_controllers - 0.0.1 - Controllers for the Robotiq gripper. - Alex Moriarty - Marq Rasmussen - BSD 3-Clause - Cory Crean - - ament_cmake - - controller_interface - std_srvs - - ament_lint_auto - ament_lint_common - - - ament_cmake - -