def _handle_base_col_3_pos_button_clicked(self):
# put arms in start position
self._handle_arms_start_pos_button_clicked()
rospy.loginfo('[GUI]: base col 3 pos')
while not rospy.is_shutdown():
try:
base_pos_dict = rospy.get_param('/base_pos_dict')
break
except:
rospy.sleep(random.uniform(0,1))
continue
base_pos_goal = base_pos_dict['column_3']
rospy.loginfo('Base setpoint: ')
rospy.loginfo(base_pos_goal)
self.get_bm_srv()
self._bm_preempt_srv.call(EmptyRequest())
req = BaseMoveRequest()
req.x = base_pos_goal[0]
req.y = base_pos_goal[1]
req.theta = base_pos_goal[5]
res = self._bm_move_srv.call(req)
def go_base_pos_async(self, base_pos_goal):
angle = base_pos_goal[5]
pos = base_pos_goal[0:2]
r = rospy.Rate(20.0)
req = BaseMoveRequest()
req.x = pos[0]
req.y = pos[1]
req.theta = angle
self.get_bm_srv()
res = self._bm_move_srv.call(req)
if self.execute_exit():
return False
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION
rospy.loginfo('action halted while moving base')
self._as.set_preempted()
self._exit = True
if self.execute_exit():
return False
return res.result
def go_base_pos_async(self, base_pos_goal):
angle = base_pos_goal[5]
pos = base_pos_goal[0:2]
r = rospy.Rate(20.0)
req = BaseMoveRequest()
req.x = pos[0]
req.y = pos[1]
req.theta = angle
self.get_bm_srv()
res = self._bm_move_srv.call(req)
if self.execute_exit():
return False
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION
rospy.loginfo('action halted while moving base')
self._as.set_preempted()
self._exit = True
if self.execute_exit():
return False
return res.result
def _handle_base_retreat_button_clicked(self):
rospy.loginfo('[GUI]: base retreat')
r = rospy.Rate(1.0)
while not self._got_shelf_pose:
rospy.loginfo('[' + rospy.get_name() + ']: waiting for shelf pose')
r.sleep()
base_pos_goal = [-1.42, -self._shelf_pose.pose.position.y, 0.0, 0.0, 0.0, 0.0]
rospy.loginfo('Base setpoint: ')
rospy.loginfo(base_pos_goal)
self.get_bm_srv()
self._bm_preempt_srv.call(EmptyRequest())
req = BaseMoveRequest()
req.x = base_pos_goal[0]
req.y = base_pos_goal[1]
req.theta = base_pos_goal[5]
res = self._bm_move_srv.call(req)
def go_base_moveit_group_pos_async(self,
base_pos_goal,
group,
joint_pos_goal,
normalize_angles=False):
angle = base_pos_goal[5]
pos = base_pos_goal[0:2]
r = rospy.Rate(20.0)
q_goal = np.array(joint_pos_goal)
if normalize_angles:
q_goal = self.normalize_angles(joint_pos_goal)
group.set_joint_value_target(joint_pos_goal)
group.go(wait=False)
q_now = np.array(group.get_current_joint_values())
if normalize_angles:
q_now = self.normalize_angles(q_now)
q_tol = group.get_goal_joint_tolerance()
if group.get_name() == 'left_arm' or group.get_name(
) == 'right_arm' or group.get_name() == 'arms' or group.get_name(
) == 'head':
q_tol = 0.04
elif group.get_name() == 'torso':
q_tol = 0.003
t_print = rospy.Time.now()
req = BaseMoveRequest()
req.x = pos[0]
req.y = pos[1]
req.theta = angle
self.get_bm_srv()
res = self._bm_move_srv.call(req)
if self.execute_exit():
return False
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION
rospy.loginfo('action halted while moving base')
self._as.set_preempted()
self._exit = True
if self.execute_exit():
return False
# check for preemption while the arm hasn't reach goal configuration
while np.max(
np.abs(q_goal - q_now)) > q_tol and not rospy.is_shutdown():
if self.execute_exit():
return False
q_now = np.array(group.get_current_joint_values())
if normalize_angles:
q_now = self.normalize_angles(q_now)
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION
group.stop()
rospy.loginfo('action halted')
self._as.set_preempted()
self._exit = True
if self.execute_exit():
return False
if (rospy.Time.now() - t_print).to_sec() > 3.0:
t_print = rospy.Time.now()
rospy.loginfo('[' + rospy.get_name() + ']: executing action')
#HERE THE CODE TO EXECUTE AS LONG AS THE BEHAVIOR TREE DOES NOT HALT THE ACTION
r.sleep()
if rospy.is_shutdown():
return False
return res.result
def go_base_moveit_group_pos_async(self, base_pos_goal, group, joint_pos_goal, normalize_angles=False):
angle = base_pos_goal[5]
pos = base_pos_goal[0:2]
r = rospy.Rate(20.0)
q_goal = np.array(joint_pos_goal)
if normalize_angles:
q_goal = self.normalize_angles(joint_pos_goal)
group.set_joint_value_target(joint_pos_goal)
group.go(wait=False)
q_now = np.array(group.get_current_joint_values())
if normalize_angles:
q_now = self.normalize_angles(q_now)
q_tol = group.get_goal_joint_tolerance()
if group.get_name()=='left_arm' or group.get_name()=='right_arm' or group.get_name()=='arms' or group.get_name()=='head':
q_tol = 0.04
elif group.get_name()=='torso':
q_tol = 0.003
t_print = rospy.Time.now()
req = BaseMoveRequest()
req.x = pos[0]
req.y = pos[1]
req.theta = angle
self.get_bm_srv()
res = self._bm_move_srv.call(req)
if self.execute_exit():
return False
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION
rospy.loginfo('action halted while moving base')
self._as.set_preempted()
self._exit = True
if self.execute_exit():
return False
# check for preemption while the arm hasn't reach goal configuration
while np.max(np.abs(q_goal-q_now)) > q_tol and not rospy.is_shutdown():
if self.execute_exit():
return False
q_now = np.array(group.get_current_joint_values())
if normalize_angles:
q_now = self.normalize_angles(q_now)
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
#HERE THE CODE TO EXECUTE WHEN THE BEHAVIOR TREE DOES HALT THE ACTION
group.stop()
rospy.loginfo('action halted')
self._as.set_preempted()
self._exit = True
if self.execute_exit():
return False
if (rospy.Time.now()-t_print).to_sec()>3.0:
t_print = rospy.Time.now()
rospy.loginfo('[' + rospy.get_name() + ']: executing action')
#HERE THE CODE TO EXECUTE AS LONG AS THE BEHAVIOR TREE DOES NOT HALT THE ACTION
r.sleep()
if rospy.is_shutdown():
return False
return res.result
