def get_trajectory_markers(trajectories,
names,
object_trajectories=None,
link_names=None,
start_id=0):
state = RobotState()
state.joint_state = tt.joint_trajectory_point_to_joint_state(
trajectories[0].joint_trajectory.points[0],
trajectories[0].joint_trajectory.joint_names)
mdf = trajectories[0].multi_dof_joint_trajectory
state.multi_dof_joint_state = tt.multi_dof_trajectory_point_to_multi_dof_state(
mdf.points[0], mdf.joint_names, mdf.frame_ids, mdf.child_frame_ids,
mdf.stamp)
n = names[0].split('-')
c = type_to_color(n[0])
marray = vt.robot_marker(state,
ns='traj_start_state' + str(start_id),
r=c.r,
g=c.g,
b=c.b,
a=0.5)
for (i, m) in enumerate(marray.markers):
m.id = start_id + i
traj = [marray]
mid = start_id
for (i, t) in enumerate(trajectories):
splits = names[i].split('-')
c = type_to_color(splits[0])
rospy.loginfo('Drawing trajectory ' + str(i) + ' of type ' + names[i] +
' and length ' + str(len(t.joint_trajectory.points)))
#print 'Trajectory is\n', t
for j in range(len(t.joint_trajectory.points)):
state.joint_state = tt.joint_trajectory_point_to_joint_state(
t.joint_trajectory.points[j], t.joint_trajectory.joint_names)
state.multi_dof_joint_state = tt.multi_dof_trajectory_point_to_multi_dof_state(
t.multi_dof_joint_trajectory.points[j],
t.multi_dof_joint_trajectory.joint_names,
t.multi_dof_joint_trajectory.frame_ids,
t.multi_dof_joint_trajectory.child_frame_ids,
t.multi_dof_joint_trajectory.stamp)
marray = vt.robot_marker(state,
link_names=link_names,
ns='trajectory',
r=c.r,
g=c.g,
b=c.b,
a=0.5)
#marray = vt.trajectory_markers(t, ns='trajectory', link_names=link_names,resolution=3,
# color=c)
for (i, m) in enumerate(marray.markers):
m.id = mid
mid += 1
traj.append(copy.deepcopy(marray))
return traj
def trajectory_markers(robot_traj, ns='', link_names=None, color=None, a=0.8, scale=1.0, resolution=1):
'''
Returns markers representing the robot trajectory.
The color of each point on the trajectory runs from -60 to 300 in hue with full saturation and value. Note that
-60 is red so this should approximately follow the rainbow.
**Args:**
**robot_traj (arm_navigation_msgs.msg.RobotTrajectory):** Trajectory for which you want markers
*ns (string):* Marker namespace.
*a (double):* Alpha (scale 0 to 1)
*scale (double):* Scaling for entire robot at each point
*resolution (int):* Draws a point only every resolution points on the trajectory. Will always draw
the first and last points.
**Returns:**
visualization_msgs.msg.MarkerArray that will draw the whole trajectory. Each point on the trajectory is
made up of many markers so this will be substantially longer than the length of the trajectory.
'''
if resolution <= 0:
resolution = 1
limit = len(robot_traj.joint_trajectory.points)
if not limit:
limit = len(robot_traj.multi_dof_joint_trajectory.points)
marray = MarkerArray()
for i in range(0, limit, resolution):
robot_state = RobotState()
if len(robot_traj.joint_trajectory.points):
robot_state.joint_state = tt.joint_trajectory_point_to_joint_state(robot_traj.joint_trajectory.points[i],
robot_traj.joint_trajectory.joint_names)
if len(robot_traj.multi_dof_joint_trajectory.points):
robot_state.multi_dof_joint_state = tt.multi_dof_trajectory_point_to_multi_dof_state(
robot_traj.multi_dof_joint_trajectory.points[i], robot_traj.multi_dof_joint_trajectory.joint_names,
robot_traj.multi_dof_joint_trajectory.frame_ids, robot_traj.multi_dof_joint_trajectory.child_frame_ids,
robot_traj.multi_dof_joint_trajectory.stamp)
if not color:
(r, g, b) = hsv_to_rgb((i/float(limit)*360 - 60)%360, 1, 1)
else:
(r, g, b) = (color.r, color.g, color.b)
marray.markers += robot_marker(robot_state, link_names=link_names, ns=ns, r=r, g=g, b=b, a=a, scale=scale).markers
if i != limit-1:
if not color:
(r, g, b) = hsv_to_rgb(300, 1, 1)
else:
(r, g, b) = (color.r, color.g, color.b)
marray.markers += robot_marker(tt.last_state_on_robot_trajectory(robot_traj), link_names=link_names, ns=ns, r=r, g=g,
b=b, a=a, scale=scale).markers
for (i, m) in enumerate(marray.markers):
m.id = i
return marray
def modify_pickle_file():
[result, goal, collision_objects,
robot_state] = pickle.load(open(sys.argv[1], 'r'))
robot_state = RobotState()
robot_state.multi_dof_joint_state = tt.multi_dof_trajectory_point_to_multi_dof_state(
result.primitive_trajectories[0].multi_dof_joint_trajectory.points[0],
result.primitive_trajectories[0].multi_dof_joint_trajectory.
joint_names,
result.primitive_trajectories[0].multi_dof_joint_trajectory.frame_ids,
result.primitive_trajectories[0].multi_dof_joint_trajectory.
child_frame_ids,
result.primitive_trajectories[0].multi_dof_joint_trajectory.stamp)
robot_state.joint_state = tt.joint_trajectory_point_to_joint_state(
result.primitive_trajectories[0].joint_trajectory.points[0],
result.primitive_trajectories[0].joint_trajectory.joint_names)
pickle.dump([result, goal, collision_objects, robot_state],
open(sys.argv[1], 'wb'))
def last_state_on_robot_trajectory(trajectory):
'''
Returns the last state on a robot trajectory
**Args:**
**trajectory (arm_navigation_msgs.msg.RobotTrajectory):** robot trajectory
**Returns:**
The last state on the trajectory as an arm_navigation_msgs.msg.RobotState
'''
robot_state = RobotState()
robot_state.joint_state = last_state_on_joint_trajectory(trajectory.joint_trajectory)
if not robot_state.joint_state: return None
mdf_traj = trajectory.multi_dof_trajectory
robot_state.multi_dof_joint_state = multi_dof_trajectory_point_to_multi_dof_state\
(mdf_traj.points[-1], mdf_traj.stamp, mdf_traj.joint_names, mdf_traj.frame_ids, mdf_traj.child_frame_ids)
return robot_state
def last_state_on_robot_trajectory(trajectory):
'''
Returns the last state on a robot trajectory
**Args:**
**trajectory (arm_navigation_msgs.msg.RobotTrajectory):** robot trajectory
**Returns:**
The last state on the trajectory as an arm_navigation_msgs.msg.RobotState
'''
robot_state = RobotState()
robot_state.joint_state = last_state_on_joint_trajectory(
trajectory.joint_trajectory)
if not robot_state.joint_state: return None
mdf_traj = trajectory.multi_dof_trajectory
robot_state.multi_dof_joint_state = multi_dof_trajectory_point_to_multi_dof_state\
(mdf_traj.points[-1], mdf_traj.stamp, mdf_traj.joint_names, mdf_traj.frame_ids, mdf_traj.child_frame_ids)
return robot_state
