From f58e3015cc73c4fcb5756c56ad8f1daba7369cac Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 1 Jun 2023 16:20:06 +0900 Subject: [PATCH 1/2] Make errors from :get-*-result readable when * is not called --- .../euslisp/franka-common-interface.l | 28 +++++++++++++++---- jsk_panda_robot/panda_eus/package.xml | 1 + 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l index 818cbb3fae..1e8ff62c4a 100644 --- a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -1,5 +1,6 @@ (require :robot-interface "package://pr2eus/robot-interface.l") +(ros::roseus-add-msgs "actionlib_msgs") (ros::roseus-add-msgs "franka_msgs") (ros::roseus-add-msgs "franka_gripper") @@ -86,19 +87,23 @@ Arguments specific to franka robot: (sethash arm gripper-grasp-actions (instance ros::simple-action-client :init (format nil "~a/franka_gripper/grasp" prefix) - franka_gripper::GraspAction)) + franka_gripper::GraspAction + :groupname groupname)) (sethash arm gripper-homing-actions (instance ros::simple-action-client :init (format nil "~a/franka_gripper/homing" prefix) - franka_gripper::HomingAction)) + franka_gripper::HomingAction + :groupname groupname)) (sethash arm gripper-move-actions (instance ros::simple-action-client :init (format nil "~a/franka_gripper/move" prefix) - franka_gripper::MoveAction)) + franka_gripper::MoveAction + :groupname groupname)) (sethash arm gripper-stop-actions (instance ros::simple-action-client :init (format nil "~a/franka_gripper/stop"prefix) - franka_gripper::StopAction)))) + franka_gripper::StopAction + :groupname groupname)))) )) (:set-joint-pd-gain (joint-name pgain dgain &key (type :default-controller)) @@ -264,10 +269,21 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka ) (:gripper-action-postprocess (arm actions wait) - (let ((arms (send self :arm2arms arm))) + (let ((arms (send self :arm2arms arm)) (wait-res t)) (if wait (dolist (a arms) - (send self :send-gripper-action-method a actions :wait-for-result))) + (setq wait-res + (and wait-res + (send self :send-gripper-action-method a actions :wait-for-result))))) + (if (not wait-res) + (dolist (a arms) + (if (eq (send self :send-gripper-action-method a actions :get-state) + actionlib_msgs::GoalStatus::*pending*) + (progn + (ros::ros-error + "No goal exists for ~a, send goal to it first (:start-grasp, :stop-grasp, ...)" + (send self :send-gripper-action-method a actions :name)) + (return-from :gripper-action-postprocess nil))))) (case arm (:arms (mapcar #'(lambda (a) diff --git a/jsk_panda_robot/panda_eus/package.xml b/jsk_panda_robot/panda_eus/package.xml index 8bc83b05dd..0867097679 100644 --- a/jsk_panda_robot/panda_eus/package.xml +++ b/jsk_panda_robot/panda_eus/package.xml @@ -22,5 +22,6 @@ franka_gripper franka_control pr2eus + actionlib_msgs From aacbf62a385afabde8612de79ceff12cd33376d9 Mon Sep 17 00:00:00 2001 From: Shun Hasegawa Date: Thu, 1 Jun 2023 16:52:07 +0900 Subject: [PATCH 2/2] Force to call all :wait-for-result and postprocess even if some of them fail --- .../euslisp/franka-common-interface.l | 24 +++++++++++-------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l index 1e8ff62c4a..69f90bf8d9 100644 --- a/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l +++ b/jsk_panda_robot/panda_eus/euslisp/franka-common-interface.l @@ -273,17 +273,21 @@ Details: `ErrorRecoveryAction` part of https://frankaemika.github.io/docs/franka (if wait (dolist (a arms) (setq wait-res - (and wait-res - (send self :send-gripper-action-method a actions :wait-for-result))))) + (and (send self :send-gripper-action-method a actions :wait-for-result) + wait-res)))) (if (not wait-res) - (dolist (a arms) - (if (eq (send self :send-gripper-action-method a actions :get-state) - actionlib_msgs::GoalStatus::*pending*) - (progn - (ros::ros-error - "No goal exists for ~a, send goal to it first (:start-grasp, :stop-grasp, ...)" - (send self :send-gripper-action-method a actions :name)) - (return-from :gripper-action-postprocess nil))))) + (progn + (setq wait-res t) + (dolist (a arms) + (if (eq (send self :send-gripper-action-method a actions :get-state) + actionlib_msgs::GoalStatus::*pending*) + (progn + (ros::ros-error + "No goal exists for ~a, send goal to it first (:start-grasp, :stop-grasp, ...)" + (send self :send-gripper-action-method a actions :name)) + (setq wait-res nil)))) + (if (not wait-res) + (return-from :gripper-action-postprocess nil)))) (case arm (:arms (mapcar #'(lambda (a)