Niko/UR_Robotiq
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

mock system working

Browse Source
This commit is contained in:
Niko Feith 2024-04-15 13:51:52 +02:00
parent 3d098d19ab
commit d0d759a787
5 changed files with 144 additions and 102 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py
View File

@ -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)

2
src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py
View File

@ -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")

79
src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro
View File

@ -18,23 +18,26 @@
<ros2_control name="${name}" type="system">
<!-- Plugins -->
<hardware>
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
<xacro:if value="${use_fake_hardware}">
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
<xacro:if value="${use_fake_hardware}">
<plugin>mock_components/GenericSystem</plugin>
<param name="mock_sensor_commands">${fake_sensor_commands}</param>
<param name="state_following_offset">0.0</param>
</xacro:if>
<param name="gripper_closed_position">0.695</param>
<param name="robot_ip">${robot_ip}</param>
<param name="robot_port">${robot_port}</param>
<param name="use_fake_hardware">${use_fake_hardware}</param>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
<param name="min_position">${min_position}</param>
<param name="max_position">${max_position}</param>
<param name="min_speed">${min_speed}</param>
<param name="max_speed">${max_speed}</param>
<param name="min_force">${min_force}</param>
<param name="max_force">${max_force}</param>
<xacro:unless value="${use_fake_hardware}">
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
<param name="gripper_closed_position">0.695</param>
<param name="robot_ip">${robot_ip}</param>
<param name="robot_port">${robot_port}</param>
<param name="use_fake_hardware">${use_fake_hardware}</param>
<param name="gripper_speed_multiplier">1.0</param>
<param name="gripper_force_multiplier">0.5</param>
<param name="min_position">${min_position}</param>
<param name="max_position">${max_position}</param>
<param name="min_speed">${min_speed}</param>
<param name="max_speed">${max_speed}</param>
<param name="min_force">${min_force}</param>
<param name="max_force">${max_force}</param>
</xacro:unless>
</hardware>
<!-- Joint interfaces -->
@ -47,7 +50,28 @@
<state_interface name="velocity"/>
</joint>
<!-- When simulating we need to include the rest of the gripper joints -->
<xacro:if value="false">
<xacro:if value="${use_fake_hardware}">
<joint name="${prefix}right_outer_knuckle_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}left_outer_finger_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}right_outer_finger_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}left_inner_knuckle_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">-1</param>
@ -55,6 +79,13 @@
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}right_inner_knuckle_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}left_inner_finger_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">1</param>
@ -62,22 +93,8 @@
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}right_outer_knuckle_joint">
<joint name="${prefix}right_inner_finger_joint">
<param name="mimic">${prefix}finger_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}robotiq_140_left_finger_tip_joint">
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
<param name="multiplier">-1</param>
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="${prefix}robotiq_140_right_finger_tip_joint">
<param name="mimic">${prefix}robotiq_140_left_knuckle_joint</param>
<param name="multiplier">1</param>
<command_interface name="position"/>
<state_interface name="position"/>

87
src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
View File

@ -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<hardware_interface::CommandInterface> 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<std::mutex> 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());
}
}

11
src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp
View File

@ -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();