From fc34a7a289115e4385477512958f5af0a4e9e23b Mon Sep 17 00:00:00 2001 From: qlin960618 Date: Wed, 18 Dec 2024 15:13:33 +0900 Subject: [PATCH] [sas_robot_driver_franka_hand_node] added initialization option for not homing hand on startup --- .../interfaces/qros_effector_driver_franka_hand.hpp | 1 + src/hand/qros_effector_driver_franka_hand.cpp | 4 +++- src/sas_robot_driver_franka_hand_node.cpp | 1 + 3 files changed, 5 insertions(+), 1 deletion(-) diff --git a/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp b/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp index e26bcc0..1dcd187 100644 --- a/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp +++ b/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp @@ -69,6 +69,7 @@ namespace qros { struct EffectorDriverFrankaHandConfiguration { std::string robot_ip; + bool initialize_with_homing = true; double thread_sampeling_time_s = 1e8; // 10Hz double default_force = 3.0; double default_speed = 0.1; diff --git a/src/hand/qros_effector_driver_franka_hand.cpp b/src/hand/qros_effector_driver_franka_hand.cpp index 0c514bf..88af19a 100644 --- a/src/hand/qros_effector_driver_franka_hand.cpp +++ b/src/hand/qros_effector_driver_franka_hand.cpp @@ -149,7 +149,9 @@ namespace qros { if (!_is_connected()) throw std::runtime_error( "[" + std::string(node_->get_name())+ "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected."); - gripper_homing(); + if (configuration_.initialize_with_homing) { + gripper_homing(); + } // start gripper status loop status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this); // check status loop with timeout diff --git a/src/sas_robot_driver_franka_hand_node.cpp b/src/sas_robot_driver_franka_hand_node.cpp index 12ede61..64415d3 100644 --- a/src/sas_robot_driver_franka_hand_node.cpp +++ b/src/sas_robot_driver_franka_hand_node.cpp @@ -81,6 +81,7 @@ int main(int argc, char **argv) RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading parameters from parameter server."); qros::EffectorDriverFrankaHandConfiguration robot_driver_franka_hand_configuration; + get_optional_parameter(node, "initialize_with_homing", robot_driver_franka_hand_configuration.initialize_with_homing); sas::get_ros_parameter(node,"robot_ip_address",robot_driver_franka_hand_configuration.robot_ip); sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_franka_hand_configuration.thread_sampeling_time_s); sas::get_ros_parameter(node,"default_force",robot_driver_franka_hand_configuration.default_force);