def _reset_parameter(self):
self.state_init = False
self.ggcnn_init = False
self.goal_set = False
self.servo_goal = ServoToPoseGoal()
self.error_code = 0
self.ggcnn_xP_arr = []
self.ggcnn_yP_arr = []
self.ggcnn_zP_arr = []
self.ggcnn_xO_arr = []
self.ggcnn_yO_arr = []
self.ggcnn_zO_arr = []
self.ggcnn_wO_arr = []
self.xP = None
self.yP = None
self.zP = None
self.P_off = None
self.xP_off = None
self.yP_off = None
self.zP_off = None
self.grab_xP = None
self.grab_yP = None
self.grab_zP = None
self.grab_xO = None
self.grab_yO = None
self.grab_zO = None
self.grab_wO = None
self.grab_width = None
self.grab_quality = None
def _reset_parameter(self):
self.init_state = False
self.init_depth = False
self.init_hand = False
self.init_darknet = False
self.goal_set = False
self.depth = None
self.depth_nan = None
self.mask_hand = None
self.best_box = None
self.last_image_pose = None
self.servo_goal = ServoToPoseGoal()
self.error_code = 0
self.xO = None
self.yO = None
self.zO = None
self.wO = None
def _set_goal(self):
# Define new goal
self.servo_goal = ServoToPoseGoal()
self.servo_goal.stamped_pose.header.frame_id = 'panda_link0'
self.servo_goal.stamped_pose.pose.position.x = self.grab_xP
self.servo_goal.stamped_pose.pose.position.y = self.grab_yP
self.servo_goal.stamped_pose.pose.position.z = self.grab_zP
self.servo_goal.stamped_pose.pose.orientation.x = self.grab_xO
self.servo_goal.stamped_pose.pose.orientation.y = self.grab_yO
self.servo_goal.stamped_pose.pose.orientation.z = self.grab_zO
self.servo_goal.stamped_pose.pose.orientation.w = self.grab_wO
self.servo_goal.scaling = self.scaling_handover
# Send goal
self.servo_pose_client.send_goal(self.servo_goal)
self.goal_set = True
print("MOVE - Goal:")
print(self.servo_goal.stamped_pose.pose)
# Publish result
if (self.visualization_type):
self.pub_visualization.publish(Empty())
def __init__(self):
# Parameter
self.state_move = 'startup'
self.init_depth = False
self.init_body = False
self.init_hand = False
self.state_init = False
self.ggcnn_init = False
self.goal_set = False
self.depth_topic = rospy.get_param('/human_robot_handover_ros/camera/depth')
self.bodyparts_topic = rospy.get_param('/human_robot_handover_ros/subscription/bodyparts')
self.egohands_topic = rospy.get_param('/human_robot_handover_ros/subscription/egohands')
self.ggcnn_topic = rospy.get_param('/human_robot_handover_ros/ggcnn/topic')
self.ggcnn_window = rospy.get_param('/human_robot_handover_ros/ggcnn/window')
self.ggcnn_dev_pos = rospy.get_param('/human_robot_handover_ros/ggcnn/deviation_position')
self.ggcnn_dev_orient = rospy.get_param('/human_robot_handover_ros/ggcnn/deviation_orientation')
self.arm_state = rospy.get_param('/human_robot_handover_ros/robot/arm_state')
self.arm_servo_pose = rospy.get_param('/human_robot_handover_ros/robot/arm_servo_pose')
self.arm_named_pose = rospy.get_param('/human_robot_handover_ros/robot/arm_named_pose')
self.arm_gripper = rospy.get_param('/human_robot_handover_ros/robot/arm_gripper')
self.dist_ggcnn = rospy.get_param('/human_robot_handover_ros/movement/dist_ggcnn')
self.dist_final = rospy.get_param('/human_robot_handover_ros/movement/dist_final')
self.speed_approach = rospy.get_param('/human_robot_handover_ros/movement/speed_approach')
self.scaling_handover = rospy.get_param('/human_robot_handover_ros/movement/scaling_handover')
self.gripper_open = rospy.get_param('/human_robot_handover_ros/gripper/gripper_open')
self.gripper_closed = rospy.get_param('/human_robot_handover_ros/gripper/gripper_closed')
self.visualization_type = rospy.get_param('/human_robot_handover_ros/visualization/activated')
self.visualization_topic = rospy.get_param('/human_robot_handover_ros/visualization/topic')
self.depth = None
self.mask_body = None
self.mask_hand = None
self.servo_goal = ServoToPoseGoal()
self.error_code = 0
self.ggcnn_xP_arr = []
self.ggcnn_yP_arr = []
self.ggcnn_zP_arr = []
self.ggcnn_xO_arr = []
self.ggcnn_yO_arr = []
self.ggcnn_zO_arr = []
self.ggcnn_wO_arr = []
self.xP = None
self.yP = None
self.zP = None
self.P_off = None
self.xP_off = None
self.yP_off = None
self.zP_off = None
self.grab_xP = None
self.grab_yP = None
self.grab_zP = None
self.grab_xO = None
self.grab_yO = None
self.grab_zO = None
self.grab_wO = None
self.grab_width = None
self.grab_quality = None
# Init
self.bridge = cv_bridge.CvBridge()
# Robot interface
self.servo_pose_client = actionlib.SimpleActionClient(self.arm_servo_pose, ServoToPoseAction)
self.servo_pose_client.wait_for_server()
print("STARTUP -> named_pose_client OK")
self.named_pose_client = actionlib.SimpleActionClient(self.arm_named_pose, MoveToNamedPoseAction)
self.named_pose_client.wait_for_server()
print("STARTUP -> named_pose_client OK")
self.gripper_client = actionlib.SimpleActionClient(self.arm_gripper, ActuateGripperAction)
self.gripper_client.wait_for_server()
print("STARTUP -> gripper_client OK")
# Subsriber
rospy.Subscriber(self.depth_topic, Image, self._callback_depth, queue_size=1)
rospy.Subscriber(self.bodyparts_topic, CompressedImage, self._callback_bodyparts, queue_size=1)
rospy.Subscriber(self.egohands_topic, CompressedImage, self._callback_egohands, queue_size=1)
rospy.Subscriber(self.ggcnn_topic, GraspPrediction, self._callback_ggcnn, queue_size=1)
rospy.Subscriber(self.arm_state, ManipulatorState, self._callback_state, queue_size=1)
# Services
self.recover = rospy.ServiceProxy('/arm/recover', EmptyRequest)
# Visualization
if (self.visualization_type):
self.pub_visualization = rospy.Publisher(self.visualization_topic, Empty, queue_size=1)
self.pub_debug = rospy.Publisher('handover/DEBUG', Image, queue_size=1)
rospy.Subscriber(self.visualization_topic, Empty, self._callback_visualization, queue_size=1)
def __init__(self):
# Class for preparations
self.prep = Preparations()
# Parameter config
self.topic_depth = rospy.get_param('/yolo_handover/subscription/depth')
self.topic_darknet = rospy.get_param('/yolo_handover/subscription/darknet')
self.topic_egohands = rospy.get_param('/yolo_handover/subscription/egohands')
self.topic_arm = rospy.get_param('/yolo_handover/robot/arm_state')
self.arm_servo_pose = rospy.get_param('/yolo_handover/robot/arm_servo_pose')
self.arm_named_pose = rospy.get_param('/yolo_handover/robot/arm_named_pose')
self.arm_gripper = rospy.get_param('/yolo_handover/robot/arm_gripper')
self.dist_ignore = rospy.get_param('/yolo_handover/movement/dist_ignore')
self.speed_approach = rospy.get_param('/yolo_handover/movement/speed_approach')
self.scaling_handover = rospy.get_param('/yolo_handover/movement/scaling_handover')
self.gripper_open = rospy.get_param('/yolo_handover/gripper/gripper_open')
self.gripper_closed = rospy.get_param('/yolo_handover/gripper/gripper_closed')
self.visualization_type = rospy.get_param('/human_robot_handover_ros/visualization/activated')
self.visualization_topic = rospy.get_param('/human_robot_handover_ros/visualization/topic')
# States
self.state_move = 'startup'
self.init_state = False
self.init_depth = False
self.init_hand = False
self.init_darknet = False
self.goal_set = False
# Variables
self.depth = None
self.depth_nan = None
self.mask_hand = None
self.best_box = None
self.last_image_pose = None
self.servo_goal = ServoToPoseGoal()
self.error_code = 0
self.xO = None
self.yO = None
self.zO = None
self.wO = None
# Init
self.bridge = cv_bridge.CvBridge()
self.last_image_pose = None
# Robot interface
self.servo_pose_client = actionlib.SimpleActionClient(self.arm_servo_pose, ServoToPoseAction)
self.servo_pose_client.wait_for_server()
print("STARTUP -> named_pose_client OK")
self.named_pose_client = actionlib.SimpleActionClient(self.arm_named_pose, MoveToNamedPoseAction)
self.named_pose_client.wait_for_server()
print("STARTUP -> named_pose_client OK")
self.gripper_client = actionlib.SimpleActionClient(self.arm_gripper, ActuateGripperAction)
self.gripper_client.wait_for_server()
print("STARTUP -> gripper_client OK")
# Subsriber
rospy.Subscriber(self.topic_arm, ManipulatorState, self._callback_state, queue_size=1)
rospy.Subscriber(self.topic_depth, Image, self._callback_depth, queue_size=1)
rospy.Subscriber(self.topic_darknet, BoundingBoxes, self._callback_darknet, queue_size=1)
rospy.Subscriber(self.topic_egohands, CompressedImage, self._callback_egohands, queue_size=1)
# Visualization
if (self.visualization_type):
self.pub_visualization = rospy.Publisher(self.visualization_topic, Empty, queue_size=1)
rospy.Subscriber(self.visualization_topic, Empty, self._callback_visualization, queue_size=1)
def __init__(self):
# Parameter
self.state_move = 'startup'
self.state_init = False
self.ggcnn_init = False
self.goal_set = False
self.ggcnn_topic = rospy.get_param(
'/human_robot_handover_ros/ggcnn/topic')
self.ggcnn_window = rospy.get_param(
'/human_robot_handover_ros/ggcnn/window')
self.ggcnn_dev_pos = rospy.get_param(
'/human_robot_handover_ros/ggcnn/deviation_position')
self.ggcnn_dev_orient = rospy.get_param(
'/human_robot_handover_ros/ggcnn/deviation_orientation')
self.arm_state = rospy.get_param(
'/human_robot_handover_ros/robot/arm_state')
self.arm_servo_pose = rospy.get_param(
'/human_robot_handover_ros/robot/arm_servo_pose')
self.arm_named_pose = rospy.get_param(
'/human_robot_handover_ros/robot/arm_named_pose')
self.arm_gripper = rospy.get_param(
'/human_robot_handover_ros/robot/arm_gripper')
self.move_x = rospy.get_param(
'/human_robot_handover_ros/movement/move_x')
self.move_y = rospy.get_param(
'/human_robot_handover_ros/movement/move_y')
self.move_z = rospy.get_param(
'/human_robot_handover_ros/movement/move_z')
self.dist_ggcnn = rospy.get_param(
'/human_robot_handover_ros/movement/dist_ggcnn')
self.dist_final = rospy.get_param(
'/human_robot_handover_ros/movement/dist_final')
self.move_final = rospy.get_param(
'/human_robot_handover_ros/movement/move_final')
self.speed_approach = rospy.get_param(
'/human_robot_handover_ros/movement/speed_approach')
self.scaling_handover = rospy.get_param(
'/human_robot_handover_ros/movement/scaling_handover')
self.gripper_open = rospy.get_param(
'/human_robot_handover_ros/gripper/gripper_open')
self.visualization_type = rospy.get_param(
'/human_robot_handover_ros/visualization/activated')
self.visualization_topic = rospy.get_param(
'/human_robot_handover_ros/visualization/topic')
self.servo_goal = ServoToPoseGoal()
self.error_code = 0
self.ggcnn_xP_arr = []
self.ggcnn_yP_arr = []
self.ggcnn_zP_arr = []
self.ggcnn_xO_arr = []
self.ggcnn_yO_arr = []
self.ggcnn_zO_arr = []
self.ggcnn_wO_arr = []
self.xP = None
self.yP = None
self.zP = None
self.P_off = None
self.xP_off = None
self.yP_off = None
self.zP_off = None
self.grab_xP = None
self.grab_yP = None
self.grab_zP = None
self.grab_xO = None
self.grab_yO = None
self.grab_zO = None
self.grab_wO = None
self.grab_width = None
self.grab_quality = None
# Robot interface
self.servo_pose_client = actionlib.SimpleActionClient(
self.arm_servo_pose, ServoToPoseAction)
self.servo_pose_client.wait_for_server()
print("STARTUP -> named_pose_client OK")
self.named_pose_client = actionlib.SimpleActionClient(
self.arm_named_pose, MoveToNamedPoseAction)
self.named_pose_client.wait_for_server()
print("STARTUP -> named_pose_client OK")
self.gripper_client = actionlib.SimpleActionClient(
self.arm_gripper, ActuateGripperAction)
self.gripper_client.wait_for_server()
print("STARTUP -> gripper_client OK")
# Subsriber
rospy.Subscriber(self.arm_state,
ManipulatorState,
self._callback_state,
queue_size=1)
rospy.Subscriber(self.ggcnn_topic,
GraspPrediction,
self._callback_ggcnn,
queue_size=1)
# Visualization
if (self.visualization_type):
self.pub_visualization = rospy.Publisher(self.visualization_topic,
Empty,
queue_size=1)
rospy.Subscriber(self.visualization_topic,
Empty,
self._callback_visualization,
queue_size=1)