def schedule(self, command: TrajectoryCommand):
"""Schedules a new trajectory.
:param TrajectoryCommand command: The trajectory command to schedule
"""
self._failed = False
stamp = command.start_time.to_msg()
command.trajectory.header.stamp = stamp
goal = FollowJointTrajectoryGoal(trajectory=command.trajectory)
self._trajectory_goal_pub.publish(
FollowJointTrajectoryActionGoal(
header=Header(stamp=stamp),
goal_id=GoalID(stamp=stamp, id=str(command)),
goal=goal,
)
)
info_log_message = f"Scheduling {command.name}"
debug_log_message = f"Subgait {command.name} starts "
if self._node.get_clock().now() < command.start_time:
time_difference = Duration.from_ros_duration(
command.start_time - self._node.get_clock().now()
)
debug_log_message += f"in {round(time_difference.seconds, 3)}s"
else:
debug_log_message += "now"
self._goals.append(command)
self.logger.info(info_log_message)
self.logger.debug(debug_log_message)
def elapsed_time(self) -> Duration:
return Duration.from_ros_duration(self._current_time -
self._start_time)
def test_from_ros_duration(self):
duration = ROSDuration(seconds=3)
self.assertEqual(Duration.from_ros_duration(duration).nanoseconds, 3000000000)
