def default_state_gripper(grp): rospy.rosinfo("Openning Gripper") joint_vals = grp.get_current_joint_values() joint_vals[0] = 0.0 grp.set_joint_value_target(joint_vals) init_plan = grp.plan() return grp.execute(init_plan)
def callback(goal): global available_goal # sets a new timestamp, but otherwise sends the same message goal.target_pose.header.stamp = rospy.Time.now() rosinfo("Sending new goal (%s, %s)", goal.target_pose.pose.position.x, goal.target_pose.pose.position.y) client.send_goal(goal) available_goal.release()
def check_targets(robot, scene, number, distance): rospy.rosinfo("Creating Boxes!") def al(type): if type == 'x': return random.uniform(0.35, 0.65) return random.uniform(-0.7, 0.7) hemme = [] cnt = 0 while len(hemme) != number: if cnt == 200: hemme = [] cnt = 0 cnt = cnt + 1 box = PoseStamped() box.header.frame_id = robot.get_planning_frame() box.pose.position.z = 0.05 box.pose.position.x = al('x') box.pose.position.y = al('y') flag = 1 for i in hemme: if abs(i.pose.position.x - box.pose.position.x) < distance or abs( i.pose.position.y - box.pose.position.y) < distance: flag = 0 if flag == 0: continue hemme.append(box) cnt = 0 names = [] for i in hemme: now = "part" + str(cnt) cnt = cnt + 1 names.append(now) add_object(name=now, x=i.pose.position.x, y=i.pose.position.y, z=i.pose.position.z, d1=0.1, d2=0.1, d3=0.1, robot=robot, scene=scene, typ=0) print "End Check!" return hemme
def pick_object(group, part_index, robot, scene): rospy.rosinfo("Pick Operation starts!") pos = copy.deepcopy(target_poses[part_index]) pos.pose.position.z += 0.22 pos.pose.orientation.y = 1 group.set_pose_target(pos) """ pl = group.plan() state =group.execute(pl) """ move_plan = group.plan() state = group.execute(move_plan) rospy.rosinfo("Execute operation for Object is %s" % str(part_index)) if (state): closed_state_gripper(robot, "part" + str(part_index)) rospy.sleep(1.0) place_object(group, part_index, robot, scene) return else: return
def closed_state_gripper(robot, obj): rospy.rosinfo("Closing Gripper") def convert(width): return 0.77 - width / 0.15316 * 0.77 width = scene.get_objects([obj])[obj].primitives[0].dimensions[0] width = convert(width) now = robot.endeffector.get_current_joint_values()[0] cnt = 0 while abs(now - width) > 0.0001: now = robot.endeffector.get_current_joint_values()[0] cnt = cnt + 1 tmp = width - abs(now - width) / 2.0 robot.endeffector.set_joint_value_target('finger_joint', tmp) robot.endeffector.go() rospy.sleep(0.05) if cnt == 7: break rospy.sleep(1.0) ret = robot.manipulator.attach_object(obj) return ret
def send_goal(goal): rosinfo("Sending new goal (%s, %s)", goal.target_pose.pose.position.x, goal.target_pose.pose.position.y) client.send_goal(goal)
if __name__ == '__main__': # Initializing rospy.init_node('goal_control') pub = rospy.Publisher(STATE_TOPIC, String, queue_size=10) rospy.Subscriber(LOW_BATTERY_TOPIC, MoveBaseGoal, low_battery_callback) rospy.Subscriber(USER_INPUT_TOPIC, MoveBaseGoal, user_input_callback) rospy.Subscriber(PATROL_TOPIC, MoveBaseGoal, patrol_callback) client = actionlib.SimpleActionClient('move_base', MoveBaseAction) # Wait for connection while (not client.wait_for_server( rospy.Duration.from_sec(5.0))) and not rospy.is_shutdown(): rosinfo("Waiting for the move_base action server to come up") rosinfo("Connected") # Give feedback to user while not rospy.is_shutdown(): # Wait for a goal while current_priority == -1 and not rospy.is_shutdown(): sleep(0.5) send_state(WAITING) client.wait_for_result() if (client.get_state() == actionlib.GoalStatus.SUCCEEDED): send_state(GOAL_SUCCESS) set_priority(NO_GOAL_PRIORITY) rosinfo("Hooray, the base moved to the goal position")
def clean_scene(scene): rospy.rosinfo("Clearing the Scene") scene.remove_world_object()
target_poses.append(add_object(name="part2",x=0.627323786056,y=-0.232866048374,z=0.05,robot=robot,scene=scene,d1=0.1,d2=0.1,d3=0.1,typ=0)) """ """ target_poses.append(add_object(name="part0",x=0.605268654986,y=0.23264250667,z=0.05,robot=robot,scene=scene,d1=0.1,d2=0.1,d3=0.1,typ=0)) target_poses.append(add_object(name="part1",x=0.503392917607,y=0.465848992641,z=0.05,robot=robot,scene=scene,d1=0.1,d2=0.1,d3=0.1,typ=0)) target_poses.append(add_object(name="part2",x=0.394430959068,y=-0.104107965218,z=0.05,robot=robot,scene=scene,d1=0.1,d2=0.1,d3=0.1,typ=0)) """ """ target_poses.append(add_object(name="part0",x=0.645338374002,y=-0.073414667672,z=0.05,robot=robot,scene=scene,d1=0.1,d2=0.1,d3=0.1,typ=0)) target_poses.append(add_object(name="part1",x=0.368566960491,y=0.494131345785,z=0.05,robot=robot,scene=scene,d1=0.1,d2=0.1,d3=0.1,typ=0)) target_poses.append(add_object(name="part2",x=0.542541262891,y=0.111709389047,z=0.05,robot=robot,scene=scene,d1=0.1,d2=0.1,d3=0.1,typ=0)) """ rospy.sleep(1) # Try until all objects placed-- j = 1 while (len(picked) != len(target_poses)): rospy.rosinfo("Try: " + str(j)) arm.set_planning_time(5.0) #arm.allow_replanning(True) arm.set_num_planning_attempts(1) #Pick and place every object for i in xrange(len(target_poses)): if i not in picked: pick_object(group=arm, part_index=i, robot=robot, scene=scene) j = j + 1 rospy.spin() roscpp_shutdown()