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 04a18dc..d685726 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,31 +1,3 @@ -# Copyright (c) 2022 PickNik, Inc. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# * Neither the name of the {copyright_holder} nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - import launch from launch.substitutions import ( Command, @@ -37,8 +9,11 @@ from launch.conditions import IfCondition import launch_ros import os - 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") @@ -124,25 +99,43 @@ def generate_launch_description(): ], ) - robotiq_gripper_controller_spawner = launch_ros.actions.Node( - package="controller_manager", - executable="spawner", - arguments=["robotiq_gripper_controller", "-c", "/controller_manager"], - ) - robotiq_activation_controller_spawner = launch_ros.actions.Node( package="controller_manager", executable="spawner", arguments=["robotiq_activation_controller", "-c", "/controller_manager"], ) + def controller_spawner(name, active=True): + inactive_flags = ["--inactive"] if not active else [] + return launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + name, + "--controller-manager", + "/controller_manager", + ] + inactive_flags, + ) + + if not use_fake_hardware_bool: + controller_spawner_names = ["robotiq_gripper_controller"] + controller_spawner_inactive_names = ["forward_gripper_position_controller", + "fake_gripper_controller"] + else: + controller_spawner_names = ["fake_gripper_controller"] + controller_spawner_inactive_names = ["forward_gripper_position_controller", + "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 + ] + nodes = [ control_node, robot_state_publisher_node, joint_state_broadcaster_spawner, - robotiq_gripper_controller_spawner, robotiq_activation_controller_spawner, rviz_node, - ] + ] + controller_spawners return launch.LaunchDescription(args + nodes) \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py index 5983666..9c58ccf 100644 --- a/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py +++ b/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py @@ -14,7 +14,7 @@ def generate_launch_description(): package="robotiq_2f_description" ).find("robotiq_2f_description") default_model_path = os.path.join( - pkg_share, "urdf", "robotiq_2f_85.urdf.xacro" + pkg_share, "urdf", "robotiq_2f_140.urdf.xacro" ) default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_urdf.rviz") diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro index b5b1cdc..cbf1543 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro @@ -18,23 +18,26 @@ - robotiq_2f_interface/RobotiqGripperHardwareInterface - - ${fake_sensor_commands} + + mock_components/GenericSystem + ${fake_sensor_commands} 0.0 - 0.695 - ${robot_ip} - ${robot_port} - ${use_fake_hardware} - 1.0 - 0.5 - ${min_position} - ${max_position} - ${min_speed} - ${max_speed} - ${min_force} - ${max_force} + + robotiq_2f_interface/RobotiqGripperHardwareInterface + 0.695 + ${robot_ip} + ${robot_port} + ${use_fake_hardware} + 1.0 + 0.5 + ${min_position} + ${max_position} + ${min_speed} + ${max_speed} + ${min_force} + ${max_force} + @@ -47,7 +50,28 @@ - + + + ${prefix}finger_joint + -1 + + + + + + ${prefix}finger_joint + 1 + + + + + + ${prefix}finger_joint + -1 + + + + ${prefix}finger_joint -1 @@ -55,6 +79,13 @@ + + ${prefix}finger_joint + -1 + + + + ${prefix}finger_joint 1 @@ -62,22 +93,8 @@ - + ${prefix}finger_joint - -1 - - - - - - ${prefix}robotiq_140_left_knuckle_joint - -1 - - - - - - ${prefix}robotiq_140_left_knuckle_joint 1 diff --git a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp index a330b20..d1d6944 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp @@ -220,8 +220,10 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( socket_adapter_->deactivate(); socket_adapter_->activate(); + communication_thread_is_running_.store(true); communication_thread_ = std::thread([this] { this->background_task(); }); + RCLCPP_INFO(logger_, "Background task thread started."); } catch (const std::exception& e) { @@ -229,8 +231,14 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( return CallbackReturn::ERROR; } - RCLCPP_INFO(logger_, "Robotiq Gripper successfully activated!"); - return CallbackReturn::SUCCESS; + if (!socket_adapter_->is_active()) { + RCLCPP_ERROR(logger_, "Attempted to activate while gripper is not active!"); + return CallbackReturn::ERROR; + } + else{ + RCLCPP_INFO(logger_, "Robotiq Gripper successfully activated!"); + return CallbackReturn::SUCCESS; + } } hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) @@ -312,7 +320,7 @@ std::vector RobotiqGripperHardwareInterfac hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_)); for (const auto& interface : command_interfaces) { - RCLCPP_DEBUG(logger_, "Exporting command interface for joint: %s type: %s", + RCLCPP_INFO(logger_, "Exporting command interface for joint: %s type: %s", interface.get_name().c_str(), interface.get_interface_name().c_str()); } @@ -347,9 +355,17 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclc } // Process asynchronous reactivation response if available - if (reactivate_gripper_async_response_.load().has_value()) { + if (!std::isnan(reactivate_gripper_cmd_)) + { + RCLCPP_INFO(logger_, "Sending gripper reactivation request."); + reactivate_gripper_async_cmd_.store(true); + reactivate_gripper_cmd_ = NO_NEW_CMD_; + } + + if (reactivate_gripper_async_response_.load().has_value()) + { reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value(); - reactivate_gripper_async_response_.store(std::nullopt); // Reset response + reactivate_gripper_async_response_.store(std::nullopt); } return hardware_interface::return_type::OK; @@ -381,38 +397,49 @@ void RobotiqGripperHardwareInterface::background_task() while (communication_thread_is_running_.load()) { std::lock_guard guard(resource_mutex_); - try + if (reactivate_gripper_async_cmd_.load()) { - // Re-activate the gripper - // This can be used, for example, to re-run the auto-calibration. - if (reactivate_gripper_async_cmd_.load()) + try { + // Execute the reactivation sequence + RCLCPP_INFO(logger_, "Reactivating gripper."); socket_adapter_->deactivate(); - socket_adapter_->activate(); - reactivate_gripper_async_cmd_.store(false); - reactivate_gripper_async_response_.store(true); + socket_adapter_->activate(false); + reactivate_gripper_async_cmd_.store(false); // Reset the command flag + reactivate_gripper_async_response_.store(true); // Store the success of the operation } - - // Write the latest command to the gripper. - int scaled_position = write_command_.load(); - int scaled_speed = write_speed_.load(); - int scaled_force = write_force_.load(); - - auto result = socket_adapter_->move(scaled_position, scaled_speed, scaled_force); - if (!std::get<0>(result)) { - throw std::runtime_error("Failed to send move command to Robotiq gripper."); + catch (const std::exception& e) + { + RCLCPP_ERROR(logger_, "Failed to reactivate gripper: %s", e.what()); + reactivate_gripper_async_response_.store(false); // Store the failure of the operation } - - // Read the state of the gripper. - int raw_position = socket_adapter_->position(); - gripper_current_state_.store(convertRawToPosition(raw_position)); - } - catch (const std::exception& e) - { - RCLCPP_ERROR(logger_, "Error in background task: %s", e.what()); } - std::this_thread::sleep_for(gripperCommsLoopPeriod); // Sleep for a predefined period + // Additional periodic operations + performRegularOperations(); + std::this_thread::sleep_for(gripperCommsLoopPeriod); // Sleep for the duration of the communication loop period + } +} + +void RobotiqGripperHardwareInterface::performRegularOperations() +{ + try + { + int scaled_position = write_command_.load(); + int scaled_speed = write_speed_.load(); + int scaled_force = write_force_.load(); + + auto result = socket_adapter_->move(scaled_position, scaled_speed, scaled_force); + if (!std::get<0>(result)) { + throw std::runtime_error("Failed to send move command to Robotiq gripper."); + } + + int raw_position = socket_adapter_->position(); + gripper_current_state_.store(convertRawToPosition(raw_position)); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(logger_, "Regular operation error: %s", e.what()); } } diff --git a/src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp b/src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp index aa63d21..68fa2e4 100644 --- a/src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp +++ b/src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp @@ -105,12 +105,17 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Roboti bool RobotiqActivationController::reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr /*req*/, std_srvs::srv::Trigger::Response::SharedPtr resp) { + RCLCPP_INFO(get_node()->get_logger(), "Received reactivation request."); command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].set_value(ASYNC_WAITING); command_interfaces_[REACTIVATE_GRIPPER_CMD].set_value(1.0); + RCLCPP_INFO(get_node()->get_logger(), "Reactivation command sent to hardware interface."); - while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING) - { - std::this_thread::sleep_for(std::chrono::milliseconds(50)); + int attempts = 0; + int max_attempts = 1000; + while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING && attempts < max_attempts) { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + attempts++; + RCLCPP_INFO(get_node()->get_logger(), "Attempt %d: Checking response from hardware interface: %f", attempts, command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value()); } resp->success = command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value();