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)