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);