def follow_configurations(self,
callback,
joint_names,
configurations,
timesteps,
timeout=None):
if len(configurations) != len(timesteps):
raise ValueError(
"%d configurations must have %d timesteps, but %d given." %
(len(configurations), len(timesteps), len(timesteps)))
if not timeout:
timeout = timesteps[-1] * 1000 * 2
points = []
num_joints = len(configurations[0].values)
for config, time in zip(configurations, timesteps):
pt = JointTrajectoryPoint(positions=config.values,
velocities=[0] * num_joints,
time_from_start=Time(secs=(time)))
points.append(pt)
joint_trajectory = JointTrajectory(Header(), joint_names,
points) # specify header necessary?
return self.follow_joint_trajectory(callback, joint_trajectory,
timeout)
def _convert_to_ros_trajectory(self, joint_trajectory):
"""Converts a ``compas_fab.robots.JointTrajectory`` into a ROS Msg joint trajectory."""
# For backwards-compatibility, accept ROS Msg directly as well and simply do not modify
if isinstance(joint_trajectory, RosMsgJointTrajectory):
return joint_trajectory
trajectory = RosMsgJointTrajectory()
trajectory.joint_names = joint_trajectory.joint_names
for point in joint_trajectory.points:
ros_point = RosMsgJointTrajectoryPoint(
positions=point.positions,
velocities=point.velocities,
accelerations=point.accelerations,
effort=point.effort,
time_from_start=Time(point.time_from_start.secs,
point.time_from_start.nsecs),
)
trajectory.points.append(ros_point)
return trajectory