def query_moveit_for_reachability(self, graspit_grasp_msg): """ :type graspit_grasp_msg: graspit_msgs.msg.Grasp """ # self.move_group.set_planning_time(rospy.get_param('~allowed_planning_time')) moveit_grasp_msg = message_utils.graspit_grasp_to_moveit_grasp(graspit_grasp_msg, self.move_group, self.listener, self.grasp_approach_tran_frame) rospy.loginfo("moveit_grasp_msg: " + str(moveit_grasp_msg)) pickup_goal = message_utils.build_pickup_goal(moveit_grasp_msg=moveit_grasp_msg, object_name=graspit_grasp_msg.object_name, planning_group=self.move_group) rospy.loginfo("pickup_goal: " + str(pickup_goal)) received_result = False try: self.pick_plan_client.wait_for_server(rospy.Duration(3)) pickup_goal.planner_id = self.planner_id self.pick_plan_client.send_goal(pickup_goal) received_result = self.pick_plan_client.wait_for_result(rospy.Duration(rospy.get_param('~allowed_planning_time', 5))) except Exception as e: rospy.logerr("failed to reach pick action server with err: %s" % e.message) if received_result: result = self.pick_plan_client.get_result() rospy.loginfo("result: " + str(result)) success = result.error_code.val == result.error_code.SUCCESS else: result = None success = False rospy.loginfo("success of pick_plan_client:" + str(success)) return success, result
def query_moveit_for_reachability(self, graspit_grasp_msg): """ :type graspit_grasp_msg: graspit_msgs.msg.Grasp """ #ipdb.set_trace() #return self.pose_reachability_checker(graspit_grasp_msg.final_grasp_pose, graspit_grasp_msg.object_name) moveit_grasp_msg = message_utils.graspit_grasp_to_moveit_grasp( graspit_grasp_msg, self.move_group, self.listener, self.grasp_approach_tran_frame) self.grasp_dict[moveit_grasp_msg.id] = moveit_grasp_msg rospy.loginfo("moveit_grasp_msg: " + str(moveit_grasp_msg)) if rospy.get_param('/debug', 0) != 0: tf_msg = pm.toTf(pm.fromMsg(moveit_grasp_msg.grasp_pose.pose)) bc = tf.TransformBroadcaster() bc.sendTransform(tf_msg[0], tf_msg[1], rospy.Time.now(), 'moveit_msg_grasp_pose', moveit_grasp_msg.grasp_pose.header.frame_id) tf_msg = pm.toTf(pm.fromMsg(graspit_grasp_msg.pre_grasp_pose)) bc.sendTransform(tf_msg[0], tf_msg[1], rospy.Time.now(), 'graspit_msg_pre_grasp_pose', graspit_grasp_msg.object_name) #moveit_grasp_msg.grasp_pose = self.move_group.get_current_pose() else: pass #moveit_grasp_msg.grasp_pose.pose = graspit_grasp_msg.pre_grasp_pose pickup_goal = message_utils.build_pickup_goal( moveit_grasp_msg=moveit_grasp_msg, object_name=graspit_grasp_msg.object_name, planning_group=self.move_group, plan_only=True) # if not self.has_msg: # self.has_msg = True # rospy.loginfo("Entering IPython") # import IPython # IPython.embed() pick_attempt_succeeded = False #try: # pick_attempt_succeeded = self.move_group.pick(graspit_grasp_msg.object_name, [moveit_grasp_msg]) # #except Exception as e: # rospy.logerr("pickup attempt failed: %s"%(e.message)) #rospy.loginfo("pickup attempt result: %s"%(str(pick_attempt_succeeded))) rospy.loginfo("pickup_goal: " + str(pickup_goal)) received_result = False try: self.pick_plan_client.wait_for_server(rospy.Duration(3)) pickup_goal.planner_id = self.planner_id self.pick_plan_client.send_goal(pickup_goal) received_result = self.pick_plan_client.wait_for_result( rospy.Duration(rospy.get_param('~allowed_planning_time', 20))) except Exception as e: rospy.logerr("failed to reach pick action server with err: %s" % e.message) if received_result: result = self.pick_plan_client.get_result() rospy.loginfo("result: " + str(result)) success = result.error_code.val == result.error_code.SUCCESS else: result = None success = False rospy.loginfo("success of pick_plan_client:" + str(success)) return success, result
def query_moveit_for_reachability(self, graspit_grasp_msg): """ :type graspit_grasp_msg: graspit_msgs.msg.Grasp """ #ipdb.set_trace() #return self.pose_reachability_checker(graspit_grasp_msg.final_grasp_pose, graspit_grasp_msg.object_name) moveit_grasp_msg = message_utils.graspit_grasp_to_moveit_grasp(graspit_grasp_msg, self.move_group, self.listener, self.grasp_approach_tran_frame) self.grasp_dict[moveit_grasp_msg.id] = moveit_grasp_msg rospy.loginfo("moveit_grasp_msg: " + str(moveit_grasp_msg)) if rospy.get_param('/debug', 0) != 0: tf_msg = pm.toTf(pm.fromMsg(moveit_grasp_msg.grasp_pose.pose)) bc = tf.TransformBroadcaster() bc.sendTransform(tf_msg[0], tf_msg[1], rospy.Time.now(), 'moveit_msg_grasp_pose', moveit_grasp_msg.grasp_pose.header.frame_id) tf_msg = pm.toTf(pm.fromMsg(graspit_grasp_msg.pre_grasp_pose)) bc.sendTransform(tf_msg[0], tf_msg[1], rospy.Time.now(), 'graspit_msg_pre_grasp_pose', graspit_grasp_msg.object_name) #moveit_grasp_msg.grasp_pose = self.move_group.get_current_pose() else: pass #moveit_grasp_msg.grasp_pose.pose = graspit_grasp_msg.pre_grasp_pose pickup_goal = message_utils.build_pickup_goal(moveit_grasp_msg=moveit_grasp_msg, object_name=graspit_grasp_msg.object_name, planning_group=self.move_group, plan_only=True) # if not self.has_msg: # self.has_msg = True # rospy.loginfo("Entering IPython") # import IPython # IPython.embed() pick_attempt_succeeded = False #try: # pick_attempt_succeeded = self.move_group.pick(graspit_grasp_msg.object_name, [moveit_grasp_msg]) # #except Exception as e: # rospy.logerr("pickup attempt failed: %s"%(e.message)) #rospy.loginfo("pickup attempt result: %s"%(str(pick_attempt_succeeded))) rospy.loginfo("pickup_goal: " + str(pickup_goal)) received_result = False try: self.pick_plan_client.wait_for_server(rospy.Duration(3)) pickup_goal.planner_id = self.planner_id self.pick_plan_client.send_goal(pickup_goal) received_result = self.pick_plan_client.wait_for_result(rospy.Duration(rospy.get_param('~allowed_planning_time', 20))) except Exception as e: rospy.logerr("failed to reach pick action server with err: %s" % e.message) if received_result: result = self.pick_plan_client.get_result() rospy.loginfo("result: " + str(result)) success = result.error_code.val == result.error_code.SUCCESS else: result = None success = False rospy.loginfo("success of pick_plan_client:" + str(success)) return success, result