def send_goal_pick_place(self, msg):
goal = msgPickPlaceGoal()
goal.message = msg
goal_handle = self._ac2.send_goal(goal, self.on_transition2, None)
print('Sending goal to pick action client for picking package', goal)
self.count2 = self.count2 + 1
self._goal_handles2.update({self.count2: goal_handle})
self._goal_info.update({self.count2: msg})
index = 0
if self._goal_handles2 != {}:
y = sorted(self._goal_handles2)
curr_goal = json.loads(self._processing_goal)
self._largest_goal = json.loads(self._goal_info[y[0]])
self._largest_goal_handle = self._goal_handles2[y[0]]
for i in range(len(y) - 1):
next_goal = json.loads(self._goal_info[y[i + 1]])
index = i
print('inside the for loop', self._goal_info)
if self.cost[self.colour[next_goal['item']]] > self.cost[
self.colour[self._largest_goal['item']]]:
self._largest_goal = next_goal
self._largest_goal_handle = self._goal_handles2[y[i + 1]]
index = i + 1
if self.cost[self.colour[self._largest_goal['item']]] > self.cost[
self.colour[curr_goal['item']]]:
self._processing_goal_handle.cancel()
goal.message = json.dumps(self._largest_goal)
self._goal_handles2.pop(y[index])
self._goal_info.pop(y[index])
def send_goal_place(self, msg):
goal = msgPickPlaceGoal()
print('msg in send_goal_place', msg)
goal.message = msg.result
goal_handle = self._ac3.send_goal(goal, self.on_transition3, None)
self.count3 = self.count3 + 1
self._goal_handles3.update({str(self.count3): goal_handle})
def send_goal_pick_place(self, msg):
goal = msgPickPlaceGoal()
goal.message = msg
goal_handle = self._ac2.send_goal(goal, self.on_transition2, None)
print('Sending goal to pick action client for picking package', goal)
self.count2 = self.count2 + 1
self._goal_handles2.update({str(self.count2): goal_handle})
def send_goal_pick_place(self, msg):
goal = msgPickPlaceGoal()
goal.message = msg
self._processing_goal_handle = self._ac2.send_goal(
goal, done_cb=self.done_callback)
self._processing_goal = goal.message
self.count2 = self.count2 + 1
self._goal_info.update({self.count2: goal.message})
def __init__(self):
self._ac1 = actionlib.ActionClient('/action_ros_iot', msgRosIotAction)
self._ac2 = actionlib.ActionClient('/action_pick', msgPickPlaceAction)
self._ac3 = actionlib.ActionClient('/action_place', msgPickPlaceAction)
rospy.init_node('node_pick_place_iot_client')
self._goal_handles1 = {}
self._goal_handles2 = {}
self._goal_handles3 = {}
self.count1 = 0
self.count2 = 0
self.count3 = 0
self._ac1.wait_for_server()
rospy.loginfo('Action Iot server up ready....')
self._ac2.wait_for_server()
rospy.loginfo('Pick server up ready....')
self.colour = {'Medicine': 'red', 'Food': 'yellow', 'Clothes': 'green'}
self.item = {'red': 'Medicine', 'yellow': 'Food', 'green': 'Clothes'}
self.priority = {'red': 'HP', 'yellow': 'MP', 'green': 'LP'}
self.cost = {'red': 450, 'yellow': 250, 'green': 150}
self._ac3.wait_for_server()
rospy.loginfo('Place server up ready....')
self._sub_mqtt_sub = rospy.Subscriber('/ros_iot_bridge/mqtt/sub',
msgMqttSub,
self.func_callback_msgMqttSub)
goal = msgPickPlaceGoal()
goal.message = 'None'
goal_handle = self._ac2.send_goal(goal, self.on_transition2, None)
self.count2 = self.count2 + 1
self._goal_handles2.update({str(self.count2): goal_handle})
def check_goal_pick_place(self, msg):
rospy.logwarn('waiting for result complete')
goal = msgPickPlaceGoal()
goal.message = msg
print('the goals collected are ', self._goal_info)
if self._goal_info != {}:
y = sorted(self._goal_info)
# curr_goal = json.loads(self._processing_goal)
self._largest_goal = json.loads(self._goal_info[y[0]])
# self._largest_goal_handle = self._goal_handles2[y[0]]
for i in range(len(y) - 1):
next_goal = json.loads(self._goal_info[y[i + 1]])
print('inside the for loop', self._goal_info)
if self.cost[self.colour[next_goal['item']]] > self.cost[
self.colour[self._largest_goal['item']]]:
self._largest_goal = next_goal
# self._largest_goal_handle = self._goal_handles2[y[i+1]]
# if self.cost[self.colour[self._largest_goal['item']]] > self.cost[self.colour[curr_goal['item']]]:
# self._processing_goal_handle.cancel()
goal.message = json.dumps(self._largest_goal)
# self._goal_handles2.pop(y[index])
# self._goal_info.pop(y[index])
print('the largest among goals')
rospy.logwarn(self._largest_goal)
self._processing_goal_handle = self._ac2.send_goal(
goal, done_cb=self.done_callback)
self._processing_goal = goal.message
rospy.logwarn('waiting for result')
print('Sending goal to pick action client for picking package', goal)
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 __init__(self):
rospy.init_node('node_pick_place_iot_client')
self._ac1 = actionlib.ActionClient('/action_ros_iot', msgRosIotAction)
self._ac2 = actionlib.SimpleActionClient('/action_pick',
msgPickPlaceAction)
self._ac3 = actionlib.SimpleActionServer('/action_place',
msgPickPlaceAction,
execute_cb=self.on_goal,
auto_start=False)
self._goal_handles1 = {}
self._goal_handles2 = {}
self._goal_handles3 = {}
self._goal_info = {}
self.count1 = 0
self.count2 = 0
self.count3 = 0
self._ac1.wait_for_server()
rospy.loginfo('Action Iot server up ready....')
self._ac2.wait_for_server()
rospy.loginfo('Pick server up ready....')
self.colour = {'Medicine': 'red', 'Food': 'yellow', 'Clothes': 'green'}
self.item = {'red': 'Medicine', 'yellow': 'Food', 'green': 'Clothes'}
self.priority = {'red': 'HP', 'yellow': 'MP', 'green': 'LP'}
self.cost = {'red': 450, 'yellow': 250, 'green': 150}
self.processing_goal_place = ''
self._processing_goal = ''
self._processing_goal_handle = ''
self.largest_goal_handle = ''
self.largest_goal = ''
rospy.loginfo('Place server up ready....')
self._sub_mqtt_sub = rospy.Subscriber('/ros_iot_bridge/mqtt/sub',
msgMqttSub,
self.func_callback_msgMqttSub)
goal = msgPickPlaceGoal()
goal.message = 'None'
self._processing_goal_handle = self._ac2.send_goal(
goal, done_cb=self.done_callback)
self._processing_goal = goal.message
self.count2 = self.count2 + 1
self._goal_info.update({self.count2: goal.message})
self._ac3.start()
