def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = 1.
trajectory.joint_names = response.trajectory.joint_trajectory.joint_names
trajectory.planning_time = response.planning_time
joint_types = [
joint_type_by_name[name] for name in trajectory.joint_names
]
trajectory.points = convert_trajectory_points(
response.trajectory.joint_trajectory.points, joint_types)
start_state = response.trajectory_start.joint_state
start_state_types = [
joint_type_by_name[name] for name in start_state.name
]
trajectory.start_configuration = Configuration(
start_state.position, start_state_types, start_state.name)
trajectory.attached_collision_meshes = list(
itertools.chain(*[
aco.to_attached_collision_meshes() for aco in
response.trajectory_start.attached_collision_objects
]))
callback(trajectory)
def convert_trajectory(joints, solution, solution_start_state, fraction,
planning_time, source_message):
trajectory = JointTrajectory()
trajectory.source_message = source_message
trajectory.fraction = fraction
trajectory.joint_names = solution.joint_trajectory.joint_names
trajectory.planning_time = planning_time
joint_types = [joints[name] for name in trajectory.joint_names]
trajectory.points = convert_trajectory_points(
solution.joint_trajectory.points, joint_types)
start_state = solution_start_state.joint_state
start_state_types = [joints[name] for name in start_state.name]
trajectory.start_configuration = Configuration(start_state.position,
start_state_types,
start_state.name)
trajectory.attached_collision_meshes = list(
itertools.chain(*[
aco.to_attached_collision_meshes()
for aco in solution_start_state.attached_collision_objects
]))
return trajectory
