def visualize_ball_contact_normal (ball_center,
contacts, vis_pub, frame_id, cumu_marker_id, marker_duration=0):
# So that RViz doesn't print hundreds of errors
if not frame_id:
return
# If empty, don't publish empty marker
if not contacts:
return
# dark green (see noise_measure_legend.py)
marker_normals = Marker ()
create_marker (Marker.LINE_LIST, 'con_normals', frame_id, cumu_marker_id,
0, 0, 0, 20/255.0, 90/255.0, 0, 0.5, 0.001, 0, 0,
marker_normals, marker_duration) # Use 0 duration for forever
# Append points to marker
for i in range (0, len (contacts)):
# Contact normal = measured point on ball - known ball center
marker_normals.points.append (ball_center)
marker_normals.points.append (contacts [i])
vis_pub.publish (marker_normals)
def plot_fingertip (self, pos, R, frame_id):
marker = Marker ()
create_marker (Marker.SPHERE, '/fingertip_bicchi', frame_id, 0,
pos[0], pos[1], pos[2], 1, 1, 1, 0.5, 2*R, 2*R, 2*R,
marker, self.marker_duration)
self.vis_pub.publish (marker)
def handCB(self, msg):
if self.pause_recording:
return
self.msg_id += 1
# Use one marker for this entire msg. Add multiple points to it later
# Parameters: tx, ty, tz, r, g, b, alpha, sx, sy, sz
marker_p = Marker()
create_marker(Marker.POINTS, 'contacts', self.marker_frame,
self.msg_id, 0, 0, 0, 1, 1, 0, 0.5, 0.01, 0.01, 0.01,
marker_p, 60) #0) # Use 0 duration for forever
# Init points array to empty list
# rospy msg "are deserialized as tuples for performance reasons, but you
# can set fields to tuples and lists interchangeably".
# Ref: http://wiki.ros.org/msg
marker_p.points = []
# Comment out to keep cumulative cloud for whole run.
# Uncomment to look at cloud at one short instance.
#self.cloud.points = []
# Update next msg's seq, even if no new pts are added (in which case the
# timestamp of cloud won't change, by design), to let user know we're
# running normally.
self.cloud.header.seq = self.msg_id
self.normals.header.seq = self.msg_id
# Get coordinates of sensors whose pressure values exceed threshold
[contacts, normal_endpts, _, _,
_] = get_contacts_live(msg, self.contact_thresh, self.marker_frame,
self.tfTrans, True)
self.add_points(marker_p, contacts, self.normals, normal_endpts)
if len(marker_p.points) > 0:
self.vis_pub.publish(marker_p)
# Init point cloud msg
# API: http://docs.ros.org/api/sensor_msgs/html/msg/PointCloud.html
# Including this in the if-stmt has advantage: then when you save to
# .pcd file, instead of saving a ton of the same one, it will overwrite
# all files with same timestamp. So you get unique files, don't need
# to dig through and delete repetitive ones.
self.cloud.header.stamp = rospy.Time.now()
# Cloud will be published in next loop in __main__. Don't publish here,
# to save callback function lag time before receiving next msg in queue
# Init normals msg
self.normals.header.stamp = rospy.Time.now()
# To see whether marker is plotted in correct x y quadrant wrt base_link
'''
def __init__(self): #, tfTrans, bc):
# Debug flag. Set to true to plot axes for all 6 Pottmann eigvals,
# from est_linear_complex(). As opposed to just the axis with min eigval,
# which is what Pottmann selects.
# Practice shows that the one with min eigval is most correct. If it's
# very off, then the other eigvals' axes are even more off. So it's
# sufficient to just plot the correct one.
# This flag is still useful though when in doubt.
self.PLOT_ALL_AXES = False
self.vis_pub = rospy.Publisher('/visualization_marker', Marker)
self.NORMALS_MARKER_ID = 1
self.TRUE_AXIS_ID = 0
self.EST_AXIS_ID = 1
self.marker_duration = 0
self.cloud_spun_pub = rospy.Publisher(
'/tactile_map/spin_seam/cloud_spun', PointCloud)
self.tfTrans = tf.TransformListener()
self.bc = tf.TransformBroadcaster()
#self.tfTrans = tfTrans
#self.bc = bc
rospy.Subscriber('/reflex_hand', Hand, self.handCB)
self.handMsg = None
self.contact_thresh = 15
self.contacts = []
self.contact_cloud = PointCloud()
self.contact_cloud.header.frame_id = '/base'
self.contact_cloud_pub = rospy.Publisher('/tactile_map/contact/cloud',
PointCloud)
self.normal_endpts = []
self.marker_normals = Marker()
# green
create_marker(Marker.LINE_LIST, 'normals', '/base', 0, 0, 0, 0, 0, 1,
0, 0.5, 0.001, 0, 0, self.marker_normals,
self.marker_duration) # Use 0 duration for forever
# Tells us that hand is in movement. Do not look at current tactile values
# or finger positions as contact points.
# This is important because hand moves really fast. Delay in code is
# longer than delay in hand opening. So you might be looking at contact
# points a moment ago, and now looking up tf, but the hand has started
# moving, so you end up getting a point in mid-air from tf, because
# that's where the finger was in that split second, but that's not where
# the contact was.
rospy.Subscriber('/tactile_map/pause', Bool, self.pauseCB)
self.pause_contacts = False
def __init__(self):
########
# Constants
self.contact_thresh = 100
########
# ROS stuff
self.tfTrans = tf.TransformListener()
self.vis_pub = rospy.Publisher('/visualization_marker', Marker)
self.cloud_pub = rospy.Publisher('/tactile_map/contact/cloud',
PointCloud)
self.msg_id = 0
# Note this is Base_link of ReFlex hand, not of whatever else.
# reflex_tf_broadcaster broadcasts a capitalized Base_link. Its parent is
# the lowercase base_link in URDF, published by robot_state_publisher.
#self.marker_frame = '/base_link'
# Use Baxter torso frame instead of hand frame, so that cloud and normals
# don't move around with the hand each time they're republished!
self.marker_frame = '/base'
# Cloud is CUMULATIVE for entire run.
# Marker is contacts at any ONE instance, from /reflex_hand.
# That is the difference btw the two.
self.cloud = PointCloud()
self.cloud.header.frame_id = self.marker_frame
# Surface normal arrow marker. CUMULATIVE for entire run.
self.normals = Marker()
# Scale: LINE_LIST uses scale.x, width of line segments
# LINE_LIST draws a line btw 0-1, 2-3, 4-5, etc. pairs of points[]
# Pose is still used, just like POINTS still uses them. Make sure to set
# identity correctly, qw=1.
create_marker(Marker.LINE_LIST, 'contacts_normals', self.marker_frame,
self.msg_id, 0, 0, 0, 1, 1, 0, 0.5, 0.002, 0, 0,
self.normals, 0) # Use 0 duration for forever
########
# Bookkeeping
self.pause_recording = False
def move_delta(center, vis_pub, increment, obj_height, obj_radius_x,
obj_radius_y):
#print (center)
#print (increment)
center = center + increment
#print ('New center: %f %f %f' % (center[0], center[1], center[2]))
# Update RViz marker
marker_center = Marker()
create_marker(Marker.POINTS, 'gauge_center', '/base', 0, 0, 0, 0, 1, 1, 0,
0.8, 0.005, 0.005, 0.005, marker_center,
0) # Use 0 duration for forever
marker_center.points.append(Point(center[0], center[1], center[2]))
vis_pub.publish(marker_center)
# Object bbox
# white
marker_obj = Marker()
create_marker(
Marker.CYLINDER,
'object',
'/base',
0,
center[0],
center[1],
center[2] + obj_height * 0.5,
# Scale: X diameter, Y diameter, Z height
1,
1,
1,
0.2,
obj_radius_x * 2,
obj_radius_y * 2,
obj_height,
marker_obj,
0) # Use 0 duration for forever
vis_pub.publish(marker_obj)
return center
def plot_point_with_label (self, pt, ns, frame_id,
marker_ids, pt_text, r=0, g=1, b=0, text_height=0.02):
# POINTS Marker: scale.x is point width, scale.y is point height
marker_pt = Marker ()
create_marker (Marker.POINTS, ns, frame_id, marker_ids[0],
0, 0, 0, r, g, b, 0.5, 0.1, 0.1, 0,
marker_pt, self.marker_duration)
marker_pt.points.append (Point (pt[0], pt[1], pt[2]))
# Offset text a bit longer than head of arrow.
# TEXT Marker: only scale.z is used, height of uppercase "A"
marker_p_text = Marker ()
create_marker (Marker.TEXT_VIEW_FACING, ns, frame_id,
marker_ids[1],
pt[0] + 0.01, pt[1] + 0.01, pt[2] + 0.01,
r, g, b, 0.5, 0, 0, text_height, marker_p_text, self.marker_duration)
marker_p_text.text = pt_text
self.vis_pub.publish (marker_pt)
self.vis_pub.publish (marker_p_text)
def visualize_individual_contacts (contacts, normal_endpts, pressures,
contact_thresh,
vis_pub, frame_id, marker_ids, marker_duration=0):
# So that RViz doesn't print hundreds of errors
if not frame_id:
return
# Each iteration uses the same unique ID, for each of the 3 namespaces.
# The ID corresponds to the sensor index, 0:37. So at any given time,
# only the latest contact is displayed, for any given sensor.
for i in range (0, len (contacts)):
# Contacted sensor
# yellow
marker_con = Marker ()
# Parameters: tx, ty, tz, r, g, b, alpha, sx, sy, sz
create_marker (Marker.POINTS, 'contacts_ind', frame_id, marker_ids[i],
0, 0, 0, 1, 1, 0, 0.5, 0.01, 0.01, 0.01,
marker_con, marker_duration) # Use 0 duration for forever
marker_con.points.append (contacts [i])
# Per-point color with alpha
marker_con.colors.append (ColorRGBA (1, 1, 0,
calc_pressure_alpha (pressures [i], contact_thresh)))
# Sensor normals
# green
marker_normals = Marker ()
create_marker (Marker.LINE_LIST, 'sen_normals_ind', frame_id, marker_ids[i],
0, 0, 0, 0, 1, 0, 0.5, 0.001, 0, 0,
marker_normals, marker_duration) # Use 0 duration for forever
marker_normals.points.append (contacts [i])
marker_normals.points.append (normal_endpts [i])
# Pressure value texts
text_height = 0.01
# yellow?
marker_text = Marker ()
create_marker (Marker.TEXT_VIEW_FACING, 'pressures_ind', frame_id,
marker_ids[i],
normal_endpts [i].x, normal_endpts [i].y, normal_endpts [i].z,
1, 1, 0, 0.5, 0, 0, text_height, marker_text, marker_duration)
marker_text.text = str (pressures [i])
vis_pub.publish (marker_con)
vis_pub.publish (marker_normals)
vis_pub.publish (marker_text)
def sparsen_normals(self, normals, N_SPARSE=5):
## Randomly pick a small set of normals to pass to Pottmann
# Unique list of random indices. choice() is random sampling.
sparse_idx = np.random.choice(range(0, len(normals)),
size=N_SPARSE,
replace=False)
normals_sparse = [normals[i] for i in sparse_idx]
print('Chose %d normals out of %d:' %
(len(normals_sparse), len(normals)))
print(sparse_idx)
## Plot the sparse set of normals chosen, in a different namespace
LEN = 0.03
marker_sparse = Marker()
# green
create_marker(Marker.LINE_LIST, 'sparse_normals', '/base', 0, 0, 0, 0,
0, 1, 0, 0.5, 0.001, 0, 0, marker_sparse,
self.marker_duration) # Use 0 duration for forever
for i in range(0, len(normals_sparse)):
endpt = normals_sparse[i][0] + LEN * normals_sparse[i][1]
marker_sparse.points.append(
Point(normals_sparse[i][0][0], normals_sparse[i][0][1],
normals_sparse[i][0][2]))
marker_sparse.points.append(Point(endpt[0], endpt[1], endpt[2]))
self.vis_pub.publish(marker_sparse)
return normals_sparse
def estimate(self):
## Sanity checks
if self.PCD0_LIVE1_TXT2 == 0:
# No cloud received yet
if self.pcd_cloud is None:
return
# If this is the first iteration
if self.pcd_cloud_remain is None:
self.pcd_cloud_remain = PointCloud(self.pcd_cloud.header,
self.pcd_cloud.points,
self.pcd_cloud.channels)
elif self.PCD0_LIVE1_TXT2 == 1:
# No sensor values received yet
if self.sensor_values is None:
return
# If current contacts are invalid (happens when hand is in open/close
# motion), don't look at them.
if self.pause_contacts:
return
## Get the contact points this iteration
# PCD
if self.PCD0_LIVE1_TXT2 == 0:
# Ret val: geometry_msgs/Point32[]
[chosen_contacts, contacts_frame] = self.get_contacts_pcd()
# Live rostopic
elif self.PCD0_LIVE1_TXT2 == 1:
contacts_frame = '/base'
# Ret val: geometry_msgs/Point[]
[chosen_contacts, _, _, _,
_] = get_contacts_live(self.sensor_values, self.contact_thresh,
contacts_frame, self.tfTrans, True)
# Recorded text file
elif self.PCD0_LIVE1_TXT2 == 2:
chosen_contacts = self.in_record_reader.get_contacts_recorded()
# If no contacts, do nothing
if len(chosen_contacts) < 1:
return
# Eliminate duplicates of captured contacts
[chosen_contacts, _, isDup, _, _] = eliminate_duplicates(
self.processed_contacts_cloud.points + self.cached_contacts,
chosen_contacts)
# If all contact points are duplicates, hand is still in previous contact
# with object. Nothing new to do.
if all(isDup):
return
# TODO Now in eliminate_duplicates(). Remove this block when new fn works.
'''
# If there are any contact points detected this iteration, and if these
# are not the first ones, check if they are duplicates of captured ones.
if (len (self.processed_contacts_cloud.points) > 0 or \
len (self.cached_contacts) > 0):
# Exclude new contact points that are same as previous
# ones. This happens if the touch on object is longer than one iter,
# which is like, always!
# Bool[]
isDup = check_dup_contacts (
self.processed_contacts_cloud.points + self.cached_contacts,
chosen_contacts)
# cached_contacts: Point[]
# processed_contacts_cloud.points: Point32[]
# chosen_contacts: Point[]
# If all contact points are duplicates, hand is still in previous contact
# with object. Nothing new to do.
if all (isDup):
return
# If only some contact points are duplicates, hand has moved, we can
# process the non-duplicate contacts as normal.
chosen_contacts = [chosen_contacts[i] \
for i in range (0, len(chosen_contacts)) if not isDup[i]]
'''
## If there are any contacts in cache (cache stores points that were too
# few to process in previous iterations)
# Add cached points to current iter's points, clear cache
if len(self.cached_contacts) > 0:
chosen_contacts.extend(self.cached_contacts)
del self.cached_contacts[:]
## Plot contacts in this iteration. Per-iteration noncumulative, by same
# marker ID.
# yellow
marker_con = Marker()
# Parameters: tx, ty, tz, r, g, b, alpha, sx, sy, sz
create_marker(Marker.POINTS, 'contacts', '/base', 0, 0, 0, 0, 1, 1, 0,
0.5, 0.01, 0.01, 0.01, marker_con,
self.marker_duration) # Use 0 duration for forever
for i in range(0, len(chosen_contacts)):
marker_con.points.append(chosen_contacts[i])
self.vis_pub.publish(marker_con)
## If we haven't returned at this point yet, there are new contact points
# in this iteration.
self.n_sim_grasps += 1
## Calc centroid from contacts in current iteration
# Require at least 3 points. Else no centroid possible, by def of
# centroid.
# If got >= 3 contacts, can calc centroid, else save in cache for next iter
# when have more points.
if len(chosen_contacts) < 3:
self.cached_contacts.extend(chosen_contacts)
print('Less than 3 new contacts. Caching.')
return
else:
# Check variance in x y z components. Make sure >= 2 components
# have a good amount of variance.
if not self.check_variance(chosen_contacts):
print('Stdev too low. Caching.')
self.cached_contacts.extend(chosen_contacts)
return
# 1 geometry_msgs/Point
# Pass in > 3 geometry_msgs/Point
centroid = calc_3d_centroid(chosen_contacts)
# Add to list of contact points seen. Convert to Numpy array.
self.processed_contacts.extend ( \
[np.asarray ([p.x, p.y, p.z]) for p in chosen_contacts])
# Add to cumulative contact cloud
self.processed_contacts_cloud.points.extend(chosen_contacts)
# Add to list of centroids
self.centroids.append(centroid)
tmp_nContacts_str = format('%d' % (len(chosen_contacts)))
print(tmp_nContacts_str)
if self.out_record_flag:
# Record for testing in simulation
self.out_record_file.write(tmp_nContacts_str + '\n')
for i in range(0, len(chosen_contacts)):
tmp_contact_str = str2 = format ('%g %g %g' \
%(chosen_contacts [i].x, chosen_contacts [i].y,
chosen_contacts [i].z))
print(tmp_contact_str)
if self.out_record_flag:
self.out_record_file.write(tmp_contact_str + '\n')
print('%d contact points' % (len(self.processed_contacts)))
#print (self.processed_contacts)
print('%d centroids' % (len(self.centroids)))
## Plot contact cloud so far. Cumulative from all iterations
self.processed_contacts_cloud.header.stamp = rospy.Time.now()
self.processed_contacts_cloud.header.frame_id = '/base'
self.contacts_pub.publish(self.processed_contacts_cloud)
## Plot centroid in this iteration. Cumulative from all iterations by
# unique marker id.
# orange
marker_cen = Marker()
self.center_msg_id = self.center_msg_id + 1
create_marker(Marker.POINTS, 'centroids', '/base', self.center_msg_id,
0, 0, 0, 1, 0.5, 0, 0.5, 0.01, 0.01, 0.01, marker_cen,
self.marker_duration) # Use 0 duration for forever
marker_cen.points.append(centroid)
self.vis_pub.publish(marker_cen)
## If only have one centroid so far, no line fitting to do, just return
if len(self.centroids) < 2:
return
## Find center axis, using the centroids found
# 20 cm. Length for plotting in RViz
LINE_LEN = 0.2
if self.RANSAC0_LS1 == 1:
## Fit least squares line through the cumulative set of centroids.
# Note this isn't as robust as RANSAC line, if there are lots outliers.
# Least squares just fits to as many pts as possible, which isn't
# always the right answer.
# Fit
[vx, vy, vz, x0, y0, z0] = est_least_sqr_line(self.centroids)
# Make a start point and end point
axis_ls_start = Point(x0, y0, z0)
axis_ls_end = Point(x0 + vx * LINE_LEN, y0 + vy * LINE_LEN,
z0 + vz * LINE_LEN)
## Plot least squares fitted line through centroids. Per iteration
# non-cumulative, by using same marker ID.
# LINE_LIST marker: only scale.x is used, for width of line segments.
# orange
marker_axis_ls = Marker()
create_marker(Marker.LINE_LIST, 'center_axis', '/base',
self.LS_MARKER_ID, 0, 0, 0, 1, 0.5, 0, 0.5, 0.01, 0,
0, marker_axis_ls,
self.marker_duration) # Use 0 duration for forever
marker_axis_ls.points.append(axis_ls_start)
marker_axis_ls.points.append(axis_ls_end)
self.vis_pub.publish(marker_axis_ls)
else:
## Estimate center axis, using the sample points from current iteration
# TODO: perhaps better idea: after estimate enough centroids, just use
# all the centroids for RANSAc?? Yes!! That sounds so much more
# reasonable than taking a bunch of points on the boundary! Then it's
# just a standard "ransac line" problem!
# Ret val 1 is list of 2 indices, they index the param passed in
# Ret val 2 is list of indices of inliers, includes ret val 1.
[axis_idx, inliers_idx] = est_ransac_line(self.centroids)
# Try RANSAC with all contact points, instead of just the centroids
#[axis_idx, inliers_idx] = est_ransac_line (self.processed_contacts_cloud.points)
# Fit a least squares line through the highest voted line and inliers
vx = vy = vz = x0 = y0 = z0 = 0
if self.fit_least_sqrs_after_ransac:
inputs = [self.centroids[i] for i in inliers_idx]
# Try RANSAC with all contact points, instead of just the centroids
#inputs = [self.processed_contacts_cloud.points [i] for i in inliers_idx]
# (vx vy vz) is a unit vector
# (x0 y0 z0) is a pt on line
(vx, vy, vz, x0, y0, z0) = est_least_sqr_line(inputs)
# Don't do least squares after RANSAC
else:
pt1 = np.asarray([
self.centroids[axis_idx[0]].x,
self.centroids[axis_idx[0]].y,
self.centroids[axis_idx[0]].z
])
pt2 = np.asarray([
self.centroids[axis_idx[1]].x,
self.centroids[axis_idx[1]].y,
self.centroids[axis_idx[1]].z
])
vec = pt2 - pt1
vec = vec / np.linalg.norm(vec)
vx = vec[0]
vy = vec[1]
vz = vec[2]
x0 = pt1[0]
y0 = pt1[1]
z0 = pt1[2]
# Make a start point and end point
axis_rs_start = Point(x0, y0, z0)
# Try RANSAC with all contact points, instead of just the centroids
#axis_rs_start = calc_3d_centroid (self.processed_contacts_cloud.points)
axis_rs_end = Point(axis_rs_start.x + vx * LINE_LEN,
axis_rs_start.y + vy * LINE_LEN,
axis_rs_start.z + vz * LINE_LEN)
## Plot RANSAC fitted line. Per iteration non-cumulative, by using same
# marker ID.
# red for RANSAC
marker_axis_rs = Marker()
create_marker(Marker.LINE_LIST, 'center_axis', '/base',
self.RANSAC_MARKER_ID, 0, 0, 0, 1, 0, 0, 0.5, 0.01,
0, 0, marker_axis_rs,
self.marker_duration) # Use 0 duration for forever
marker_axis_rs.points.append(axis_rs_start)
marker_axis_rs.points.append(axis_rs_end)
self.vis_pub.publish(marker_axis_rs)
# Plot text saying how many contacts there has been
marker_n_iters = Marker()
# red
# TEXT Marker: only scale.z is used, height of uppercase "A"
# Offset text a bit longer than head of arrow.
create_marker(Marker.TEXT_VIEW_FACING, 'center_axis', '/base',
self.N_ITERS_MARKER_ID, axis_rs_end.x + 0.01,
axis_rs_end.y + 0.01, axis_rs_end.z + 0.01, 1, 0, 0,
0.5, 0, 0, 0.02, marker_n_iters,
self.marker_duration) # Use 0 duration for forever
# If publisher for n_grasps isn't running (happens when in sim), print
# n_sim_grasps, our best estimate for a "grasp"
if self.n_grasps < 0:
marker_n_iters.text = str(self.n_sim_grasps) + ' grasps'
else:
marker_n_iters.text = str(self.n_grasps) + ' grasps'
self.vis_pub.publish(marker_n_iters)
## Plot inliers in a different color than yellow
# TODO best way is to replace them, using their existing marker ID,
# instead of plotting new ones. saves memory and no overlapping points
# to confuse me.
# Use inlier_idx
## Predict and plot what the object might look like, using the estimated
# center axis and perpendicular distance of contact points to the axis.
self.predict(
np.asarray([axis_rs_start.x, axis_rs_start.y,
axis_rs_start.z]),
np.asarray([axis_rs_end.x, axis_rs_end.y, axis_rs_end.z]),
self.processed_contacts)
def visualize_contacts (contacts, normal_endpts, pressures, contact_thresh,
vis_pub, frame_id, cumu_marker_id, text_marker_ids, marker_duration=0):
if not frame_id:
rospy.logwarn ('detect_reflex_contacts.py visualize_contacts() got empty frame_id passed in. This will result in RViz warnings of source_frame in tf2 frame_ids cannot be empty. You might not get correct contact XYZ positions!')
return
# Nothing to publish. Do not publish empty markers!
if not contacts:
return
# Contacted sensors
# Per-iteration, non-cumulative marker
# yellow
marker_con = Marker ()
# Parameters: tx, ty, tz, r, g, b, alpha, sx, sy, sz
create_marker (Marker.POINTS, 'contacts_curr', frame_id, 0,
0, 0, 0, 1, 1, 0, 0.5, 0.01, 0.01, 0.01,
marker_con, marker_duration) # Use 0 duration for forever
# Cumulative marker. Difference from non-cumulative in just namespace and
# marker ID (which should be unique) here is supplied by caller each time.
# yellow
marker_con_cumu = Marker ()
# Parameters: tx, ty, tz, r, g, b, alpha, sx, sy, sz
create_marker (Marker.POINTS, 'contacts', frame_id, cumu_marker_id,
0, 0, 0, 1, 1, 0, 0.5, 0.01, 0.01, 0.01,
marker_con_cumu, marker_duration) # Use 0 duration for forever
# Sensor normals
marker_normals = Marker ()
# green
create_marker (Marker.LINE_LIST, 'sen_normals_curr', frame_id, 0,
0, 0, 0, 0, 1, 0, 0.5, 0.001, 0, 0,
marker_normals, marker_duration) # Use 0 duration for forever
marker_normals_cumu = Marker ()
# green
create_marker (Marker.LINE_LIST, 'sen_normals', frame_id, cumu_marker_id,
0, 0, 0, 0, 1, 0, 0.5, 0.001, 0, 0,
marker_normals_cumu, marker_duration) # Use 0 duration for forever
# Add points to markers
for i in range (0, len (contacts)):
# Contacted sensors
marker_con.points.append (contacts [i])
# Per-point color with alpha
marker_con.colors.append (ColorRGBA (1, 1, 0,
calc_pressure_alpha (pressures [i], contact_thresh)))
marker_con_cumu.points.append (contacts [i])
# Per-point color with alpha
marker_con_cumu.colors.append (ColorRGBA (1, 1, 0,
calc_pressure_alpha (pressures [i], contact_thresh)))
# Sensor normals
marker_normals.points.append (contacts [i])
marker_normals.points.append (normal_endpts [i])
marker_normals_cumu.points.append (contacts [i])
marker_normals_cumu.points.append (normal_endpts [i])
# Pressure value texts
# Can't do multiple texts per marker, so will need to do many markers
# yellow?
marker_text = Marker ()
create_marker (Marker.TEXT_VIEW_FACING, 'pressures_curr', frame_id, i,
normal_endpts [i].x, normal_endpts [i].y, normal_endpts [i].z,
1, 1, 0, 0.5, 0, 0, 0.02, marker_text, marker_duration)
marker_text.text = str (pressures [i])
vis_pub.publish (marker_text)
marker_text_cumu = Marker ()
create_marker (Marker.TEXT_VIEW_FACING, 'pressures', frame_id,
text_marker_ids[i],
normal_endpts [i].x, normal_endpts [i].y, normal_endpts [i].z,
1, 1, 0, 0.5, 0, 0, 0.02, marker_text_cumu, marker_duration)
marker_text_cumu.text = str (pressures [i])
vis_pub.publish (marker_text_cumu)
vis_pub.publish (marker_con)
vis_pub.publish (marker_con_cumu)
vis_pub.publish (marker_normals)
vis_pub.publish (marker_normals_cumu)
def interact_plot_3d(histdd,
tri_params,
shift=(0, 0, 0),
ns='hist',
ns_suffix='',
title=''):
#, bin_volume=1000):
if not title:
title = ns
# Cube bin width in RViz visualization. This is not the actual bin width in
# the histogram.
# TODO: Might want to scale cube using actual bin width... that requires
# passing in edgesdd too.
vis_bin_w = .01
cmap = matplotlib.cm.get_cmap('jet')
#ttl_bin_counts = float (np.sum (histdd)) / bin_volume # all blue, too big
#ttl_bin_counts = float (np.sum (histdd)) * bin_volume # product is 1, only red and blue boxes, no yellow or orange. `.` Same as not having this var!
#ttl_bin_counts = float (np.sum (histdd)) # all blue, too big
# Is the max better, for denominator of color?
ttl_bin_counts = float(
np.max(histdd)
) # faint light blue, yellow red. I think this is the most correct color scale. Biggest bin is red, others are cooler colors
#ttl_bin_counts = float (np.sum (histdd)) / 1000.0 # This produces best result from full histograms... but once turn off zero-bins, these look too red!
#ttl_bin_counts = 1
#print ('Total bin counts: %f, bin_volume %f' % (ttl_bin_counts, bin_volume))
if ttl_bin_counts == 0:
print(
'interact_plot_3d(): Total bin count is 0. Not plotting anything.')
return
vis_arr_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=2)
marker_arr = MarkerArray()
marker_id = 0
ns = ns
if ns_suffix:
ns = ns + ' ' + ns_suffix
for i in range(0, np.shape(histdd)[0]):
# Center (x, y, z) of where the 3D bin is
# Assuming axis origins at 0 0 0,
# 1st bin on x-axis is at 1*bin_width/2,
# 2nd is at 3*bin_width/2,
# 3rd is at 5*bin_width/2, etc.
bx = vis_bin_w * (2 * i + 1) * 0.5
for j in range(0, np.shape(histdd)[1]):
by = vis_bin_w * (2 * j + 1) * 0.5
for k in range(0, np.shape(histdd)[2]):
bz = vis_bin_w * (2 * k + 1) * 0.5
bin_count = histdd[i, j, k] / ttl_bin_counts
#if bin_count > 0.0:
# print (bin_count)
rgba = cmap(bin_count)
# Visualize non-zero bins (zero bins are too many dark blue cubes,
# blocks out the actual non-zero bins, even when translucent)
if bin_count > 0:
marker_bin = Marker()
create_marker(Marker.CUBE, ns, '/base', marker_id,
bx + shift[0], by + shift[1], bz + shift[2],
rgba[0], rgba[1], rgba[2], 0.2, vis_bin_w,
vis_bin_w, vis_bin_w, marker_bin,
0) # Use 0 duration for forever
marker_arr.markers.append(marker_bin)
marker_id += 1
# Draw 3 axes
axis_len = 0.1
marker_ax = Marker()
create_marker(
Marker.LINE_LIST,
ns,
'/base',
marker_id,
# scale.x is width of line
0,
0,
0,
1,
1,
1,
0.5,
0.001,
0,
0,
marker_ax,
0) # Use 0 duration for forever
marker_ax.points.append(Point(shift[0], shift[1], shift[2]))
marker_ax.points.append(Point(shift[0], shift[1], shift[2] + axis_len))
marker_ax.points.append(Point(shift[0], shift[1], shift[2]))
marker_ax.points.append(Point(shift[0], shift[1] + axis_len, shift[2]))
marker_ax.points.append(Point(shift[0], shift[1], shift[2]))
marker_ax.points.append(Point(shift[0] + axis_len, shift[1], shift[2]))
marker_arr.markers.append(marker_ax)
marker_id += 1
# Draw axis labels
fontsize = 0.01
lbl_pos = [[axis_len, 0, 0], [0, axis_len, 0], [0, 0, axis_len]]
for i in range(0, 3):
marker_lbl = Marker()
create_marker(Marker.TEXT_VIEW_FACING, ns, '/base', marker_id,
shift[0] + lbl_pos[i][0], shift[1] + lbl_pos[i][1],
shift[2] + lbl_pos[i][2], 1, 1, 1, 1, 0, 0, fontsize,
marker_lbl, 0)
marker_lbl.text = tri_params[i]
marker_arr.markers.append(marker_lbl)
marker_id += 1
# Draw title of plot
marker_text = Marker()
create_marker(
Marker.TEXT_VIEW_FACING,
ns,
'/base',
marker_id,
#shift[0], shift[1]-0.03, shift[2],
shift[0] + axis_len * 0.5,
shift[1] - 0.03 + axis_len * 0.5,
shift[2] + axis_len,
# scale.z specifies height of an uppercase A
1,
1,
1,
1,
0,
0,
fontsize,
marker_text,
0)
marker_text.text = title
marker_arr.markers.append(marker_text)
marker_id += 1
for i in range(0, 10):
vis_arr_pub.publish(marker_arr)
time.sleep(0.05)
def main():
#####
# User adjust params
#####
# True means 10 Hz pause btw each visualization. Set to False if you just want
# to see the histogram plots quickly.
doRViz = False
#####
# Init ROS
#####
rospy.init_node('sample_obj', anonymous=True)
thisNode = SampleObj()
# Prompt to display at keyboard_interface.py prompt
prompt_pub = rospy.Publisher('keyboard_interface/prompt', String)
prompt_msg = String()
prompt_msg.data = 'Press space to pause, '
vis_pub = rospy.Publisher('/visualization_marker', Marker)
cloud_pub = rospy.Publisher('/sample_obj/cloud', PointCloud)
wait_rate = rospy.Rate(10)
print('sample_obj node initialized...')
# All model files
model_path = '/Users/master/courses/15summer/MenglongModels/'
model_name = [
'axe1_c.obj', 'bigbox_c.obj', 'bottle_c.obj', 'broom_c.obj',
'brush_c.obj', 'flowerspray_c.obj', 'gastank_c.obj',
'handlebottle_c.obj', 'heavyranch1_c.obj', 'pan_c.obj', 'pipe_c.obj',
'shovel_c.obj', 'spadefork_c.obj', 'spraybottle_c.obj',
'watercan_c.obj', 'wreckbar1_c.obj'
]
#model_name = ['broom_c.obj', 'watercan_c.obj']
# For plotting histograms
ylbls = ['len1', 'len2', 'alpha']
figs = [None] * 3
axes = [None] * 3
# For each triangle parameter, make a 4 x 4 subplot grid
for i in range(0, 3):
figs[i], axes[i] = create_subplot([4, 4])
# Loop through each obj file
for obj_idx in range(0, len(model_name)):
if rospy.is_shutdown():
break
#####
# Load OBJ file
#####
# See ObjParser type here, and all the fields it contains (vertices,
# normals, tex_coords), on my MacBook Air:
# /usr/local/lib/python2.7/site-packages/pywavefront/__init__.py
# pywavefront module for Python: https://pypi.python.org/pypi/PyWavefront
model = pywavefront.Wavefront(
os.path.join(model_path, model_name[obj_idx]))
nPts = len(model.parser.vertices)
print('%d vertices' % nPts)
# Convert to numpy array so I can index using a list of indices below
# Shrink model, because orig points are in hundreds scale, too big for
# meters. I'm guessing they are in milimeters?
verts = 0.001 * np.asarray(model.parser.vertices)
#####
# Downsample cloud to 1 point every 8 mm (0.008 m)
#####
# TODO now that I don't use pcl, i'll have to do this myself. Sort point
# by euclidean somehow, and then skip every so and so points. I don't know.
# Or just bin them into a grid, and only keep 1 point per bin. That might
# be easier.
# Naive approach of downsampling (because there's toooo many points, I need
# to quickly test a downsampled model, before I have time to implement a
# real downsampling).
# Just randomly pick 10% of the points
down_ratio = 0.1
down_idx = random.sample(range(0, len(model.parser.vertices)),
int(np.floor(nPts * down_ratio)))
verts_down = verts[down_idx]
nPts_down = len(verts_down)
print('%d vertices after RANDOM downsampling' % nPts_down)
#####
# Sample a set of 3 points, many sets.
#####
nTri_exhaust = nCk(nPts, 3)
print('%d triangles needed to be exhaustive (%d choose 3)' %
(nTri_exhaust, nPts))
nTri_down_exhaust = nCk(nPts_down, 3)
print('%d triangles needed for downsampled model' %
(nTri_down_exhaust))
# Pick a realistic number. Note since sampling is not enforced to be unique
# (for now), this has to be bigger than what you think, to account for
# triangles that were sampled more than once.
nTri_down = 200 #len (verts_down)
# List of 3-point numpy arrays. Numpy arrays are
# array([[x1, y1, z1], [x2, y2, z2], [x3, y3, z3]])
samples = []
# List of triangle parameters
l1 = []
l2 = []
alpha = []
for i in range(0, nTri_down):
# Do this in a loop, instead of just sample 3 * nTri_down points. `.`
# sample() returns unique numbers.
sample_idx = random.sample(range(0, len(verts_down)), 3)
# Append a list, to group these 3 pts together
samples.append(verts_down[sample_idx])
#####
# Compute triangle connected by the 3 sampled pts, represented by 3 params
# TODO: decide what params to use, any combination (except for 3 angles) of
# 3 angles and 3 sides will work:
# Ref: https://www.mathsisfun.com/algebra/trig-solving-triangles.html
# SAS looks easiest to compute. But... what's the best for a distinctive
# 3D object descriptor though? I'd think 2 angles + 1 side... Is it?
# but that doesn't tell you as much about the length... length tells you
# about size obj. Angles are kind of arbitrary and don't tell you much
# about the object, since the 3 points are randomly sampled! You may
# very well end up with two triangles with 2 same angles, on very diff
# objs, because the location of the 3 sample points!
# This choice might also have to be empirical, in that we do all 5 cases,
# compute the distance btw histograms of all objs, and pick the
# parameterization that gives the greatest distance!
#####
# I will use SAS for now. It tells 2 side lengths, which can be informative
# about object size. 2 anglees + 1 side seems more arbitrary and not as
# distinctive, since points are randomly chosen, so angles are random too
# and can be same btw differnet objs.
# Pick the first two sides
# They must subtract by same point, in order for dot product to compute
# correctly! Else your sign may be wrong. Subtracting by same point
# puts the two vectors at same origin, w hich is what dot product
# requires, in order to give you correct theta btw the two vectors!
s1 = verts_down[sample_idx][0] - verts_down[sample_idx][1]
s2 = verts_down[sample_idx][2] - verts_down[sample_idx][1]
# Two side lengths
len_s1 = np.linalg.norm(s1)
len_s2 = np.linalg.norm(s2)
# Angle btw two vectors.
# Dot product is dot(a,b) = |a||b| cos (theta).
# dot(a,b) / (|a||b|) = cos(theta)
# theta = acos (dot(a,b) / (|a||b|))
angle = np.arccos(np.dot(s1, s2) / (len_s1 * len_s2))
# Exception case: length is 0. In this case, should really re-select the
# triangle. But for time being just let it go. TODO fix this
if np.isnan(angle) or np.isinf(angle):
print('angle was NaN or Inf. Replacing with 0')
angle = 0
if np.isnan(len_s1) or np.isinf(len_s1):
print('len_s1 was NaN or Inf. Replacing with 0')
len_s1 = 0
if np.isnan(len_s2) or np.isinf(len_s2):
print('len_s2 was NaN or Inf. Replacing with 0')
len_s2 = 0
# Add to lists for histogram descriptor calculation
l1.append(len_s1)
l2.append(len_s2)
alpha.append(angle)
#####
# Visualize sampled points and the triangle connecting them, in sequence
# in RViz
#####
cloud = PointCloud()
cloud.header.stamp = rospy.Time.now()
cloud.header.frame_id = '/'
# Create a PointCloud type, from all points in downsampled model
for i in range(0, len(verts_down)):
# PointCloud.points is Point32[] type
cloud.points.append(
Point32(verts_down[i][0], verts_down[i][1], verts_down[i][2]))
#####
# Create histogram from the 3 params, for this entire obj, i.e. for all sets
# of 3 points sampled.
# This is the final descriptor for this object.
#####
# Convert list of lists to np.array, this gives a 3 x n array. Transpose to
# get n x 3, which histogramdd() wants.
tri_params = np.array([l1, l2, alpha]).T
print(tri_params)
# Now done in plot_hist_dd.py. Delete from here when that works.
'''
# Normalize the histogram, because different objects have different number of
# points. Then histogram will have higher numbers for big object model.
# Bias is not good.
hist, edges = np.histogramdd (tri_params, bins=[10,10,18], normed=True)
#print ('Shape of 3D histogram:')
#print (np.shape (hist))
#print (np.shape (hist[0]))
#print (hist)
#print ('Shape of edges:')
#print (np.shape (edges))
#print (edges)
#####
# Plot the histogram in matplotlib
#####
# Every parameter is 3 by something, other than obj_idx, which is a scalar,
# indicating which subplot to use.
plot_hist_dd (hist, edges, figs, axes, obj_idx, ylbls)
'''
# Every parameter is 3 by something, other than tri_params which is n x 3,
# and obj_idx, which is a scalar, indicating which subplot to use.
plot_hist_dd(tri_params, [10, 10, 18], figs, axes, obj_idx, ylbls)
#####
# Visualize things in RViz, using a ROS loop
#####
seq = 0
while doRViz and not rospy.is_shutdown():
prompt_pub.publish(prompt_msg)
if not thisNode.doPause:
# Grab the next 3 sampled poseqnts
p0 = Point ( \
samples [seq] [0] [0], samples [seq] [0] [1], samples [seq] [0] [2])
p1 = Point (\
samples [seq] [1] [0], samples [seq] [1] [1], samples [seq] [1] [2])
p2 = Point (\
samples [seq] [2] [0], samples [seq] [2] [1], samples [seq] [2] [2])
# Create a marker of 3 points
marker_sample = Marker()
create_marker(Marker.POINTS, 'sample_pts', '/', 0, 0, 0, 0, 1,
0, 0, 0.8, 0.01, 0.01, 0.01, marker_sample,
0) # Use 0 duration for forever
marker_sample.points.append(p0)
marker_sample.points.append(p1)
marker_sample.points.append(p2)
# Make a copy of the marker of 3 pts, to publish in cumulative namespace.
# This lets us see how well the sampling of triangle was - did it sample
# enough triangles to cover the entire object.
marker_sample_cumu = Marker()
create_marker(Marker.POINTS, 'sample_pts_cumu', '/', seq, 0, 0,
0, 1, 1, 0, 0.8, 0.01, 0.01, 0.01,
marker_sample_cumu,
0) # Use 0 duration for forever
marker_sample_cumu.points = marker_sample.points[:]
# Create a LINE_LIST Marker for the triangle
# Simply connect the 3 points to visualize the triangle
marker_tri = Marker()
create_marker(Marker.LINE_LIST, 'sample_tri', '/', 0, 0, 0, 0,
1, 0, 0, 0.8, 0.001, 0, 0, marker_tri,
0) # Use 0 duration for forever
marker_tri.points.append(p0)
marker_tri.points.append(p1)
marker_tri.points.append(p1)
marker_tri.points.append(p2)
marker_tri.points.append(p2)
marker_tri.points.append(p0)
# Make a copy for cumulative namespace
marker_tri_cumu = Marker()
create_marker(Marker.LINE_LIST, 'sample_tri_cumu', '/', seq, 0,
0, 0, 1, 0.5, 0, 0.8, 0.001, 0, 0,
marker_tri_cumu, 0) # Use 0 duration for forever
marker_tri_cumu.points = marker_tri.points[:]
# Create text labels for sides and angle, to visually see if I calculated
# correctly.
# Ref: http://stackoverflow.com/questions/5309978/sprintf-like-functionality-in-python
# Draw text at midpoint of side
# NOTE: l1 is currently side btw pts [0] and [1]. Change if that changes.
marker_l1 = Marker()
create_marker(Marker.TEXT_VIEW_FACING, 'text', '/', 0,
(p0.x + p1.x) * 0.5, (p0.y + p1.y) * 0.5,
(p0.z + p1.z) * 0.5, 1, 0, 0, 0.8, 0, 0, 0.02,
marker_l1, 0)
marker_l1.text = '%.2f' % l1[seq]
# Draw text at midpoint of side
# NOTE: l2 is currently side btw pts [1] and [2]. Change if that changes.
marker_l2 = Marker()
create_marker(Marker.TEXT_VIEW_FACING, 'text', '/', 1,
(p1.x + p2.x) * 0.5, (p1.y + p2.y) * 0.5,
(p1.z + p2.z) * 0.5, 1, 0, 0, 0.8, 0, 0, 0.02,
marker_l2, 0)
marker_l2.text = '%.2f' % l2[seq]
# NOTE: Angle currently is btw the sides [0][1] and [1][2], so plot angle at
# point [1]. If change definition of angle, need to change this too!
marker_alpha = Marker()
create_marker(Marker.TEXT_VIEW_FACING, 'text', '/', 2, p1.x,
p1.y, p1.z, 1, 0, 0, 0.8, 0, 0, 0.02,
marker_alpha, 0)
marker_alpha.text = '%.2f' % (alpha[seq] * 180.0 / np.pi)
# Publish .obj model cloud
cloud_pub.publish(cloud)
# Publish sampled points and triangle.
# To see current selected sample, only enabe namespaces without the _cumu
# suffix, i.e. enable sample_pts and sample_tri in RViz.
vis_pub.publish(marker_sample)
vis_pub.publish(marker_sample_cumu)
vis_pub.publish(marker_tri)
vis_pub.publish(marker_tri_cumu)
# Text labels
vis_pub.publish(marker_l1)
vis_pub.publish(marker_l2)
vis_pub.publish(marker_alpha)
# Update book-keeping for next iter
seq += 1
if seq >= len(samples):
#seq = 0
break
# Don't flip flag yet, flip it in outer loop
if thisNode.doSkip:
break
if thisNode.doTerminate:
break
# ROS loop control
try:
wait_rate.sleep()
except rospy.exceptions.ROSInterruptException, err:
break
# Check termination again in outer loop
if thisNode.doSkip:
thisNode.doSkip = False
continue
if thisNode.doTerminate:
break
# ROS loop control
try:
wait_rate.sleep()
except rospy.exceptions.ROSInterruptException, err:
break
def visualize_one_point(self,
curr_pt,
curr_vec,
curr_q,
curr_mat,
marker_arr,
p_i,
frame_id,
duration=0,
quat_wrt=(1.0, 0.0, 0.0),
vec_inward=True,
vis_quat=True,
vis_idx=True,
vis_frames=False,
extra_rot_if_iden=None):
# Plot RGB little frames at each point on ellipsoid, 1 cm long
frm_len = 0.04
alpha = 0.7
frms_alpha = 0.4
#r = np.cos (p_i) / 180.0 * np.pi)
#g = np.sin (p_i) / 180.0 * np.pi)
#b = np.fabs (r - g)
# Take p_i divided by total number of points, to get [0, 1], then
# multiply by 2 * pi, so we stay in 360 range, to get a unique color
# per point.
# Blue-yellow color theme
#r = p_i / float (len (self.surf_pts))
#g = p_i / float (len (self.surf_pts))
#b = .5
# Green-magenta color theme
r = p_i / float(len(self.surf_pts))
g = .5
b = p_i / float(len(self.surf_pts))
#print ('r %.1f g %.1f b %.1f' % (r, g, b))
marker_p = Marker()
create_marker(Marker.POINTS, 'ellipsoid_pts', frame_id, p_i, 0, 0, 0,
r, g, b, alpha, 0.01, 0.01, 0.01, marker_p,
duration) # Use 0 duration for forever
marker_p.points.append(Point(curr_pt[0], curr_pt[1], curr_pt[2]))
marker_arr.markers.append(marker_p)
# Visualize the quaternion to make sure it's correct
if vis_quat:
marker_v = Marker()
# Set where to start the arrow
if not vec_inward:
v_x = self.cx
v_y = self.cy
v_z = self.cz
else:
v_x = curr_pt[0]
v_y = curr_pt[1]
v_z = curr_pt[2]
# Rotate the quaternion to the axis requested to be visualized.
# `.` curr_q is a regular quaternion, by default is from identity,
# which is x-axis (1, 0, 0).
# Get the rotation between x-axis (1 0 0) to the desired axis to be
# visualized. e.g. if quat_wrt=(0 0 1), aug_q should be a rotation of
# -90 wrt y.
aug_q, aug_mat = get_relative_rotation_v((1.0, 0.0, 0.0), quat_wrt)
#print (aug_mat)
# Apply the additional rotation to curr_q, which is orientation of x-axis.
# This will make a quaternion that rotates to the desired axis to be
# visualized.
curr_q_aug = tf.transformations.quaternion_multiply(curr_q, aug_q)
create_marker(
Marker.ARROW,
'ellipsoid_vec',
frame_id,
p_i,
v_x,
v_y,
v_z,
r,
g,
b,
alpha,
# scale.x is length, scale.y is arrow width, scale.z is arrow height
np.linalg.norm(curr_vec),
0.02,
0.02,
marker_v,
duration, # Use 0 duration for forever
qw=curr_q_aug[3],
qx=curr_q_aug[0],
qy=curr_q_aug[1],
qz=curr_q_aug[2])
marker_arr.markers.append(marker_v)
# For RGB frame; and for offsetting text index label so they don't all
# overlap, if the same position has multiple orientations to be plotted.
# Multiply basis axes by the rotation matrix, to get the rotated
# frame's basis axes.
x_ax = np.dot(curr_mat[0:3, 0:3], np.array([1.0, 0.0, 0.0]))
x_ax = x_ax / np.linalg.norm(x_ax) * frm_len
# RGB frame that the rotation would create
if vis_frames:
y_ax = np.dot(curr_mat[0:3, 0:3], np.array([0.0, 1.0, 0.0]))
y_ax = y_ax / np.linalg.norm(y_ax) * frm_len
z_ax = np.dot(curr_mat[0:3, 0:3], np.array([0.0, 0.0, 1.0]))
z_ax = z_ax / np.linalg.norm(z_ax) * frm_len
marker_x_frm = Marker()
marker_y_frm = Marker()
marker_z_frm = Marker()
create_marker(
Marker.ARROW,
'ellipsoid_frms_x',
frame_id,
p_i,
# scale.x shaft diameter, scale.y head diameter, scale.z head length
0,
0,
0,
1,
0,
0,
frms_alpha,
0.007,
0.009,
0,
marker_x_frm,
duration)
create_marker(Marker.ARROW, 'ellipsoid_frms_y', frame_id, p_i, 0,
0, 0, 0, 1, 0, frms_alpha, 0.007, 0.009, 0,
marker_y_frm, duration)
create_marker(Marker.ARROW, 'ellipsoid_frms_z', frame_id, p_i, 0,
0, 0, 0, 0, 1, frms_alpha, 0.007, 0.009, 0,
marker_z_frm, duration)
marker_x_frm.points.append(
Point(curr_pt[0], curr_pt[1], curr_pt[2]))
marker_x_frm.points.append(
Point(curr_pt[0] + x_ax[0], curr_pt[1] + x_ax[1],
curr_pt[2] + x_ax[2]))
marker_y_frm.points.append(
Point(curr_pt[0], curr_pt[1], curr_pt[2]))
marker_y_frm.points.append(
Point(curr_pt[0] + y_ax[0], curr_pt[1] + y_ax[1],
curr_pt[2] + y_ax[2]))
marker_z_frm.points.append(
Point(curr_pt[0], curr_pt[1], curr_pt[2]))
marker_z_frm.points.append(
Point(curr_pt[0] + z_ax[0], curr_pt[1] + z_ax[1],
curr_pt[2] + z_ax[2]))
marker_arr.markers.append(marker_x_frm)
#marker_arr.markers.append (marker_y_frm)
marker_arr.markers.append(marker_z_frm)
# Text showing point index
if vis_idx:
marker_t = Marker()
# Offset to be above x-axis tip, so that when the same point on ellipsoid
# has multiple orientations (in the case of custom points edited in
# caller of this class), the texts don't all overlap.
create_marker(Marker.TEXT_VIEW_FACING, 'ellipsoid_idx', frame_id,
p_i, curr_pt[0] + x_ax[0], curr_pt[1] + x_ax[1],
curr_pt[2] + x_ax[2] + 0.01, r, g, b, alpha, 0, 0,
0.03, marker_t, duration)
marker_t.text = '%d' % p_i
marker_arr.markers.append(marker_t)
return marker_arr
def visualize_wrist_pose(p_wrist, q_wrist, tri_center, obj_center, pt0, pt1,
pt2, tri_idx, vis_arr_pub):
# Marker code copied from geo_ellipsoid.py, where I also visualize a
# quaternion with an arrow! That has the correct code.
marker_arr = MarkerArray()
frame_id = '/base'
alpha = 0.8
duration = 0
# Plot a cyan square at wrist position
marker_p = Marker()
create_marker(Marker.POINTS, 'wrist_pos', frame_id, 0, 0, 0, 0, 0, 1, 1,
alpha, 0.005, 0.005, 0.005, marker_p,
duration) # Use 0 duration for forever
marker_p.points.append(Point(p_wrist[0], p_wrist[1], p_wrist[2]))
marker_arr.markers.append(marker_p)
# Add a copy to cumulative markers
marker_p = Marker()
create_marker(Marker.POINTS, 'wrist_pos_cumu', frame_id, tri_idx, 0, 0, 0,
0, 1, 1, alpha, 0.002, 0.002, 0.002, marker_p,
duration) # Use 0 duration for forever
marker_p.points.append(Point(p_wrist[0], p_wrist[1], p_wrist[2]))
marker_arr.markers.append(marker_p)
# Plot a cyan arrow for orientation. Arrow starts at wrist orientation, ends
# at average center of current triangle.
marker_q = Marker()
create_marker(
Marker.ARROW,
'wrist_quat',
frame_id,
0,
p_wrist[0],
p_wrist[1],
p_wrist[2],
0,
1,
1,
alpha,
# scale.x is length, scale.y is arrow width, scale.z is arrow height
np.linalg.norm(tri_center - p_wrist),
0.002,
0,
marker_q,
duration, # Use 0 duration for forever
qw=q_wrist[3],
qx=q_wrist[0],
qy=q_wrist[1],
qz=q_wrist[2])
marker_arr.markers.append(marker_q)
# Plot a cyan arrow from average center of current triangle, to average
# center of object.
marker_out = Marker()
create_marker(
Marker.ARROW,
'tri_normal',
frame_id,
0,
0,
0,
0,
0,
1,
1,
alpha,
# scale.x is shaft diameter, scale.y is arrowhead diameter, scale.z is
# arrowhead length if non-zero
0.001,
0.002,
0,
marker_out,
duration) # Use 0 duration for forever
marker_out.points.append(Point(obj_center[0], obj_center[1],
obj_center[2]))
marker_out.points.append(Point(tri_center[0], tri_center[1],
tri_center[2]))
marker_arr.markers.append(marker_out)
# Plot current triangle, red
marker_tri_l = Marker()
create_marker(
Marker.LINE_STRIP,
'sample_tri',
frame_id,
0,
# scale.x is width of line
0,
0,
0,
1,
0,
0,
alpha,
0.001,
0,
0,
marker_tri_l,
duration) # Use 0 duration for forever
marker_tri_l.points.append(Point(pt0[0], pt0[1], pt0[2]))
marker_tri_l.points.append(Point(pt1[0], pt1[1], pt1[2]))
marker_tri_l.points.append(Point(pt2[0], pt2[1], pt2[2]))
marker_tri_l.points.append(Point(pt0[0], pt0[1], pt0[2]))
marker_arr.markers.append(marker_tri_l)
marker_tri_p = Marker()
create_marker(Marker.POINTS, 'sample_pts', frame_id, 0, 0, 0, 0, 1, 0, 0,
alpha, 0.005, 0.005, 0.005, marker_tri_p,
duration) # Use 0 duration for forever
marker_tri_p.points.append(Point(pt0[0], pt0[1], pt0[2]))
marker_tri_p.points.append(Point(pt1[0], pt1[1], pt1[2]))
marker_tri_p.points.append(Point(pt2[0], pt2[1], pt2[2]))
marker_arr.markers.append(marker_tri_p)
vis_arr_pub.publish(marker_arr)
# Pause a bit, to let msg get pushed through
rospy.sleep(0.1)
def gen_cylinder(self):
## Constants to define the shape
# Center point in base
base_pt = np.array([0.1, 0.1, 0.1])
# 30 cm
height = 0.3
# 1 cm
height_step = 0.01
height_range = np.arange(0, height, height_step)
# Just 2 heights, for testing util_geometry est_linear_complex(). Simpler
# for me to see. Actually harder for algo too.
#height_range = [0, height]
n_slices = len(height_range)
# 5 cm
radius = 0.05
# 10 degrees
radians_step = 10.0 / 180.0 * np.pi
# Just replicate the same radius, for a straight cylinder
#radii = [radius] * n_slices
# Generate some random radii
radius_max = 0.05
radii = np.random.rand(n_slices, 1) * radius_max
# Sort the radii to get a smooth shape
radii.sort(0)
## Generate the shape, upright, in the tilted frame
obj_frame = '/obj'
# For now, test on an upright cylinder. Axis is z-axis.
axis_unit = np.array([0, 0, 1])
# Points at center of cylinder bottom and top
axis_pt1 = base_pt
axis_pt2 = base_pt + height * axis_unit
# Generate cloud
cloud = PointCloud()
n_pts_per_slice = spin_cloud(axis_pt1, axis_pt2, height_range, radii,
obj_frame, cloud, 1)
self.cloud_spun_pub.publish(cloud)
## Convert the upright object from tilted /obj frame to robot /base frame.
# Now the object should be a tilted object in /base frame.
# This is to test if Pottmann algo's implementation est_linear_complex()
# makes any upright assumptions of object. If it does, then result
# will plot incorrectly. It shouldn't.
cloud_tilt = PointCloud()
cloud_tilt.header.frame_id = '/base'
for i in range(0, len(cloud.points)):
pt_tilt = tf_get_pose(cloud.header.frame_id,
cloud_tilt.header.frame_id,
cloud.points[i].x, cloud.points[i].y,
cloud.points[i].z, 0, 0, 0, 0, self.tfTrans,
True, None, False)
cloud_tilt.points.append(pt_tilt.point)
cloud_tilt.header.stamp = rospy.Time.now()
## Print ground truth axis in /base frame
axis_pt1_tilt = tf_get_pose(cloud.header.frame_id,
cloud_tilt.header.frame_id, axis_pt1[0],
axis_pt1[1], axis_pt1[2], 0, 0, 0, 0,
self.tfTrans, True, None, False)
axis_pt2_tilt = tf_get_pose(cloud.header.frame_id,
cloud_tilt.header.frame_id, axis_pt2[0],
axis_pt2[1], axis_pt2[2], 0, 0, 0, 0,
self.tfTrans, True, None, False)
axis_unit_tilt_np = np.asarray([
axis_pt2_tilt.point.x - axis_pt1_tilt.point.x,
axis_pt2_tilt.point.y - axis_pt1_tilt.point.y,
axis_pt2_tilt.point.z - axis_pt1_tilt.point.z
])
axis_unit_tilt_np = axis_unit_tilt_np / np.linalg.norm(
axis_unit_tilt_np)
print('Truth unit axis: ' + str(axis_unit_tilt_np))
# Draw ground truth axis in /base frame
# green
marker_axis = Marker()
create_marker(Marker.LINE_LIST, 'center_axis', '/base',
self.TRUE_AXIS_ID, 0, 0, 0, 0, 1, 0, 0.5, 0.01, 0, 0,
marker_axis,
self.marker_duration) # Use 0 duration for forever
marker_axis.points.append(axis_pt1_tilt.point)
marker_axis.points.append(axis_pt2_tilt.point)
self.vis_pub.publish(marker_axis)
## Get rough normals, for the tilted object
marker = Marker()
# green
# Use base frame, to be wrt robot. This tilting shape is to test if any
# function makes any upright assumptions. They shouldn't. If they do,
# then the normals will plot incorrectly.
create_marker(Marker.LINE_LIST, 'normals', '/base',
self.NORMALS_MARKER_ID, 0, 0, 0, 0, 1, 0, 0.5, 0.001, 0,
0, marker,
self.marker_duration) # Use 0 duration for forever
# Get normals of generated shape
normals = calc_rough_normals(axis_pt1, axis_pt2, cloud_tilt,
n_pts_per_slice, marker)
self.vis_pub.publish(marker)
return (normals, axis_unit_tilt_np)
def estimate(self, normals, axis_true=None):
if not self.PLOT_ALL_AXES:
## Estimate center axis
(axis_pt1, axis_pt2) = est_linear_complex(normals)
## Draw the axis returned by est_linear_complex()
# red
marker_axis = Marker()
create_marker(Marker.LINE_LIST, 'center_axis', '/base',
self.EST_AXIS_ID, 0, 0, 0, 1, 0, 0, 0.5, 0.01, 0, 0,
marker_axis,
self.marker_duration) # Use 0 duration for forever
marker_axis.points.append(
Point(axis_pt1[0], axis_pt1[1], axis_pt1[2]))
marker_axis.points.append(
Point(axis_pt2[0], axis_pt2[1], axis_pt2[2]))
self.vis_pub.publish(marker_axis)
else:
# Test every eigvec
for i in range(0, 6):
# Parameter: [[(x1 y1 z1), (x2 y2 z2)]_1, .... [(x1 y1 z1), (x2 y2 z2)]_n]
# A list of k lists of size-2 Numpy arrays. Each size-2 Numpy array
# represents a line, described by 2 points on the line.
# [] denotes Python list, () denotes Numpy.
# lines[i][0] is line i point 1, lines[i][1] is line i point 2.
(axis_pt1, axis_pt2) = est_linear_complex(normals, i)
# Check correctness of estimate, against ground truth
# Unit vector of estimated axis
axis_unit = axis_pt2 - axis_pt1
axis_unit = axis_unit / np.linalg.norm(axis_unit)
# Default color: red
if i == 0:
r = 1
g = 0
b = 0
elif i == 1:
r = 1
g = 1
b = 1
elif i == 2:
r = 0
g = 0
b = 1
elif i == 3:
r = 1
g = 1
b = 0
elif i == 4:
r = 0
g = 1
b = 1
elif i == 5:
r = 1
g = 0
b = 1
# If dot prod of true axis and estimated axis is +/- 1, then cos == 1,
# meaning estimated axis is correct
if (axis_true is not None) and \
(abs (abs (axis_true.dot (axis_unit)) - 1) < 1e-6):
print('Correct axis has eigval index %d' % i)
# Correct color: green
r = 0
g = 1
b = 0
# Draw the axis returned by est_linear_complex()
# red
marker_axis = Marker()
create_marker(
Marker.LINE_LIST, 'center_axis', '/base', i, 0, 0, 0, r, g,
b, 0.5, 0.01, 0, 0, marker_axis,
self.marker_duration) # Use 0 duration for forever
marker_axis.points.append(
Point(axis_pt1[0], axis_pt1[1], axis_pt1[2]))
marker_axis.points.append(
Point(axis_pt2[0], axis_pt2[1], axis_pt2[2]))
self.vis_pub.publish(marker_axis)