def on_transition2(self, goal_handle):
result = msgPickPlaceResult()
index = 0
for i in self._goal_handles2:
if self._goal_handles2[i] == goal_handle:
index = i
break
rospy.loginfo(
"Transition Callback.From pick action Client Goal Handle #: " +
str(index))
rospy.loginfo("Comm. State: " + str(goal_handle.get_comm_state()))
rospy.loginfo("Goal Status: " + str(goal_handle.get_goal_status()))
# Comm State - Monitors the State Machine of the Client which is different from Server's
# Comm State = 2 -> Active
# Comm State = 3 -> Wating for Result
# Comm State = 7 -> Done
# if (Comm State == ACTIVE)
if goal_handle.get_comm_state() == 2:
rospy.loginfo(str(index) + ": Goal just went active.")
# if (Comm State == DONE)
if goal_handle.get_comm_state() == 7:
rospy.loginfo(str(index) + ": Goal is DONE")
rospy.loginfo(goal_handle.get_terminal_state())
# get_result() gets the result produced by the Action Server
result = goal_handle.get_result()
rospy.loginfo(result)
if (result.success == True):
if result.state == 0:
res = json.loads(result.result)
sen = msgPickPlaceResult()
for i in res:
sen.result = json.dumps(res[i])
sen.success = True
sen.state = 0
self.send_goal_iot(sen)
else:
rospy.loginfo(
"Goal successfully completed. Client Goal Handle #: " +
str(index))
self.send_goal_place(result)
self.send_goal_iot(result)
else:
rospy.loginfo("Goal failed. Client Goal Handle #: " +
str(index))
def on_goal(self, goal):
print("new goal recieved from place action server", goal)
res = msgPickPlaceResult()
res.result = goal.message
res.state = 1
res.success = True
self.send_goal_iot(res)
self._ac3.set_succeeded(res)
def done_callback(self, status, result):
rospy.loginfo(
'The goal has been completed by the pick simple action server')
rospy.loginfo(result)
rospy.logwarn(self._processing_goal)
rospy.logwarn(self._goal_info)
index = 0
for i in self._goal_info:
if self._goal_info[i] == self._processing_goal:
index = i
break
self._processing_goal = ''
self._processing_goal_handle = ''
if (result.success == True):
self._goal_info.pop(index)
if result.state == 0:
res = json.loads(result.result)
sen = msgPickPlaceResult()
for i in res:
sen.result = json.dumps(res[i])
sen.success = True
sen.state = 0
self.send_goal_iot(sen)
else:
rospy.loginfo(
"Goal successfully completed by pick action server. Client Goal Handle #: "
+ str(index))
y = sorted(self._goal_info)
if self._goal_info != {}:
self.check_goal_pick_place(self._goal_info[y[0]])
self.send_goal_iot(result)
def on_goal(self, goal):
rospy.loginfo("Received new goal from Pick Action Client, This is pick Action Server")
rospy.loginfo(goal)
goal = goal.message
if goal != 'None':
x = json.loads(goal)
# self.count=self.count+1
# self.goal_info.update({self.count:x})
# self._goal_handles.update({self.count:goal_handle})
if goal == 'None':
# goal_handle.set_accepted()
stored_info = self.stored_in
parameters={}
i=0
if stored_info != {}:
for j in stored_info:
i=i+1
parameters[i] = {'id':'Inventory','Team Id':'VB#0620','Unique Id':'FAAMAMYU','SKU':stored_info[j][0].upper()+str(j)+self.sku(),'Item':self.item[str(stored_info[j])],'Priority':self.priority[stored_info[j]],'Storage Number':'R'+str(int(j[0])-1)
+' '
+'C'+str(int(j[1])-1),
"Cost":self.cost[stored_info[j]],"Quantity":'1'}
parameters = json.dumps(parameters)
feed = msgPickPlaceResult()
feed.result = parameters
feed.state = 0
feed.success = True
self._as.set_succeeded(feed)
lst_joint_angles_1 = [0.13683998732796088, -2.4423682116751824, -1.0174361654883883, -1.2520121813310645, 1.57071913195598, 0.13659848941419117]
self.ur5_1.hard_set_joint_angles(lst_joint_angles_1,5)
self.ur5_2.hard_set_joint_angles(lst_joint_angles_1,5)
else:
items=['package21','package02','package01','package10','package11','package12','package22','package20','package00']
#to move to the initial home position
# lst_joint_angles_1 = [0.13683998732796088, -2.4423682116751824, -1.0174361654883883, -1.2520121813310645, 1.57071913195598, 0.13659848941419117]
#
# self.ur5_2.hard_set_joint_angles(lst_joint_angles_1,5)
result = msgPickPlaceResult()
# if x['item'] == 'Medicine':
#
#
# # goal_handle.set_accepted()
# key = self.camera_info['red'][0]
#
# self.ur5_1.key_play(key)
#
# self.camera_info['red'].remove(key)
# self.goal_info.pop(self.count)
# self._goal_handles.pop(self.count)
# result.success = True
# result.result = parameters
# result.state = 2
# goal_handle.set_succeeded(result)
#
# elif len(self._goal_handles) <=1:
# goal_handle.set_accepted()
key = self.camera_info[self.colour[x['item']]][0]
self.ur5_1.key_play1(key)
self.ur5_1.key_play2(key)
parameters = {'id':'OrdersDispatched','Team Id':'VB#0620','Unique Id':'FAAMAMYU','Order ID':x['order_id'],'Item':x['item'],'Priority':self.priority[self.colour[x['item']]],
"Dispatch Quantity":'1','City':x['city'],'Longitude':x['lon'],'Latitude':x['lat'],'Cost':self.cost[self.colour[x['item']]],'Dispatch Status':'Yes','Dispatch Date and Time':self.get_time_str()}
self.count_x = self.count_x + 1
self.ur5_2.x.update({self.count_x:parameters})
parameters = json.dumps(parameters)
self.camera_info[self.colour[x['item']]].remove(key)
# self.goal_info.pop(self.count)
# self._goal_handles.pop(self.count)
result.success = True
result.result = parameters
result.state = 2
self._as.set_succeeded(result)
# else:
#
# for j in [1,2,3]:
#
# for i in self.goal_info.keys():
#
# goal = self._goal_handles[i].get_goal()
# x = json.loads(goal)
#
# parameters = {'id':'OrdersDispatched','Team ID':'Vb#0620','Unique Id':'FAAMAMYU','Order ID':x['order_id'],'Dispatch Date and Time':x['order_time'],'Item':x['item'],'Priority':self.priority[self.colour[x['item']]],
# "Dispatch Quantity":'1','City':x['city'],'Longitude':x['lon'],'Latitude':x['lat'],'Cost':self.cost[self.colour[x['item']]],'Dispatch Status':'Yes'}
# parameters = json.dumps(parameters)
#
# if j == 1 and self.goal_info[i]['item'] == 'Medicine':
#
# self._goal_handles[i].set_accepted()
# key = self.camera_info['red'][0]
#
# self.ur5_1.key_play(key)
#
# # feed = msgPickPlaceFeedback()
# # feed.feedback = json.dumps(self.goal_info[i])
# # feed.state = 1
# # self._as.publish_feedback(2,feed)
#
# self.camera_info['red'].remove(key)
# self.goal_info.pop(i)
# self._goal_handles.pop(i)
# result.success = True
# result.result = parameters
# result.state = 2
#
# self._goal_handles[i].set_succeeded(result)
#
# elif j == 2 and self.goal_info[i]['item'] == 'Food':
#
# self._goal_handles[i].set_accepted()
# key = self.camera_info['yellow'][0]
#
# self.ur5_1.key_play(key)
#
# # feed = msgPickPlaceFeedback()
# # feed.feedback = json.dumps(self.goal_info[i])
# # feed.state = 1
# # self._as.publish_feedback(2,feed)
#
# self.camera_info['yellow'].remove(key)
# self.goal_info.pop(i)
# self._goal_handles.pop(i)
# result.success = True
# result.result = parameters
# result.state = 2
# self._goal_handles[i].set_succeeded(result)
#
# elif j == 3 and self.goal_info[i]['item'] == 'Clothes':
#
# self._goal_handles[i].set_accepted()
# key = self.camera_info['green'][0]
#
# self.ur5_1.key_play(key)
#
# # feed = msgPickPlaceFeedback()
# # feed.feedback = json.dumps(self.goal_info[i])
# # feed.state = 1
# # self._as.publish_feedback(2,feed)
#
# self.camera_info['green'].remove(key)
# self.goal_info.pop(i)
# self._goal_handles.pop(i)
# result.success = True
# result.result = parameters
# result.state = 2
# self._goal_handles[i].set_succeeded(result)
print('goal completed by pick action server')
def box_sorter(self,box_id,new_pose_infox,new_pose_infoy):
ee_pose=self._group.get_current_pose().pose
#to get the distance the EE should move to grasp the box
trans_x=ee_pose.position.x-new_pose_infox
trans_y=ee_pose.position.y-new_pose_infoy
self.hard_ee_cartesian_translation(-trans_x,0,0,0.01,5)
boolv=(self.attach_detach_gazebo('attach'))
while boolv.result == False:
boolv=(self.attach_detach_gazebo('attach'))
print(boolv)
results = msgPickPlaceResult()
y = sorted(self.x)
x = self.x[y[0]]
self.x.pop(y[0])
print('printing x',x)
#check the diffrent boxes and put them into their respective bins and then return to the home position
if box_id=='red':
# self.hard_ee_cartesian_translation((0.044638+0.7),(0.713571-0.3),0,0.7,5)
lst_joint_angles_1 = [-1.4925768075228811, -2.290621254092094, -1.296816231494799, -1.1248335293912657, 1.5712629130319442, -1.4927827292724123]
self.hard_set_joint_angles(lst_joint_angles_1,5)
self.attach_detach_gazebo('detach')
# self.ee_cartesian_translation(-(0.044638+0.8),-(0.713571),0,1)
elif box_id=='yellow':
lst_joint_angles_1 = [-0.2256,
0,
0,
-1.5700,
-1.5794,
0]
self.hard_set_joint_angles(lst_joint_angles_1,5)
self.attach_detach_gazebo('detach')
elif box_id=='green':
lst_joint_angles_1 = [1.6453901250740568, -2.243975595151765, -1.378693271924699, -1.0897202799753245, 1.5707963586574083, 1.6453901250739564]
self.hard_set_joint_angles(lst_joint_angles_1,5)
self.attach_detach_gazebo('detach')
# results.result = parameters
# results.state = 3
# results.success = True
parameter = {'id':'OrdersShipped','Team Id':'VB#0620','Unique Id':'FAAMAMYU','Order ID':x['Order ID'],'Item':x['Item'],'Priority':self.priority[self.colour[x['Item']]],
"Shipped Quantity":'1','City':x['City'],'Cost':self.cost[self.colour[x['Item']]],'Shipped Status':'Yes','Shipped Date and Time':self.get_time_str(),'Estimated Time of Delivery': self.estimated_time_delivery(x['Item'])}
parameters = json.dumps(parameter)
print('paraaaaaaaaaaaaaaaaaaaaaa',parameters)
goal = msgPickPlaceGoal()
goal.message = parameters
self._sas.send_goal(goal,done_cb = self.done_callback)
print('resultsssssssssssssssssssssssssssss',goal)
lst_joint_angles_1 = [0.13683998732796088, -2.4423682116751824, -1.0174361654883883, -1.2520121813310645, 1.57071913195598, 0.13659848941419117]
self.hard_set_joint_angles(lst_joint_angles_1,5)
def box_sorter(self, box_id, new_pose_infox, new_pose_infoy):
ee_pose = self._group.get_current_pose().pose
#to get the distance the EE should move to grasp the box
trans_x = ee_pose.position.x - new_pose_infox
trans_y = ee_pose.position.y - new_pose_infoy
self.hard_ee_cartesian_translation(-trans_x, 0, 0, 0.01, 5)
boolv = (self.attach_detach_gazebo('attach'))
while boolv.result == False:
boolv = (self.attach_detach_gazebo('attach'))
print(boolv)
results = msgPickPlaceResult()
print('printing x', self.x)
parameter = {
'id': 'OrdersShipped',
'Team Id': 'VB#0620',
'Unique Id': 'FAAMAMYU',
'Order ID': self.x['Order ID'],
'Item': self.x['Item'],
'Priority': self.priority[self.colour[self.x['Item']]],
"Shipped Quantity": '1',
'City': self.x['City'],
'Cost': self.cost[self.colour[self.x['Item']]],
'Shipped Status': 'Yes'
}
parameters = json.dumps(parameter)
print('paraaaaaaaaaaaaaaaaaaaaaa', parameters)
#check the diffrent boxes and put them into their respective bins and then return to the home position
if box_id == 'red':
self.hard_ee_cartesian_translation((0.044638 + 0.7),
(0.713571 - 0.3), 0, 0.7, 5)
self.attach_detach_gazebo('detach')
# self.ee_cartesian_translation(-(0.044638+0.8),-(0.713571),0,1)
elif box_id == 'yellow':
lst_joint_angles_1 = [-0.2256, 0, 0, -1.5700, -1.5794, 0]
self.hard_set_joint_angles(lst_joint_angles_1, 5)
self.attach_detach_gazebo('detach')
elif box_id == 'green':
lst_joint_angles_1 = [
1.6453901250740568, -2.243975595151765, -1.378693271924699,
-1.0897202799753245, 1.5707963586574083, 1.6453901250739564
]
self.hard_set_joint_angles(lst_joint_angles_1, 5)
self.attach_detach_gazebo('detach')
results.result = parameters
results.state = 3
results.success = True
print('resultsssssssssssssssssssssssssssss', results)
self._goal_handle.set_succeeded(results)
lst_joint_angles_1 = [
0.13683998732796088, -2.4423682116751824, -1.0174361654883883,
-1.2520121813310645, 1.57071913195598, 0.13659848941419117
]
self.hard_set_joint_angles(lst_joint_angles_1, 5)
def on_transition2(self, goal_handle):
result = msgPickPlaceResult()
print(goal_handle, type(goal_handle))
index = 0
for i in self._goal_handles2:
if self._goal_handles2[i] == goal_handle:
index = i
break
rospy.loginfo(
"Transition Callback.From pick action Client Goal Handle #: " +
str(index))
rospy.loginfo("Comm. State: pick action client " +
str(goal_handle.get_comm_state()))
rospy.loginfo("Goal Status: pick action client " +
str(goal_handle.get_goal_status()))
# Comm State - Monitors the State Machine of the Client which is different from Server's
# Comm State = 2 -> Active
# Comm State = 3 -> Wating for Result
# Comm State = 7 -> Done
# if (Comm State == ACTIVE)
if goal_handle.get_comm_state() == 2:
rospy.loginfo(
str(index) + ": Goal just went active. in pick action server")
if goal_handle.get_comm_state() == 3:
self._processing_goal_handle = self._goal_handles2[index]
self._processing_goal = self._goal_info[index]
print('current_processing goal', self._processing_goal)
# if (Comm State == DONE)
if goal_handle.get_comm_state() == 7:
rospy.loginfo(str(index) + ": Goal is DONE by pick action server")
rospy.loginfo(goal_handle.get_terminal_state())
# get_result() gets the result produced by the Action Server
result = goal_handle.get_result()
rospy.loginfo(result)
if (result.success == True):
self._goal_handles2.pop(index)
self._goal_info.pop(index)
if result.state == 0:
res = json.loads(result.result)
sen = msgPickPlaceResult()
for i in res:
sen.result = json.dumps(res[i])
sen.success = True
sen.state = 0
self.send_goal_iot(sen)
else:
rospy.loginfo(
"Goal successfully completed by pick action server. Client Goal Handle #: "
+ str(index))
y = sorted(self._goal_handles2)
if self._goal_handles2 != {}:
self.send_goal_pick_place(self._goal_info[y[0]])
self.send_goal_place(result)
self.send_goal_iot(result)
else:
rospy.loginfo("Goal failed. Client Goal Handle #: " +
str(index))