diff --git a/config/franka_gripper_node.yaml b/config/franka_gripper_node.yaml index 8f638ec7f296a126a9eea4c632388553fb980142..687e5d3db2cbfb5afd3c3525e3b6f3efb5ca7180 100644 --- a/config/franka_gripper_node.yaml +++ b/config/franka_gripper_node.yaml @@ -1,5 +1,5 @@ publish_rate: 30 # [Hz] -default_speed: 0.08 # [m/s] +default_speed: 0.1 # [m/s] default_grasp_epsilon: - inner: 0.01 # [m] - outer: 0.01 # [m] + inner: 0.005 # [m] + outer: 0.005 # [m] diff --git a/launch/franka_gripper.launch b/launch/franka_gripper.launch index d56656352efaf03356d7d3a82ccfe528f2003013..92a476e13beb822db3ae127461ec075fac8da087 100644 --- a/launch/franka_gripper.launch +++ b/launch/franka_gripper.launch @@ -1,11 +1,13 @@ <?xml version="1.0" ?> <launch> <arg name="robot_ip" /> + <arg name="stop_at_shutdown" default="false" /> <arg name="arm_id" default="panda" /> <arg name="joint_names" default="[$(arg arm_id)_finger_joint1, $(arg arm_id)_finger_joint2]" /> <node name="franka_gripper" pkg="franka_gripper" type="franka_gripper_node" output="screen"> <param name="robot_ip" value="$(arg robot_ip)"/> + <param name="stop_at_shutdown" value="$(arg stop_at_shutdown)" /> <rosparam command="load" file="$(find franka_gripper)/config/franka_gripper_node.yaml" /> <rosparam param="joint_names" subst_value="true">$(arg joint_names)</rosparam> </node> diff --git a/package.xml b/package.xml index cf1e314dc0bab020281631b534df89bad7ec6e1f..85d48ae9dec30d9ca8a72e171241ba9369ea24e5 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ <?xml version="1.0"?> <package format="2"> <name>franka_gripper</name> - <version>0.7.0</version> + <version>0.8.2</version> <description>This package implements the franka gripper of type Franka Hand for the use in ros</description> <maintainer email="support@franka.de">Franka Emika GmbH</maintainer> <license>Apache 2.0</license> diff --git a/src/franka_gripper.cpp b/src/franka_gripper.cpp index 28647289836bd77c313159b3c89ca23740f22b19..59970d53cfb428a6b24366589109c5400fb2dcc8 100644 --- a/src/franka_gripper.cpp +++ b/src/franka_gripper.cpp @@ -33,7 +33,6 @@ void gripperCommandExecuteCallback( double default_speed, actionlib::SimpleActionServer<control_msgs::GripperCommandAction>* action_server, const control_msgs::GripperCommandGoalConstPtr& goal) { - auto gripper_command_handler = [goal, grasp_epsilon, default_speed, &gripper]() { // HACK: As one gripper finger is <mimic>, MoveIt!'s trajectory execution manager // only sends us the width of one finger. Multiply by 2 to get the intended width. diff --git a/src/franka_gripper_node.cpp b/src/franka_gripper_node.cpp index 3b48a1970e727ed7894d262ee0716ebdc0cda305..fd83ba03d4b678ada0b4893cc66ab74312810a2b 100644 --- a/src/franka_gripper_node.cpp +++ b/src/franka_gripper_node.cpp @@ -152,6 +152,12 @@ int main(int argc, char** argv) { return -1; } + bool stop_at_shutdown(false); + if (!node_handle.getParam("stop_at_shutdown", stop_at_shutdown)) { + ROS_INFO_STREAM("franka_gripper_node: Could not find parameter stop_at_shutdown. Defaulting to " + << std::boolalpha << stop_at_shutdown); + } + franka::GripperState gripper_state; std::mutex gripper_state_mutex; std::thread read_thread([&gripper_state, &gripper, &gripper_state_mutex]() { @@ -188,5 +194,8 @@ int main(int argc, char** argv) { rate.sleep(); } read_thread.join(); + if (stop_at_shutdown) { + gripper.stop(); + } return 0; }