def _exec_traj(self):
t = self.robot_parent.gettime()
trajectory = self._active_trajectory
try:
if time_isafter(trajectory["starttime"], t):
return
if time_isafter(
t, trajectory["starttime"] +
trajectory["points"][-1]["time_from_start"]):
#trajectory execution is over!
self._active_trajectory = None
# TODO: check here the final pose match the last point pose
self.completed(status.SUCCESS, None)
for p in trajectory["points"]:
end = trajectory["starttime"] + p["time_from_start"]
if "started" in p and time_isafter(end, t):
# currently going to this waypoint: fine!
break
elif "started" not in p and time_isafter(end, t):
# start the new waypoint
allocated_time = end - t
assert (allocated_time > 0)
target = OrderedDict(
zip(self.local_data.keys(), p["positions"]))
for joint in target.keys():
# compute the distance based on actual current joint pose
dist = target[joint] - self._get_joint_value(joint)
self.joint_speed[joint] = dist / allocated_time
self.local_data = target
p["started"] = True
break
elif "started" not in p and time_isafter(t, end):
logger.warning(
"Skipped a waypoint on armature <%s>. Wrong 'time_from_start'?"
% self.name())
# case: "started" and t > end: do nothing, go to next point
except KeyError as ke:
self._active_trajectory = None
self.completed(
status.FAILED,
"Error: invalid trajectory: key %s was expected." % ke)
def periodic_call(self):
"""
Return true if the component must be called on this loop call, False otherwise
"""
# must be called on each loop occurence
if not hasattr(self, '_component_period'):
return True
# First call
if not self._last_call:
self._last_call = self.__time.time
self._nb_call = 1
return True
else:
"""
We deal with a complete simulated second to deal with
frequencies that are not a fraction of the main simulator frequency.
For that purpose, we integrate until self._nb_call ==
self._frequency, i.e. self._nb_call * self._component_period == 1.0
"""
must_call = time_isafter(self.__time.time, self._last_call + self._nb_call * self._component_period)
if must_call:
if (self._nb_call == self._frequency):
self._nb_call = 1
self._last_call = copy(self.__time.time)
else:
self._nb_call += 1
return must_call
def _exec_traj(self):
t = self.robot_parent.gettime() / 1000.0
trajectory = self._active_trajectory
try:
if time_isafter(trajectory["starttime"], t):
return
if time_isafter(t, trajectory["starttime"] + trajectory["points"][-1]["time_from_start"]):
#trajectory execution is over!
self._active_trajectory = None
# TODO: check here the final pose match the last point pose
self.completed(status.SUCCESS, None)
for p in trajectory["points"]:
end = trajectory["starttime"] + p["time_from_start"]
if "started" in p and time_isafter(end, t):
# currently going to this waypoint: fine!
break
elif "started" not in p and time_isafter(end, t):
# start the new waypoint
allocated_time = end - t
assert(allocated_time > 0)
target = OrderedDict(zip(self.local_data.keys(),
p["positions"]))
for joint in target.keys():
# compute the distance based on actual current joint pose
dist = target[joint] - self._get_joint_value(joint)
self.joint_speed[joint] = dist/allocated_time
self.local_data = target
p["started"] = True
break
elif "started" not in p and time_isafter(t, end):
logger.warning("Skipped a waypoint on armature <%s>. Wrong 'time_from_start'?" % self.name())
# case: "started" and t > end: do nothing, go to next point
except KeyError as ke:
self._active_trajectory = None
self.completed(status.FAILED, "Error: invalid trajectory: key %s was expected." % ke)
def action(self):
if self._alarm_time and time_isafter(self.time.time, self._alarm_time):
logger.debug('alarm fired at %f difference %f' % (self.time.time, self._alarm_time - self.time.time))
self._alarm_time = None
self.completed(status.SUCCESS)
def action(self):
if self._alarm_time and time_isafter(self.time.time, self._alarm_time):
logger.debug('alarm fired at %f difference %f' %
(self.time.time, self._alarm_time - self.time.time))
self._alarm_time = None
self.completed(status.SUCCESS)