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