def execute(self, userdata): rospy.loginfo("ErrorHandler executing...") if userdata.gripper_status == "closed": for i in range(100): rospy.logerr( "********** Going to drop the item in 30 seconds **********" ) for i in range(30): rospy.logerr( "********** Going to drop the item in %s seconds **********" % (30 - i)) rospy.sleep(1) while robot_state.is_stopped(): rospy.logwarn("Waiting until e-stop is removed to handle errors") rospy.sleep(1) rospy.loginfo("Removing objects that could be causing problems") remove_object() remove_object("pointcloud_voxels") rospy.loginfo("Setting gripper to open state for safety") gripper.open() userdata.gripper_status = "open" if robot_state.has_error(): rospy.logerr("Robot alarm is set, going home should clear it") if not go_home(shelf=BIN(userdata.bin)): rospy.logfatal("Robot should continue going home forever") return 'Failed' if robot_state.has_error(): rospy.logfatal("Robot alarm is set, should be cleared by now") return 'Continue'
def Generate_traj_for_pnt2pnt(goal_config_set, group_handle): success_num = 0 total_traj_num = len(goal_config_set) * len(goal_config_set) print "GoalConfigNum: ", len( goal_config_set), ", Try to generate ", total_traj_num, "trajectories" if __name__ == '__main__': try: print ">>>> Initializing... >>>>" moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('Trajectory generator', anonymous=True) remove_object() motoman_sda10 = moveit_commander.RobotCommander() arm_left_group = moveit_commander.MoveGroupCommander("arm_left_torso") arm_right_group = moveit_commander.MoveGroupCommander( "arm_right_torso") arm_left_group.allow_replanning(True) arm_right_group.allow_replanning(True) arm_left_group.set_planner_id("RRTConnectkConfigDefault") arm_right_group.set_planner_id("RRTConnectkConfigDefault") arm_init(arm_left_group, arm_right_group) #arm_left_group.set_planner_id("RRTstarkConfigDefault");
def Generate_traj_for_key2pnt(key_config_set, goal_config_set, group_handle): success_num = 0 total_traj_num = len(key_config_set) * len(goal_config_set) print "Key_Config Num: ", len(key_config_set) print "Goal_Config Num: ", len(goal_config_set) print "Try to generate ", total_traj_num, "trajectories" start_pos = key_config_set[0] drop_pos = key_config_set[1] # Goto start pnt group_handle.set_joint_value_target(start_pos.jnt_val) group_handle.go() count = 0 planning_attemps = 0 while (count < len(goal_config_set)): # From start pnt to entrance pnt entrance_goal_config = goal_config_set[count] #if entrance_goal_config.bin_num != "B": # count += 2; # continue; goal_jnt_value_msg = Generate_joint_state_msg( group_handle, entrance_goal_config.jnt_val) group_handle.set_joint_value_target(goal_jnt_value_msg) print ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" print "Now planning for bin", entrance_goal_config.bin_num print "======================================================" print "Planning from start pos to bin", entrance_goal_config.bin_num, entrance_goal_config.pos_property plan = group_handle.plan() planning_attemps = 1 while (len(plan.joint_trajectory.points) == 0 and planning_attemps <= 20): print "Planning Attempts:", planning_attemps plan = group_handle.plan() planning_attemps += 1 if len(plan.joint_trajectory.points): rospy.sleep(5) group_handle.execute(plan) folder_name = os.path.join( os.path.dirname(__file__), "../../trajectories/bin") + entrance_goal_config.bin_num file_name = folder_name + "/" + "forward" Save_traj(file_name, plan) success_num += 1 # From entrance pnt to exit pnt count += 1 exit_goal_config = goal_config_set[count] goal_jnt_value_msg = Generate_joint_state_msg( group_handle, exit_goal_config.jnt_val) group_handle.set_joint_value_target(goal_jnt_value_msg) print "----------------------------------------------" print "From ", entrance_goal_config.bin_num, entrance_goal_config.pos_property, " move to", exit_goal_config.bin_num, exit_goal_config.pos_property group_handle.go() rospy.sleep(3) # Plan from exit pnt to drop print "----------------------------------------------" #Add_collision_ball(); print "Planning from bin", exit_goal_config.bin_num, exit_goal_config.pos_property, "to drop" group_handle.set_joint_value_target(drop_pos.jnt_val) plan = group_handle.plan() count += 1 if len(plan.joint_trajectory.points): rospy.sleep(5) group_handle.execute(plan) file_name = folder_name + "/" + "drop" Save_traj(file_name, plan) success_num += 1 else: print "Planning from bin", exit_goal_config.bin_num, exit_goal_config.pos_property, "to Drop position Failed!" count = count - 2 remove_object() else: print "Planning from Start Position to bin", entrance_goal_config.bin_num, entrance_goal_config.pos_property, " Failed!" count += 2 print "Total success number: ", success_num, "/", total_traj_num
def runTrajectory(req): print "---------------------------------" print req.task print " " print req.bin_num print " " print req.using_torso print "---------------------------------" # Get the trajectory file_root = os.path.join(os.path.dirname(__file__), "../trajectories"); #file_root = os.path.join(os.path.dirname(__file__), "../traj_updates"); traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); plan = RobotTrajectory(); # Gripper Hand if req.task == "use_tray": file_name = file_root + "/left_arm_drop2home"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso"); elif req.task == "use_gripper": file_name = file_root + "/left_arm_home2drop"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso"); elif req.task == "Forward": file_name = file_root+"/bin" + req.bin_num +"/forward"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso"); elif req.task == "Drop": file_name = file_root+"/bin" + req.bin_num+"/drop"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso"); elif req.task == "Scan": file_name = file_root+"/bin" + req.bin_num+"/scan"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso"); # Tray Hand elif req.task == "Pick": file_name = file_root+"/bin" + req.bin_num+"/Pick"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); elif req.task == "Dump": file_name = file_root+"/bin" + req.bin_num+"/Dump"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); elif req.task == "Lift": file_name = file_root+"/bin" + req.bin_num+"/Lift"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); elif req.task == "Home": file_name = file_root+"/bin" + req.bin_num+"/Home"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); elif req.task == "Rotate": file_name = file_root+"/bin" + req.bin_num+"/Rotate"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); else : print "No plan selected, exit!"; return GetTrajectoryResponse(plan,True); f = open(file_name,"r"); buf = f.read(); plan_file = RobotTrajectory(); plan_file.deserialize(buf); plan = copy.deepcopy(plan_file); move_to_start_pos(plan, traj_execute_group); if check_trajectory_validity(plan): print "Trajectory is valid"; # Display Current Trajectory robot = moveit_commander.RobotCommander(); display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory) display_trajectory = moveit_msgs.msg.DisplayTrajectory(); display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while", file_name, " is visualized (again)..." pose = PoseStamped() pose.header.frame_id = "/arm_left_link_7_t" pose.header.stamp = rospy.Time.now() pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = -0.35 pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 #attach_sphere("arm_left_link_7_t", "Object", pose, 0.17, ["hand_left_finger_1_link_2", "hand_left_finger_1_link_3", "hand_left_finger_1_link_3_tip", "hand_left_finger_2_link_2", "hand_left_finger_2_link_3", "hand_left_finger_2_link_3_tip", "hand_left_finger_middle_link_2", "hand_left_finger_middle_link_3", "hand_left_finger_middle_link_3_tip"]); traj_execute_group.execute(plan); print "Trajectory ", file_name, " finished!" f.close(); remove_object(); return GetTrajectoryResponse(plan,True); else: print "Trajectory is not valid! Test failed!"; return GetTrajectoryResponse(plan,True);