def on_enter(self, userdata):
userdata.goal = None
self._goal_pose = None
if self._sub.has_msg(self._topic):
Logger.loginfo('%s Clearing prior goal' % (self.name))
self._sub.remove_last_msg(self._topic)
Logger.loghint('%s Input new 2D Nav goal (e.g. via RViz) ' % (self.name))
def execute(self, d):
'''Execute this state'''
force_msg = self._sub.get_last_msg(self._force_topic)
if force_msg is None:
return
#current_force = np.array([np.clip(force_msg.wrench.force.x, -99, 0), np.clip(force_msg.wrench.force.y, 0, 99), force_msg.wrench.force.z*0.5])
force_norm = force_msg.data
current_threshold = self._threshold # * (0.5+current_acc)
if d.carrying:
current_threshold *= 1.5
#y contains gravity here
#current_force = force_msg.wrench.force.y
#force_norm = np.linalg.norm(current_force)
#Logger.loginfo('time: %r got force: force_norm %r _threshold %r adapted thresh %r' % (rospy.Time.now().nsecs/1000000, force_norm, self._threshold, current_threshold))
if force_norm > current_threshold:
Logger.loghint(
'got force: force_norm %r _threshold %r adapted thresh %r' %
(force_norm, self._threshold, current_threshold))
return 'success'
def execute(self, userdata): '''Execute this state''' if self._subscriber.has_msg(self._topic): msg = self._subscriber.get_last_msg(self._topic) # in case you want to make sure the same message is not processed twice: self._subscriber.remove_last_msg(self._topic) if not msg.found: return 'success' self.calculate_error(msg) Logger.loghint(self._error_result) for key in self._keys: # check tolerance if self._tolerance < abs(self._error_result[key]): userdata.output_value = self._error_result return 'unacceptable'
def execute(self, userdata):
cur_pose = self._hand_sub.get_last_msg(self._topic)
self._hand_sub.remove_last_msg(self._topic)
if cur_pose is None:
# print('current pose none')
return
if rospy.Time.now() - cur_pose.header.stamp > rospy.Duration(0.5):
print('got old msg')
return
ps = PoseStamped()
ps.header = cur_pose.header
ps.pose = cur_pose.pose
try:
dist_pose = self._listener.transformPose(self._frame, ps)
except Exception:
print('oh no:')
return
dist = np.linalg.norm(
np.array([
dist_pose.pose.position.x + self._off_x,
dist_pose.pose.position.y + self._off_y,
dist_pose.pose.position.z + self._off_z
]))
# print('dist:' + str(dist))
self._dist_buf.appendleft(dist)
if len(self._dist_buf) == self._n_steps and (np.array(
self._dist_buf) < self._dist_threshold).all():
Logger.loghint('hand nearness threshold reached!')
return 'near'
def on_exit(self, userdata):
Logger.loghint('gazing at ' + str(self._target) + ' lost: ' +
str((rospy.get_rostime() - self._start_time).to_sec()))
def on_enter(self, userdata): if self._hint is not None: Logger.loghint(self._hint)
def on_enter(self, userdata):
if self._wait:
Logger.loghint("Waiting for %s" % str(self._target_mode))
def on_exit(self, userdata):
if self._PathSelected == True:
Logger.loghint('the %s path was selected' % self._path)
else:
Logger.logwarn('no path found fall back to default path')
def on_enter(self, userdata):
if self._wait:
Logger.loghint("Waiting for %s" % str(self._target_mode))