def lidar_callback(self, data): T = self.tf_buffer.lookup_transform("lidar", "velodyne", rospy.Time()) data.intensities = [1] * len(data.ranges) pc = self.laser_projection.projectLaser( data, channel_options=self.laser_projection.ChannelOption.INTENSITY) pc_transformed = do_transform_cloud(pc, T) pc_transformed.header.frame_id = "velodyne" pc_transformed.is_dense = True self.lidar_pc2_pub.publish(pc_transformed)
def project_point_cloud(velodyne, img_msg, image_pub): # Read image using CV bridge try: img = CV_BRIDGE.imgmsg_to_cv2(img_msg, 'bgr8') except CvBridgeError as e: rospy.logerr(e) return # Transform the point cloud try: transform = TF_BUFFER.lookup_transform('world', 'velodyne', rospy.Time()) velodyne = do_transform_cloud(velodyne, transform) except tf2_ros.LookupException: pass # Extract points from message points3D = ros_numpy.point_cloud2.pointcloud2_to_array(velodyne) points3D = np.asarray(points3D.tolist()) # Group all beams together and pick the first 4 columns for X, Y, Z, intensity. if OUSTER_LIDAR: points3D = points3D.reshape(-1, 9)[:, :4] # Filter points in front of camera inrange = np.where((points3D[:, 2] > 0) & (points3D[:, 2] < 6) & (np.abs(points3D[:, 0]) < 6) & (np.abs(points3D[:, 1]) < 6)) max_intensity = np.max(points3D[:, -1]) points3D = points3D[inrange[0]] # Color map for the points cmap = matplotlib.cm.get_cmap('jet') colors = cmap(points3D[:, -1] / max_intensity) * 255 # Project to 2D and filter points within image boundaries points2D = [ CAMERA_MODEL.project3dToPixel(point) for point in points3D[:, :3] ] points2D = np.asarray(points2D) inrange = np.where((points2D[:, 0] >= 0) & (points2D[:, 1] >= 0) & (points2D[:, 0] < img.shape[1]) & (points2D[:, 1] < img.shape[0])) points2D = points2D[inrange[0]].round().astype('int') # Draw the projected 2D points for i in range(len(points2D)): cv2.circle(img, tuple(points2D[i]), 2, tuple(colors[i]), -1) # Publish the projected points image try: image_pub.publish(CV_BRIDGE.cv2_to_imgmsg(img, "bgr8")) except CvBridgeError as e: rospy.logerr(e)
def callback(self, msg): try: transform = self.tf_buffer.lookup_transform(self.tracking_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(2)) except tf2.LookupException as ex: rospy.logwarn(ex) return except tf2.ExtrapolationException as ex: rospy.logwarn(ex) return cloud_out = do_transform_cloud(msg,transform) self.publisher.publish(cloud_out)
def _callback_masks(self, msg: Image, publisher: rospy.Publisher, encoding='8UC1', scale: float = 1.0): """ Projects a mask from the input image as a pointcloud on the field plane. """ # Get field plane field = self.get_plane(msg.header.stamp, 0.0) if field is None: return # Convert subsampled image image = cv2.resize(self._cv_bridge.imgmsg_to_cv2(msg, encoding), (0, 0), fx=scale, fy=scale, interpolation=cv2.INTER_NEAREST) # Get indices for all non 0 pixels (the pixels which should be displayed in the pointcloud) point_idx_tuple = np.where(image != 0) # Restructure index tuple to a array point_idx_array = np.empty((point_idx_tuple[0].shape[0], 3)) point_idx_array[:, 0] = point_idx_tuple[1] point_idx_array[:, 1] = point_idx_tuple[0] # Project the pixels onto the field plane points_on_plane_from_cam = self._get_field_intersection_for_pixels( point_idx_array, field, scale=scale) # Make a pointcloud2 out of them pc_in_image_frame = pc2.create_cloud_xyz32(msg.header, points_on_plane_from_cam) # Lookup the transform from the camera to the field plane try: trans = self._tf_buffer.lookup_transform( self._publish_frame, self._camera_info.header.frame_id, msg.header.stamp) except tf2_ros.LookupException as ex: rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex)) return except tf2_ros.ExtrapolationException as ex: rospy.logwarn_throttle(5.0, rospy.get_name() + ": " + str(ex)) return # Transform the whole point cloud accordingly pc_relative = do_transform_cloud(pc_in_image_frame, trans) # Publish point cloud publisher.publish(pc_relative)
def kinect_cb(self, pcl_msg): min_z = float('inf') points = [] for ix, p in enumerate( read_points(pcl_msg, field_names=('x', 'y', 'z'), skip_nans=True)): if p[2] < min_z: min_z = p[2] points.append( p if abs(p[1]) < self.epsilon else [np.nan, np.nan, np.nan]) points_np = np.array(points) indices = points_np[:, 0].argsort() points_np = points_np[indices] first_nan = np.where(np.isnan(points_np[:, 0]))[0][0] indices = indices[:first_nan] # points_np = points_np[:first_nan] # self.plot_distance(points_np) self.vertical_pub.publish(min_z) source_frame = pcl_msg.header.frame_id lookup_time = pcl_msg.header.stamp try: trans = self.tf_buffer.lookup_transform(self.robot_frame, source_frame, lookup_time, rospy.Duration(1.0)) except tf2.LookupException as ex: rospy.logwarn(ex) return except tf2.ExtrapolationException as ex: rospy.logwarn(ex) return pcl_robot = do_transform_cloud(pcl_msg, trans) points_robot = [] for ix, p in enumerate( read_points(pcl_robot, field_names=('y', 'z'), skip_nans=True)): points_robot.append(p) points_robot_np = np.array(points_robot) points_robot_np = points_robot_np[indices] below_thresh = points_robot_np[:, 1] < self.z_threshold if np.any(below_thresh): corner_ix = np.argmax(below_thresh) lateral_distance = points_np[corner_ix][0] else: lateral_distance = 1.0 self.lateral_pub.publish(lateral_distance)
def find_edge(pcl_msg): source_frame = pcl_msg.header.frame_id lookup_time = pcl_msg.header.stamp trans_robot = self.tf_buffer.lookup_transform(self.robot_frame, source_frame, lookup_time, rospy.Duration(5.0)) trans_world = self.tf_buffer.lookup_transform(self.world_frame, source_frame, lookup_time, rospy.Duration(5.0)) pcl_robot = do_transform_cloud(pcl_msg, trans_robot) pcl_world = do_transform_cloud(pcl_msg, trans_world) points_y = np.squeeze(np.array(list(read_points(pcl_robot, field_names=('y'), skip_nans=True)))) points_z = np.squeeze(np.array(list(read_points(pcl_world, field_names=('z'), skip_nans=True)))) indices_sorted = points_y.argsort() points_y = points_y[indices_sorted] points_z = points_z[indices_sorted] max_d['z'] = max(max_d['z'], points_z.max()) edge_y = -1.0 for y, z in zip(points_y, points_z): if z > self.z_threshold: edge_y = y return edge_y
def transform_pointcloud(pointcloud): trans = TransformStamped() trans.header.stamp = pointcloud.header.stamp trans.header.frame_id = pointcloud.header.frame_id trans.header.seq = pointcloud.header.seq trans.child_frame_id = "rotated" trans.transform.translation.x = 0 trans.transform.translation.y = 0 trans.transform.translation.z = 0 trans.transform.rotation.x = 0 trans.transform.rotation.y = 0 trans.transform.rotation.z = -0.022 trans.transform.rotation.w = 1.0 newpointcloud = do_transform_cloud(pointcloud, trans) return newpointcloud
def point_cloud_callback(self, msg): lookup_time = msg.header.stamp + rospy.Duration(self.config.offset_lookup_time) target_frame = msg.header.frame_id if self.config.target_frame == "" else self.config.target_frame source_frame = msg.header.frame_id if self.config.source_frame == "" else self.config.source_frame try: trans = self.tf_buffer.lookup_transform(target_frame, source_frame, lookup_time, rospy.Duration(self.config.timeout)) except tf2.LookupException as ex: rospy.logwarn(str(lookup_time.to_sec())) rospy.logwarn(ex) return except tf2.ExtrapolationException as ex: rospy.logwarn(str(lookup_time.to_sec())) rospy.logwarn(ex) return cloud_out = do_transform_cloud(msg, trans) self.pub.publish(cloud_out)
def laser_cb(self, msg = LaserScan()): now = rospy.Time.now() if now - self.ts_old > rospy.Duration(0.05): cloud_out = self.laser_projection.projectLaser(msg) #print(rospy.Time.now()) (trans, rot) = self.tf_listener.lookupTransform("/map", "pr1/left_laser_link",rospy.Time(0)) self.transform.transform.translation.x = trans[0] self.transform.transform.translation.y = trans[1] self.transform.transform.rotation.x = rot[0] self.transform.transform.rotation.y = rot[1] self.transform.transform.rotation.z = rot[2] self.transform.transform.rotation.w = rot[3] cloud_transformed = do_transform_cloud(cloud_out, self.transform) #self.pc2_pub.publish(cloud_transformed) pc_np_transformed = ros_numpy.point_cloud2.pointcloud2_to_xyz_array(cloud_transformed, remove_nans=True) # for i in range(0,len(pc_np_transformed),15): # #print("index: ", i) # status = self.br.sendTransform((pc_np_transformed[i][0], pc_np_transformed[i][1], 0), # tf.transformations.quaternion_from_euler(0, 0, 0), # rospy.Time.now(), # "scan" + str(i), # "map") matches = 0 for i in range(0,len(pc_np_transformed),5): for j in range(0,len(self.obstacles_x),6): if (abs(pc_np_transformed[i][0] - self.obstacles_x[j]) < 0.15) and (abs(pc_np_transformed[i][1] - self.obstacles_y[j]) < 0.15): matches += 1 # self.br.sendTransform((pc_np_transformed[i][0], pc_np_transformed[i][1], 0), # tf.transformations.quaternion_from_euler(0, 0, 0), # rospy.Time.now(), # "match_" + str(i), # "map") break print(matches) #print(self.obstacles_x, self.obstacles_y) now2 = rospy.Time.now() print("rechenzeit: ", (now2 - now)* 0.000001) self.ts_old = now
def geometry_to_cloud2(self, fore, left, right, chin): fore_cloud = [(fore.translation.x)/1000, (fore.translation.y)/1000, (fore.translation.z)/1000] left_cloud = [(left.translation.x)/1000, (left.translation.y)/1000, (left.translation.z)/1000] right_cloud = [(right.translation.x)/1000, (right.translation.y)/1000, (right.translation.z)/1000] chin_cloud = [(chin.translation.x)/1000, (chin.translation.y)/1000, (chin.translation.z)/1000] #cloud_header header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'cloud_map' cloud = [fore_cloud, left_cloud, right_cloud, chin_cloud] #create pcl2 clouds from points scaled_pcl = pcl2.create_cloud_xyz32(header, cloud) transformed_cloud = do_transform_cloud(scaled_pcl, self.transformer) self.handle_head_pose(self.supername) self.pcl_pub.publish(transformed_cloud)
def pcloud_transform(self, cloud, target_frame): """ Function to transform point cloud into a desired frame """ try: source_frame = cloud.header.frame_id #Get current frame trans = self.tfBuffer.lookup_transform( target_frame, source_frame, rospy.Time.now(), rospy.Duration( 1.0)) #get transform from one transform to the other newcloud = do_transform_cloud(cloud, trans) #transform our pointcloud2 except (tf2_ros.ExtrapolationException, tf2_ros.LookupException, tf2_ros.ConnectivityException) as e: #if something goes wrong? print e #print the exception newcloud = None #and ignore this step, continuing on to the next pointcloud return newcloud #return points and cloud
def callback(self, msg): lookup_time = msg.header.stamp target_frame = self.target_frame source_frame = msg.header.frame_id try: trans = self.tf_buffer.lookup_transform(target_frame, source_frame, lookup_time, rospy.Duration(0.0)) except tf2.LookupException as ex: rospy.logwarn(str(lookup_time.to_sec())) rospy.logwarn(ex) return except tf2.ExtrapolationException as ex: rospy.logwarn(str(lookup_time.to_sec())) rospy.logwarn(ex) return cloud_out = do_transform_cloud(msg, trans) self.pub.publish(cloud_out)
def get_panel_bbox(self): if self.last_panel_points_msg is None: return None try: transform = self.tf_buffer.lookup_transform_full( self.sub.last_image_header.frame_id, self.sub.last_image_header.stamp, self.last_panel_points_msg.header.frame_id, self.last_panel_points_msg.header.stamp, "enu", timeout=rospy.Duration(1)) except tf2_ros.TransformException as e: rospy.logwarn(e) return None transformed_cloud = do_transform_cloud(self.last_panel_points_msg, transform) points = np.array(list(sensor_msgs.point_cloud2.read_points(transformed_cloud, skip_nans=True))) if len(points) < 4: rospy.logwarn('less than 4 points') return None if self.camera_model is None: rospy.logwarn('no camera model') return None roi = roi_enclosing_points(self.camera_model, points) if roi is None: rospy.logwarn('No points project into camera.') return None rect = rect_from_roi(roi) ul, br = rect xmin, ymin = ul xmax, ymax = br x_shrink_pixels = 15 y_shrink_pixels = x_shrink_pixels xmin += x_shrink_pixels xmax -= x_shrink_pixels ymin += y_shrink_pixels ymax -= y_shrink_pixels new_rect = ((xmin, ymin), (xmax, ymax)) bbox_contour = bbox_countour_from_rectangle(new_rect) return bbox_contour
def pcListener(self, message): self.point_cloud = message try: self.transform = self.tfBuffer.lookup_transform( "usb_cam", "royale_camera_optical_frame", rospy.Time()) # check to see if transform is the same as last time, if it is, delete it if (self.transform.transform.translation.x == self.prev_transform): self.transform = 0 # update the previous transform self.prev_transform = self.transform.transform.translation.x except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): self.transform = 0 if (self.transform != 0): out_cloud = do_transform_cloud(self.point_cloud, self.transform) print(self.transform) self.pub.publish(out_cloud) else: print("Transform Error")
def merge(self, *args): """ merge received point clouds also preprocess: delete points that are outside of the boundary """ # take any number of input arguments merged_points = [] for points in args: # obtain the tf transform for this link #trans = self.tf_buffer.lookup_transform('world', points.header.frame_id, points.header.stamp, rospy.Duration(1)) print('points header frame:') print(points.header.frame_id) trans = self.tf_buffer.lookup_transform('world', points.header.frame_id, rospy.Time(0), rospy.Duration(1)) # ask for last known info points = do_transform_cloud(points, trans) points_list = [] for p in pcl2.read_points(points, field_names = ("x", "y", "z"), skip_nans=True): # check boundary info ### TODO: edit this boundary checking for customization """ if p[0] >= -0.5 and p[0] <= 0.5 and p[1] >= -0.5 and p[1] <= 0.5: # remove this point cloud, otherwise can't generate collision-free planning continue if p[2] >= 1.8: # remove camera point cloud continue """ # subsample value = np.random.uniform(low=0.,high=1.0) if value <= self.sub_sample_ratio: points_list.append([p[0],p[1],p[2]]) merged_points += points_list print('length of point cloud: %d' % (len(merged_points))) #merged_points = filter(merged_points, 0.01, 0.1) header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'world' #create pcl from points merged_pointcloud2 = pcl2.create_cloud_xyz32(header, merged_points) #publish self.merge_pub.publish(merged_pointcloud2)
def global_coord(): global X,Y,state_,position_,pc,point_list tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) transform = tf_buffer.lookup_transform("odom","sensor_laser", rospy.Time(),rospy.Duration(5.0)) pcd2 = do_transform_cloud(pc, transform) point_list = pc2.read_points(pcd2) x = [] y = [] for point in point_list: x.append(point[0]) y.append(point[1]) X = np.array(x) Y = np.array(y) X = np.reshape(np.abs((X+1.434)/0.18),(720,1)) Y = np.reshape(np.abs((Y-1.434)/0.18),(720,1))
def pointcloudToPlanningScene(msg): global pointCloud global completed msg = pointCloud if not completed: try: trans = tf_buffer.lookup_transform("map", msg.header.frame_id, rospy.Time(0), rospy.Duration(4)) except tf2.LookupException as ex: rospy.logwarn(ex) return except tf2.ExtrapolationException as ex: rospy.logwarn(ex) return cloud_out = do_transform_cloud(msg, trans) rospy.sleep(2) scene.remove_world_object() rospy.sleep(2) data = pc2.read_points(cloud_out, field_names=("x", "y", "z", "rgb"), skip_nans=True) counter = 0 limitCounter = 0 limit = 5 for value in data: if limitCounter == limit: limitCounter = 0 p = PointStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = value[0] p.pose.position.y = value[1] p.pose.position.z = value[2] scene.add_box("point" + str(counter), p, (0.01, 0.01, 0.01)) counter = counter + 1 completed = True limitCounter = limitCounter + 1 print("completed scene")
def callback_plato(data): global sub_plato, pointcloud_plato, pointcloud_holo, done_plato, done_holo global threshold, number_of_initials, angle_from, angle_to global max_iterations, tolerance, tf_buffer, tf_listener, number_of_points_icp if done_holo: transform1 = None try: rospy.loginfo("Start getting data from velodyne") transform1 = tf_buffer.lookup_transform("map", data.header.frame_id, rospy.Time(0)) except tf2.LookupException as ex: rospy.logwarn(ex) return except tf2.ExtrapolationException as ex: rospy.logwarn(ex) return data = do_transform_cloud(data, transform1) done_holo = False x_points, y_points, z_points = [], [], [] for point in sensor_msgs.point_cloud2.read_points(data, skip_nans=True): x_points.append(point[0]) y_points.append(point[1]) z_points.append(point[2]) pointcloud_plato = np.column_stack((x_points, y_points, z_points)) sub_plato.unregister() rospy.loginfo("Data from plato received!") rospy.loginfo("Matching...") T = match_pointclouds(pointcloud_holo, pointcloud_plato, threshold, number_of_initials, angle_from, angle_to, max_iterations, tolerance, number_of_points_icp) broadcast_tf(T) rospy.loginfo('Matching done!')
def capture_pcl(self, trans): # Save pcl multiple times self.pcl_sub = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.pcl_callback) while self.pcl_count < self.pcl_count_thres: continue self.pcl_sub.unregister() # Convert point cloud to base frame pcl_global = do_transform_cloud(self.pcl[-1], trans) # Convert to open3d format self.pcl_converted = convertCloudFromRosToOpen3d( pcl_global, self.point_min_z) # Downsample self.pcl_converted = self.downsamplePCL(self.pcl_converted) # Estimate normals self.pcl_converted.estimate_normals( search_param=o3d.geometry.KDTreeSearchParamHybrid( radius=0.01, max_nn=30)) # using points in 1cm radius self.pcl_converted.orient_normals_towards_camera_location( camera_location=[0, 0, 0]) # Visualize o3d.visualization.draw_geometries([self.pcl_converted]) num_points = len(self.pcl_converted.points) print('Number of points: ', num_points) # Choose point contact_point_idx_list = [] for idx, point in enumerate(self.pcl_converted.points): if point[2] > 0.05: contact_point_idx_list += [idx] return self.pcl_converted, contact_point_idx_list
def merge(self, *args): """ merge received point clouds also preprocess: delete points that are outside of the boundary """ # take any number of input arguments merged_points = [] for points in args: # obtain the tf transform for this link trans = self.tf_buffer.lookup_transform('map', points.header.frame_id, points.header.stamp, rospy.Duration(1)) points = do_transform_cloud(points, trans) points_list = [] for p in pcl2.read_points(points, field_names=("x", "y", "z"), skip_nans=True): # check boundary info ### TODO: edit this boundary checking for customization if p[0] >= -0.5 and p[0] <= 0.5 and p[1] >= -0.5 and p[ 1] <= 0.5: # remove this point cloud, otherwise can't generate collision-free planning continue if p[2] >= 1.8: # remove camera point cloud continue points_list.append([p[0], p[1], p[2]]) merged_points += points_list header = std_msgs.msg.Header() header.stamp = rospy.Time.now() header.frame_id = 'map' #create pcl from points merged_pointcloud2 = pcl2.create_cloud_xyz32(header, merged_points) #publish rospy.loginfo("happily publishing sample pointcloud.. !") self.merge_pub.publish(merged_pointcloud2)
def velodyne_cb(self, pointcloud): # Transforming velodyne to map transform = self.tf_buffer.lookup_transform("map", pointcloud.header.frame_id, pointcloud.header.stamp, rospy.Duration(3.0)) pointcloud = do_transform_cloud(pointcloud, transform) #pointcloud_map cloud_points = list(read_points( pointcloud, skip_nans=True, field_names=("x", "y", "z"))) person_found = [] for p in cloud_points: px = p[0] - self.pedestrian_recog_area_c[0] py = p[1] - self.pedestrian_recog_area_c[1] if (abs(py) < self.pedestrian_recog_area_wide_y): # Taking points at the right of the pedestrian if (self.pedestrian_recog_area_wide_x[0] < px and px < self.pedestrian_recog_area_wide_x[1]): person_found.append([p[0], p[1], p[2]]) if len(person_found) > self.n_measurements_consider_valid: self.publish_person_found('CROSSING') self.publish_person_found_area(person_found) return self.publish_person_found('CLEAR') return
def callback(msg1, msg2): # print msg1.data try: fname = str(time.time()) print "a" transform_w3 = tf_buffer.lookup_transform( "workspace_link", "camera3_depth_optical_frame", rospy.Time()) #3B transform_w4 = tf_buffer.lookup_transform( "workspace_link", "camera4_depth_optical_frame", rospy.Time()) #4B transform_43 = tf_buffer.lookup_transform( "camera4_depth_optical_frame", "camera3_depth_optical_frame", rospy.Time()) # 4B transform_o4 = tf_buffer.lookup_transform( "camera4_depth_optical_frame", "camera4_link", rospy.Time()) # 4B transform_wo4 = tf_buffer.lookup_transform("workspace_link", "camera4_link", rospy.Time()) #4B print "trans_wo4", transform_wo4 p, q = transform_to_pq(transform_wo4) Two4 = convertRostfTotransform(p, q) print "init two4", Two4 # trans=tf_buffer.lookup_transform("hand_link","base_link",rospy.Time()) # # print trans pcd1_transformed = do_transform_cloud(msg1, transform_w3) # temp=convertCloudFromRosToOpen3d(msg1) q1, p1 = transform_to_pq(transform_w3) Tw3 = convertRostfTotransform(q1, p1) # temp2=copy.deepcopy(temp).transform(Tw3) # o3d.io.write_point_cloud("cam3Tw3.ply",temp2) # o3d.io.write_point_cloud("cam3raw.ply",temp) # temp=convertCloudFromRosToOpen3d(pcd1_transformed) # o3d.io.write_point_cloud("cam3trans1.ply",temp) #convert pcd from camera 3,4 to work_link pcd2_transformed = do_transform_cloud(msg2, transform_w4) pcd1_o3d = convertCloudFromRosToOpen3d( pcd1_transformed ) #conver pcd from Pointcloud2 to open3d Pointcloud pcd2_o3d = convertCloudFromRosToOpen3d(pcd2_transformed) center1 = vol.crop_point_cloud(pcd1_o3d) #crop center2 = vol.crop_point_cloud(pcd2_o3d) # #-------------------- # trans_init = np.asarray([ [1, 0.0, -0.0, 0.], # 変換無し [-0.0, 1, -0.0, 0.0], [0.0, 0.0, 1.0, 0], [0.0, 0.0, 0.0, 1.0] ]) "center 1 2 単位I" # draw_registration_result(center1, center2, trans_init) # # #-------------------- p1, q1 = transform_to_pq( transform_w3 ) #use homogeneous Matrix to conver back to camera coordination # print("trans2",p1,q1) Tw3 = convertRostfTotransform(p1, q1) # T3B T3w = np.linalg.inv(Tw3) p2, q2 = transform_to_pq(transform_w4) # T4b Tw4 = convertRostfTotransform(p2, q2) T4w = np.linalg.inv(Tw4) p12, q12 = transform_to_pq(transform_43) # T4b T43 = convertRostfTotransform(p12, q12) source = center1.transform( T3w ) # cam3 #convert pcd back to camera frame target = center2.transform(T4w) # cam4 o3d.io.write_point_cloud("source.ply", source) o3d.io.write_point_cloud("target.ply", target) # #-------------------- # # trans_init = np.asarray([[1, 0.0, -0.0, 0.], # 変換無し # [-0.0, 1, -0.0, 0.0], # [0.0, 0.0, 1.0, 0], # [0.0, 0.0, 0.0, 1.0]]) # # # #-------------------- trans_init = T43 #init Matrix evaluation = o3d.registration.evaluate_registration( source, target, #init result threshold, trans_init) "back to 3,4, with initT=T43 should match" draw_registration_result(source, target, trans_init) # print("Trans 34",transform_43.transform) # print("tf T43") # print("T3b inv Tw3") # print(Tw3) print("Tf published T43") print(T43) print("Init evaluation :", evaluation) print("Init workspace_link to camera4_depth_optical_frame is :") # print("tf tb4") # print("init calc tb4") # print(np.dot(Tw3,T43)) # print("init tf Tb4") print(T4w) print("Apply point-to-point ICP") reg_p2p = o3d.registration.registration_icp( #P2P ICP result source, target, threshold, trans_init, o3d.registration.TransformationEstimationPointToPoint()) print("ICP T43") print(reg_p2p) print("point-to-point ICP Transformation is:") print(reg_p2p.transformation) resultTw4 = np.dot(Tw3, np.linalg.inv(np.array(reg_p2p.transformation))) print("result Tw4", resultTw4) p, q = transform_to_pq(transform_o4) To4 = convertRostfTotransform(p, q) print "To4", To4 resultTwo4 = np.dot(resultTw4, To4) print "result Two4", resultTwo4 print(resultTwo4) "ICP caulced should match" draw_registration_result(source, target, reg_p2p.transformation) #----------------------------------- # try: # (trans1, rot1) = tf_listener.lookupTransform('camera3_link', 'workspace_link', rospy.Time(0)) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # print("err") # # # pcd1_base = convertCloudFromOpen3dToRos(center1, frame_id="workspace_link") # pcd2_base = convertCloudFromOpen3dToRos(center2, frame_id="workspace_link") # # transform1_ = tf_buffer.lookup_transform("camera3_depth_optical_frame","workspace_link", rospy.Time()) # transform2_ = tf_buffer.lookup_transform("camera4_depth_optical_frame","workspace_link", rospy.Time()) # # pcd1_returned = do_transform_cloud(pcd1_base, transform1_) # pcd2_returned = do_transform_cloud(pcd2_base, transform2_) # # pcd1_returned_o3d=convertCloudFromRosToOpen3d(pcd1_returned) # pcd2_returned_o3d=convertCloudFromRosToOpen3d(pcd2_returned) # # o3d.io.write_point_cloud("/home/finibo/ws1/libs/test/Open3D/examples/TestData/pcd1_returned_ros2o3d.ply", pcd1_returned_o3d) # o3d.io.write_point_cloud("/home/finibo/ws1/libs/test/Open3D/examples/TestData/pcd2_returned_ros2o3d.ply", pcd2_returned_o3d) # estimate_normals(cloud=center1, search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) # estimate_normals(cloud=center2, search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) # # print("Apply point-to-plane ICP") # reg_p2p = o3d.registration.registration_icp( # pcd1_returned_o3d, pcd2_returned_o3d, threshold, trans_init, # o3d.registration.TransformationEstimationPointToPoint()) # print(reg_p2p) # print("Transformation is:") # print(reg_p2p.transformation) camera = "camera4" Q = Quaternion(matrix=resultTwo4) print directory print "w" with open(directory + "/{}_transform.yaml".format(camera), 'w') as f: f.write("eye_on_hand: false\n") f.write("robot_base_frame: workspace_link\n") f.write("tracking_base_frame: {}_link\n".format(camera)) f.write("transformation: {") f.write("x: {}, ".format(resultTwo4[0, 3])) f.write("y: {}, ".format(resultTwo4[1, 3])) f.write("z: {}, ".format(resultTwo4[2, 3])) f.write("qx: {}, ".format(Q[1])) f.write("qy: {}, ".format(Q[2])) f.write("qz: {}, ".format(Q[3])) f.write("qw: {} }}\n".format(Q[0])) print "------------written in yaml------------" except: pass
def point_cloud_callback(self, msg, data): """ :param msg: points , type=msg.PointCloud2 :param data: mask image ,type=np.array :return: points, type= msg.PointCloud2 """ # tramform from lidar tp camera timea = time.time() #print("!!!!!!*******+") lookup_time = msg.header.stamp + rospy.Duration(10) target_frame = self.target_frame source_frame = msg.header.frame_id if self.count == 0: try: self.trans = self.tf_buffer.lookup_transform( target_frame, source_frame, rospy.Time(), rospy.Duration(10)) #lookup_time, print(self.trans.transform.translation) print(self.trans.transform.rotation) except tf2.LookupException as ex: rospy.logwarn(str(lookup_time.to_sec())) rospy.logwarn(ex) return except tf2.ExtrapolationException as ex: rospy.logwarn(str(lookup_time.to_sec())) rospy.logwarn(ex) return self.count += 1 timeb = time.time() cloud_out = do_transform_cloud(msg, self.trans) print("!!!!!!!!*****") print("time in lookup = ", timeb - timea) # tranform from camre to image #time0 = time.time() point = list( pc2.read_points(cloud_out, field_names=("x", "y", "z"), skip_nans=True)) #time1 = time.time() #print("time append",time1-time0) points = np.array(point) cloud_segment = self.segment_pointcloud(points, data) time2 = time.time() #print("coordinate translate",time2-time1) #print("generate filter points1111111111") cloud_seg = self.pointcloud_ge(cloud_segment) #print("coordinate translate",time.time()-time2) print("generate filter points") return cloud_seg
def callback(graph, cloud): global assembledCloud global posesDict global cloudsDict global pub begin = rospy.get_time() nodeId = graph.posesId[-1] pose = graph.poses[-1] size = cloud.width posesDict[nodeId] = pose cloudsDict[nodeId] = cloud # Update pose of our buffered clouds. # Check also if the clouds have moved because of a loop closure. If so, we have to update the rendering. maxDiff = 0 for i in range(0, len(graph.posesId)): if graph.posesId[i] in posesDict: currentPose = posesDict[graph.posesId[i]].position newPose = graph.poses[i].position diff = max([ abs(currentPose.x - newPose.x), abs(currentPose.y - newPose.y), abs(currentPose.z - newPose.z) ]) if maxDiff < diff: maxDiff = diff else: rospy.loginfo( "Old node %d not found in cache, creating an empty cloud.", graph.posesId[i]) posesDict[graph.posesId[i]] = graph.poses[i] cloudsDict[graph.posesId[i]] = PointCloud2() # If we don't move, some nodes would be removed from the graph, so remove them from our buffered clouds. newGraph = Set(graph.posesId) totalPoints = 0 for p in posesDict.keys(): if p not in newGraph: posesDict.pop(p) cloudsDict.pop(p) else: totalPoints = totalPoints + cloudsDict[p].width if maxDiff > 0.1: # if any node moved more than 10 cm, request an update of the assembled map so far newAssembledCloud = PointCloud2() rospy.loginfo( "Map has been optimized! maxDiff=%.3fm, re-updating the whole map...", maxDiff) for i in range(0, len(graph.posesId)): posesDict[graph.posesId[i]] = graph.poses[i] t = TransformStamped() p = posesDict[graph.posesId[i]] t.transform.translation = p.position t.transform.rotation = p.orientation transformedCloud = do_transform_cloud(cloudsDict[graph.posesId[i]], t) if i == 0: newAssembledCloud = transformedCloud else: newAssembledCloud.data = newAssembledCloud.data + transformedCloud.data newAssembledCloud.width = newAssembledCloud.width + transformedCloud.width newAssembledCloud.row_step = newAssembledCloud.row_step + transformedCloud.row_step assembledCloud = newAssembledCloud else: t = TransformStamped() t.transform.translation = pose.position t.transform.rotation = pose.orientation transformedCloud = do_transform_cloud(cloud, t) # just concatenate new cloud to current assembled map if assembledCloud.width == 0: assembledCloud = transformedCloud else: # Adding only the difference would be more efficient assembledCloud.data = assembledCloud.data + transformedCloud.data assembledCloud.width = assembledCloud.width + transformedCloud.width assembledCloud.row_step = assembledCloud.row_step + transformedCloud.row_step updateTime = rospy.get_time() - begin rospy.loginfo( "Received node %d (%d pts) at xyz=%.2f %.2f %.2f, q_xyzw=%.2f %.2f %.2f %.2f (Map: Nodes=%d Points=%d Assembled=%d Update=%.0fms)", nodeId, size, pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w, len(cloudsDict), totalPoints, assembledCloud.width, updateTime * 1000) assembledCloud.header = graph.header pub.publish(assembledCloud)
def cloud_callback(self, msg): #self.cloud_count += 1 #print "Cloud has ", len(msg.data), " points" //len(msg.data) does not give num points in cloud. It is encoded differently. if (self.cloud_count <= 1 ): #TEMPORARY JUST TO TESTS MULTIPLE ITERATIONS WITH ONE CLOUD dx = 0.0 dy = 0.0 s = 0. c = 1. try: # TRANSFORM THE LASER POINTCLOUD FROM THE LASER FRAME TO MAP FRAME... # LOOKUP THE TRANSFORM AT THE TIME THAT CORRESPONDNS WITH THE POINTCLOUD DATA (msg.header.stamp) # IF MUTLIPLE ITERATIONS ARE DONE FOR ONE POINTCLOUD MESSAGE, YOU WILL NEED TO USE A DIFFERENT TIME # IF YOU WANT TO TRY TO USE update_pose() EACH ITERATION # MAYBE YOU CAN JUST DEFINE self.transform properly using self.odom_x,y,theta # ANOTHER OPTION MAY BE TO DO THE ITERATIONS AFTER left_list IS FILLED AND JUST TRANSFORM left_list #if(self.transform == None): # not working with bag file. Requires extrapolation into future ( < 50 msec) # tf MessageFilter or waitForTransform? #self.transform = self.tf_buffer.lookup_transform("map","laser", msg.header.stamp) # Works with nav_sim, not bag, not jeep live self.transform = self.tf_buffer.lookup_transform( "map", "laser", rospy.Time()) # rospy.Time(0) requests latest #self.transform = self.tf_buffer.waitForTransform("map","laser", msg.header.stamp) pc_map = do_transform_cloud(msg, self.transform) #self.cloud_pub.publish(pc_map) #Temporary for validating the transformed point cloud print "\nNEW POINT CLOUD" r = 0 best_r = 0 left_list = [] right_list = [] ref_type_list = [] nc = 0 numPoints = 0 prev_x, prev_y = 0., 0. # THESE DUPLICATE POINTS MOSTLY COME FROM READINGS AT LOCAL LASER COORDINATE (0,0) delta_x_sum = 0. delta_y_sum = 0. delta_x_count = 0. delta_y_count = 0. line_segs = [] line_seg_pts = [] nSegPts = 0 #for point_map in pcl2.read_points(pc_map, skip_nans=True): # for points in both (map frame) and (in laser frame directly from scan msg) for k, (point_map, point_laser) in enumerate( zip(pcl2.read_points(pc_map, skip_nans=True), pcl2.read_points(msg, skip_nans=True))): corr_found = False range_sqd = point_laser[0]**2 + point_laser[ 1]**2 # only if you loop thru local laser points if (nSegPts == 0): segPtA = point_map segPt1 = point_map line_seg_pts.append(segPtA) ang_cur = 0. nSegPts = 1 elif (range_sqd > 0.25 and range_sqd < 400.0): segPtB = point_map ang = self.angle(segPt1, segPtB) sub_seg_dist_sqd = (segPtB[0] - segPt1[0])**2 + ( segPtB[1] - segPt1[1])**2 angA = self.angle(segPtA, segPtB) #or (nSegPts > 2 and abs(angA-ang_cur) < self.angle_eps_rad) # too easy for corner transition to blend with prev seg if (sub_seg_dist_sqd < self.short_gap**2): angle_eps = self.angle_eps_short_rad else: angle_eps = self.angle_eps_long_rad # temp debug #~ print "DEBUG LINE SEGMENTS" #~ print "pt1: ", segPt1 #~ print "ptB: ", segPtB #~ print "ang: ", ang, ", sub_dist: ", math.sqrt(sub_seg_dist_sqd) #~ print "ang diff: ", self.get_abs_angle_diff(ang,ang_cur) #~ print "END DEBUG LINE SEGMENTS" if ( (nSegPts == 1 or self.get_abs_angle_diff(ang, ang_cur) < angle_eps) and sub_seg_dist_sqd < 8.**2 ): # and segPtB[3]-segPt1[3] <= 2 # not available with bag data from scanse line_seg_pts.append(segPtB) nSegPts += 1 #~ print "SEG Pt PASSED" if (nSegPts == 2): ang_cur = ang # ang_cur = ang_cur*(1-alpha) + ang*alpha #only if nSegPts > 2 else: if (nSegPts >= self.min_nSegPts): line_segs.append( np.array([[segPtA[0], segPtA[1]], [segPt1[0], segPt1[1]]])) print "FOUND LINE SEGMENT, nSegPts: ", nSegPts, "angle: ", self.angle( segPtA, segPt1) print line_segs[-1] for pk in range(nSegPts): print line_seg_pts[pk] segPtA = segPt1 ang_cur = ang line_seg_pts = [] if (sub_seg_dist_sqd < 8.**2): nSegPts = 2 line_seg_pts.append(segPtA) line_seg_pts.append(segPtB) else: nSegPts = 1 segPtA = segPtB line_seg_pts.append(segPtA) segPt1 = segPtB if ((point_map[2] > self.min_scan_elev) and (range_sqd > 0.25 and range_sqd < 900.0)): numPoints += 1 pt_x = point_map[0] pt_y = point_map[1] if (numPoints > 1): dist_sqd = (pt_x - prev_x)**2 + (pt_y - prev_y)**2 if (dist_sqd < 0.1): #print "duplicate point ", pt_x, ", ", pt_y continue prev_x = pt_x prev_y = pt_y #pt_z = point_map[2] #print pt_x, pt_y dist = 100. r_count = 0 # Loop thru landmarks and find closest landmark point to current point ref_type = None while (r_count < self.nLandmarks): ref = self.landmarks[r] pot_dist, ref_pt = self.get_dist( ref, (pt_x, pt_y), self.max_correlation_dist) if (pot_dist < dist and not (ref_pt == None)): dist = pot_dist best_r = r if (not corr_found): corr_found = True left_list.append((pt_x, pt_y)) right_list.append(ref_pt) ref_type = ref.ref_type ref_type_list.append(ref_type) nc += 1 else: right_list[nc - 1] = ref_pt ref_type = ref.ref_type ref_type_list[nc - 1] = ref_type #if(dist < self.FOUND_DIST_THRESH): # BAD BECAUSE THE EAST VS. WEST SHED WALL IS CLOSE # break r = (r + 1) % (self.nLandmarks) r_count += 1 r = best_r if (not ref_type == None): if (not ref_type == 'horz'): delta_x_count += 1 delta_x_sum += right_list[nc - 1][0] - pt_x if (not ref_type == 'vert'): delta_y_count += 1 delta_y_sum += right_list[nc - 1][1] - pt_y print "numPoints: ", numPoints print "delta_x_count, delta_x_sum: ", delta_x_count, delta_x_sum print "delta_y_count, delta_y_sum: ", delta_y_count, delta_y_sum print "lx, ly, rx, ry" #~ for pk, ref_type in enumerate(ref_type_list): #~ if(ref_type == 'horz' and delta_x_count > 0): #~ right_list[pk] = (right_list[pk][0]+delta_x_sum/delta_x_count, right_list[pk][1]) #~ if(ref_type == 'vert' and delta_y_count > 0): #~ right_list[pk] = (right_list[pk][0], right_list[pk][1]+delta_y_sum/delta_y_count) #for l,r in zip(left_list,right_list): # print l[0], ',', l[1], ',', r[0], ',', r[1] if (len(left_list) >= self.min_point_pairs): trafo = estimate_transform(left_list, right_list, fix_scale=True) cur_dist_sum = 0. new_dist_sum = 0. if (not trafo == None): cur_dist_sum, new_dist_sum = self.trafo_eval( trafo, left_list, right_list) if (self.first_scan): self.first_scan = False self.prev_eval_dist_sum = cur_dist_sum if ((not trafo == None) and (new_dist_sum < cur_dist_sum + 1.0) and (abs(cur_dist_sum - self.prev_eval_dist_sum) < 30.)): self.prev_eval_dist_sum = cur_dist_sum print "enough points, better trafo" if (abs(trafo[2]) < self.MAX_DELTA_THETA_RAD and abs(trafo[3]) < self.MAX_DELTA_X and abs(trafo[4]) < self.MAX_DELTA_Y): #print "lx, ly, rx, ry" #for l,r in zip(left_list,right_list): # print l[0], ',', l[1], ',', r[0], ',', r[1] print "trafo:" print trafo la, c, s, dx, dy = trafo #except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): except Exception as e: print "tf issue" print repr(e) pass print c, s, dx, dy #Complementary, low pass, filter: filt = filt*(1-alpha) + raw*(alpha) alpha = self.alpha x = self.odom_x y = self.odom_y raw_x = c * x - s * y + dx self.odom_x = x * (1. - alpha) + raw_x * (alpha) raw_y = s * x + c * y + dy self.odom_y = y * (1. - alpha) + raw_y * (alpha) self.odom_theta = self.odom_theta * (1. - alpha) + math.atan2( s, c) * (alpha) self.update_pose()
def mask_msg_callback(self, req): array = rosnp.point_cloud2.pointcloud2_to_xyz_array( self.cloud_msg, False) label_store, score_store, box_store, mask_store = [], [], [], [] #custom msg creation with 3D coordinates of each pixel in each masks #message=Mask_Depth() #message.header.stamp=rospy.Time.now() rospy.loginfo("Parsing detections") for item in req.detections: #Init variables points_list = [] #Get mask array and starting coordinate of the bounding box obj_mask = item.mask.mask x_offset = item.box.x1 y_offset = item.box.y1 #create (X,Y) coordinates of a rectagle with the same size of the bounding box img = np.full((item.mask.height, item.mask.width, 3), (0, 0, 255), dtype=np.uint8) gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) xy_coords = np.flip(np.column_stack(np.where(gray >= 0)), axis=1) #adding an offset to fit the coordinates of the actual bounding box of the mask for pixels in xy_coords: pixels[0] += x_offset pixels[1] += y_offset #checking only coordinates with mask inside the bounding box coord_mask = list(compress(xy_coords, obj_mask)) rospy.loginfo("AAA") """ create custom message with 3D points of mask mask_message = Mask_Coordinates() mask_message.class_name = label_store[i] mask_message.score = score_store[i] """ # Parsing each pixel of the mask, add the Z coordinate from Pointcloud and # transform the mask into the map frame for j in range(0, len(coord_mask)): if math.isnan(array[coord_mask[j][1]][coord_mask[j][0]][0]) or \ math.isnan(array[coord_mask[j][1]][coord_mask[j][0]][1]) or \ math.isnan(array[coord_mask[j][1]][coord_mask[j][0]][2]): pass else: #points = PointStamped() #points.header.frame_id = "kinect_depth_optical_frame" #points.header.frame_id = "kinect_depth_link" #points.header.stamp=rospy.Time(0) #points.point.x = array[coord_mask[j][1]][coord_mask[j][0]][0] #points.point.y = array[coord_mask[j][1]][coord_mask[j][0]][1] #points.point.z = array[coord_mask[j][1]][coord_mask[j][0]][2] #p = self.listener.transformPoint("map",points) points_list.append([ array[coord_mask[j][1]][coord_mask[j][0]][0], array[coord_mask[j][1]][coord_mask[j][0]][1], array[coord_mask[j][1]][coord_mask[j][0]][2] ]) #Create a PCL in the camera link h = std_msgs.msg.Header() h.stamp = rospy.Time.now() h.frame_id = 'kinect_depth_link' #change to ros.getparam scaled_polygon_pcl = pcl2.create_cloud_xyz32(h, points_list) #Transform the PCL into the base_link frame #tf_buffer = tf2_ros.Buffer() #tf_listener = tf2_ros.TransformListener(tf_buffer) #self.listener.waitForTransform("base_footprint","kinect_depth_link", rospy.Time(0), rospy.Duration(4.0)) #transform = tf_buffer.lookup_transform("base_footprint","kinect_depth_link", rospy.Time(0)) #cloud_out = do_transform_cloud(scaled_polygon_pcl, transform) #Transform the PCL into the map frame tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) #tf_listener.waitForTransform("map","kinect_depth_link", rospy.Time(0), rospy.Duration(4.0)) transform = tf_buffer.lookup_transform("map", "kinect_depth_link", rospy.Time(0), rospy.Duration(1.0)) cloud_out = do_transform_cloud(scaled_polygon_pcl, transform) if self.valid: self.pointcloud_publisher.publish(cloud_out) valid = False
try: trans1 = tf_buffer.lookup_transform('world', cam1_msg.header.frame_id, rospy.Time(0), rospy.Duration(10)) trans2 = tf_buffer.lookup_transform('world', cam2_msg.header.frame_id, rospy.Time(0), rospy.Duration(10)) trans3 = tf_buffer.lookup_transform('world', cam3_msg.header.frame_id, rospy.Time(0), rospy.Duration(10)) except tf2.LookupException as ex: rospy.logwarn(ex) pass except tf2.ExtrapolationException as ex: rospy.logwarn(ex) pass cloud_out1 = do_transform_cloud(cam1_msg, trans1) cloud_out2 = do_transform_cloud(cam2_msg, trans2) cloud_out3 = do_transform_cloud(cam3_msg, trans3) cloud_np1 = ros_numpy.numpify(cloud_out1) cloud_np2 = ros_numpy.numpify(cloud_out2) cloud_np3 = ros_numpy.numpify(cloud_out3) print('Clouds Transformed to fixed frame.') points1 = np.zeros((cloud_np1.shape[0], 3)) points2 = np.zeros((cloud_np2.shape[0], 3)) points3 = np.zeros((cloud_np3.shape[0], 3)) #colors=100*np.ones((pc.shape[0],3)) points1[:, 0] = cloud_np1['x'] points1[:, 1] = cloud_np1['y']
def cloud_callback(self, msg): #self.cloud_count += 1 #print "Cloud has ", len(msg.data), " points" //len(msg.data) does not give num points in cloud. It is encoded differently. if (self.cloud_count <= 1 ): #TEMPORARY JUST TO TESTS MULTIPLE ITERATIONS WITH ONE CLOUD for k in range( 1): #PROBABLY REMOVE THE FOR LOOP, ITERATIONS FOR ICP print "iter ", k dx = 0.0 dy = 0.0 s = 0. c = 1. try: # TRANSFORM THE LASER POINTCLOUD FROM THE LASER FRAME TO MAP FRAME... # LOOKUP THE TRANSFORM AT THE TIME THAT CORRESPONDNS WITH THE POINTCLOUD DATA (msg.header.stamp) # IF MUTLIPLE ITERATIONS ARE DONE FOR ONE POINTCLOUD MESSAGE, YOU WILL NEED TO USE A DIFFERENT TIME # IF YOU WANT TO TRY TO USE update_pose() EACH ITERATION # MAYBE YOU CAN JUST DEFINE self.transform properly using self.odom_x,y,theta # ANOTHER OPTION MAY BE TO DO THE ITERATIONS AFTER left_list IS FILLED AND JUST TRANSFORM left_list #if(self.transform == None): # not working with bag file. Requires extrapolation into future ( < 50 msec) # tf MessageFilter or waitForTransform? #self.transform = self.tf_buffer.lookup_transform("map","laser", msg.header.stamp) # Works with nav_sim, not bag, not jeep live self.transform = self.tf_buffer.lookup_transform( "map", "laser", rospy.Time()) # rospy.Time(0) requests latest #self.transform = self.tf_buffer.waitForTransform("map","laser", msg.header.stamp) pc_map = do_transform_cloud(msg, self.transform) #self.cloud_pub.publish(pc_map) #Temporary for validating the transformed point cloud print "NEW POINT CLOUD" r = 0 best_r = 0 left_list = [] right_list = [] ref_type_list = [] nc = 0 numPoints = 0 prev_x, prev_y = 0., 0. # THESE DUPLICATE POINTS MOSTLY COME FROM READINGS AT LOCAL LASER COORDINATE (0,0) delta_x_sum = 0. delta_y_sum = 0. delta_x_count = 0. delta_y_count = 0. #for point_map in pcl2.read_points(pc_map, skip_nans=True): # for points in both (map frame) and (in laser frame directly from scan msg) for point_map, point_laser in zip( pcl2.read_points(pc_map, skip_nans=True), pcl2.read_points(msg, skip_nans=True)): #numPoints += 1 corr_found = False range_sqd = point_laser[0]**2 + point_laser[ 1]**2 # only if you loop thru local laser points if ((point_map[2] > self.min_scan_elev) and (range_sqd > 0.25 and range_sqd < 900.0)): numPoints += 1 pt_x = point_map[0] pt_y = point_map[1] if (numPoints > 1): dist_sqd = (pt_x - prev_x)**2 + (pt_y - prev_y)**2 if (dist_sqd < 0.3): print "duplicate point ", pt_x, ", ", pt_y continue prev_x = pt_x prev_y = pt_y #pt_z = point_map[2] #print pt_x, pt_y dist = 100. r_count = 0 # Loop thru landmarks and find closest landmark point to current point ref_type = None while (r_count < self.nLandmarks): ref = self.landmarks[r] pot_dist, ref_pt = self.get_dist( ref, (pt_x, pt_y), self.max_correlation_dist) if (pot_dist < dist and not (ref_pt == None)): dist = pot_dist best_r = r if (not corr_found): corr_found = True left_list.append((pt_x, pt_y)) right_list.append(ref_pt) ref_type = ref.ref_type ref_type_list.append(ref_type) nc += 1 else: right_list[nc - 1] = ref_pt ref_type = ref.ref_type ref_type_list[nc - 1] = ref_type #if(dist < self.FOUND_DIST_THRESH): # BAD BECAUSE THE EAST VS. WEST SHED WALL IS CLOSE # break r = (r + 1) % (self.nLandmarks) r_count += 1 r = best_r if (not ref_type == None): if (not ref_type == 'horz'): delta_x_count += 1 delta_x_sum += right_list[nc - 1][0] - pt_x if (not ref_type == 'vert'): delta_y_count += 1 delta_y_sum += right_list[nc - 1][1] - pt_y print "numPoints: ", numPoints print "delta_x_count, delta_x_sum: ", delta_x_count, delta_x_sum print "delta_y_count, delta_y_sum: ", delta_y_count, delta_y_sum print "lx, ly, rx, ry" #~ for pk, ref_type in enumerate(ref_type_list): #~ if(ref_type == 'horz' and delta_x_count > 0): #~ right_list[pk] = (right_list[pk][0]+delta_x_sum/delta_x_count, right_list[pk][1]) #~ if(ref_type == 'vert' and delta_y_count > 0): #~ right_list[pk] = (right_list[pk][0], right_list[pk][1]+delta_y_sum/delta_y_count) for l, r in zip(left_list, right_list): print l[0], ',', l[1], ',', r[0], ',', r[1] if (len(left_list) >= self.min_point_pairs): trafo = estimate_transform(left_list, right_list, fix_scale=True) cur_dist_sum = 0. new_dist_sum = 0. if (not trafo == None): cur_dist_sum, new_dist_sum = self.trafo_eval( trafo, left_list, right_list) if (self.first_scan): self.first_scan = False self.prev_eval_dist_sum = cur_dist_sum if ((not trafo == None) and (new_dist_sum < cur_dist_sum + 1.0) and (abs(cur_dist_sum - self.prev_eval_dist_sum) < 30.)): self.prev_eval_dist_sum = cur_dist_sum print "enough points, better trafo" if (abs(trafo[2]) < self.MAX_DELTA_THETA_RAD and abs(trafo[3]) < self.MAX_DELTA_X and abs(trafo[4]) < self.MAX_DELTA_Y): #print "lx, ly, rx, ry" #for l,r in zip(left_list,right_list): # print l[0], ',', l[1], ',', r[0], ',', r[1] print "trafo:" print trafo la, c, s, dx, dy = trafo else: print "exit for loop, iter ", k break #except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): except Exception as e: print "tf issue" print repr(e) pass print c, s, dx, dy #Complementary, low pass, filter: filt = filt*(1-alpha) + raw*(alpha) alpha = self.alpha x = self.odom_x y = self.odom_y raw_x = c * x - s * y + dx self.odom_x = x * (1. - alpha) + raw_x * (alpha) raw_y = s * x + c * y + dy self.odom_y = y * (1. - alpha) + raw_y * (alpha) self.odom_theta = self.odom_theta * (1. - alpha) + math.atan2( s, c) * (alpha)
def cloud_callback(self, msg): #self.cloud_count += 1 #print "Cloud has ", len(msg.data), " points" //len(msg.data) does not give num points in cloud. It is encoded differently. if (self.cloud_count <= 1 ): #TEMPORARY JUST TO TESTS MULTIPLE ITERATIONS WITH ONE CLOUD dx = 0.0 dy = 0.0 s = 0. c = 1. try: # TRANSFORM THE LASER POINTCLOUD FROM THE LASER FRAME TO MAP FRAME... # LOOKUP THE TRANSFORM AT THE TIME THAT CORRESPONDNS WITH THE POINTCLOUD DATA (msg.header.stamp) # IF MUTLIPLE ITERATIONS ARE DONE FOR ONE POINTCLOUD MESSAGE, YOU WILL NEED TO USE A DIFFERENT TIME # IF YOU WANT TO TRY TO USE update_pose() EACH ITERATION # MAYBE YOU CAN JUST DEFINE self.transform properly using self.odom_x,y,theta # ANOTHER OPTION MAY BE TO DO THE ITERATIONS AFTER left_list IS FILLED AND JUST TRANSFORM left_list #if(self.transform == None): # not working with bag file. Requires extrapolation into future ( < 50 msec) # tf MessageFilter or waitForTransform? #self.transform = self.tf_buffer.lookup_transform("map","laser", msg.header.stamp) # Works with nav_sim, not bag, not jeep live self.transform = self.tf_buffer.lookup_transform( "map", "laser", rospy.Time()) # rospy.Time(0) requests latest #self.transform = self.tf_buffer.waitForTransform("map","laser", msg.header.stamp) pc_map = do_transform_cloud(msg, self.transform) #self.cloud_pub.publish(pc_map) #Temporary for validating the transformed point cloud print "NEW POINT CLOUD" rk = 0 best_rk = 0 left_list = [] right_list = [] ref_type_list = [] nc = 0 numPoints = 0 prev_x, prev_y = 0., 0. # THESE DUPLICATE POINTS MOSTLY COME FROM READINGS AT LOCAL LASER COORDINATE (0,0) delta_x_sum = 0. delta_y_sum = 0. delta_x_count = 0. delta_y_count = 0. #for point_map in pcl2.read_points(pc_map, skip_nans=True): # for points in both (map frame) and (in laser frame directly from scan msg) for point_map, point_laser in zip( pcl2.read_points(pc_map, skip_nans=True), pcl2.read_points(msg, skip_nans=True)): range_sqd = point_laser[0]**2 + point_laser[ 1]**2 # only if you loop thru local laser points if ((point_map[2] > self.min_scan_elev) and (range_sqd > 0.25 and range_sqd < 900.0)): numPoints += 1 pt_x = point_map[0] pt_y = point_map[1] if (numPoints > 1): dist_sqd = (pt_x - prev_x)**2 + (pt_y - prev_y)**2 if (dist_sqd < 0.3): print "duplicate point ", pt_x, ", ", pt_y continue prev_x = pt_x prev_y = pt_y #pt_z = point_map[2] #print pt_x, pt_y rk, ref_pt, ref = self.find_match(rk, pt_x, pt_y) if (not ref == None): left_list.append((pt_x, pt_y)) right_list.append(ref_pt) ref_type_list.append(ref.ref_type) print "numPoints: ", numPoints print "lx, ly, rx, ry" for l, r in zip(left_list, right_list): print l[0], ',', l[1], ',', r[0], ',', r[1] if (len(left_list) >= self.min_point_pairs): cum_trafo = (1., 1., 0., 0., 0.) trafo = estimate_transform(left_list, right_list, fix_scale=True) if (not trafo == None): cum_trafo = concatenate_transform(trafo, cum_trafo) for j in xrange(5): rk = 0 new_left_list = [ apply_transform(cum_trafo, p) for p in left_list ] left_list = [] right_list = [] ref_type_list = [] # currently not used for pk, left in enumerate(new_left_list): rk, ref_pt, ref = self.find_match( rk, left[0], left[1]) if (not ref == None): left_list.append((left[0], left[1])) right_list.append(ref_pt) ref_type_list.append(ref.ref_type) trafo = estimate_transform(left_list, right_list, fix_scale=True) if (not trafo == None): cum_trafo = concatenate_transform(trafo, cum_trafo) trafo = cum_trafo print "AFTER ITERS: lx, ly, rx, ry" for l, r in zip(left_list, right_list): print l[0], ',', l[1], ',', r[0], ',', r[1] cur_dist_sum, new_dist_sum = self.trafo_eval( trafo, left_list, right_list) if (self.first_scan): self.first_scan = False self.prev_eval_dist_sum = cur_dist_sum if ( (new_dist_sum < cur_dist_sum) ): #and (abs(cur_dist_sum - self.prev_eval_dist_sum) < 30.) ): self.prev_eval_dist_sum = cur_dist_sum print "enough points, better trafo" if (abs(trafo[2]) < self.MAX_DELTA_THETA_RAD and abs(trafo[3]) < self.MAX_DELTA_X and abs(trafo[4]) < self.MAX_DELTA_Y): print "trafo:" print trafo la, c, s, dx, dy = trafo #except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): except Exception as e: print "tf issue" print repr(e) pass print c, s, dx, dy #Complementary, low pass, filter: filt = filt*(1-alpha) + raw*(alpha) alpha = self.alpha x = self.odom_x y = self.odom_y raw_x = c * x - s * y + dx self.odom_x = x * (1. - alpha) + raw_x * (alpha) raw_y = s * x + c * y + dy self.odom_y = y * (1. - alpha) + raw_y * (alpha) self.odom_theta = self.odom_theta * (1. - alpha) + math.atan2( s, c) * (alpha)
def callback(data): assert isinstance(data, PointCloud2) # gen = pc2.read_points(data,field_names=("x","y","z","rgb"),skip_nans=True) # print gen.Header # time.sleep(1) # print msg1.data # print "3" rate = rospy.Rate(10.0) # try: fname = str(time.time()) # transform1 = tf_buffer.lookup_transform("workspace_link", "camera3_depth_optical_frame", rospy.Time()) #3B # print transform1 # transform = tf_buffer.lookup_transform("hand_link", "camera3_depth_optical_frame", rospy.Time()) #3B transform_h3 = tf_buffer.lookup_transform("hand_link", "camera3_depth_optical_frame", rospy.Time()) # 3B transform_wh = tf_buffer.lookup_transform("workspace_link", "hand_link", rospy.Time()) # 3B p, q = transform_to_pq(transform_h3) Th3 = convertRostfTotransform(p, q) print "trans h 3 call", transform_h3 print "Th3", Th3 pcd_transformed_h3 = do_transform_cloud( data, transform_h3) #cam3 to hand crop this temppcd = convertCloudFromRosToOpen3d(pcd_transformed_h3) # o3d.io.write_point_cloud("rostrans.ply",temppcd) temppcd = convertCloudFromRosToOpen3d(data) temppcd.transform(Th3) # o3d.io.write_point_cloud("o3dtrans.ply",temppcd) p, q = transform_to_pq(transform_h3) T = convertRostfTotransform(p, q) print T data_o3d = convertCloudFromRosToOpen3d(data) # o3d.io.write_point_cloud("cam3raw.ply",data_o3d) # can delete pcd_o3d_3h = convertCloudFromRosToOpen3d(pcd_transformed_h3) # o3d.io.write_point_cloud("3h_test.ply",pcd_o3d_3h) #delete cropped = vol.crop_point_cloud(pcd_o3d_3h) T3h = np.linalg.inv(Th3) cropped_h3 = copy.deepcopy(cropped).transform(T3h) # o3d.io.write_point_cloud("crop_back.ply",cropped_h3) # o3d.draw_geometries([cropped_h3]) pwh, qwh = transform_to_pq(transform_wh) Twh = convertRostfTotransform(pwh, qwh) threshold = 0.02 trans_init = T3h source = pcd_stl target = cropped_h3 # estimate_normals(cloud=source,search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) # estimate_normals(cloud=target,search_param=o3d.geometry.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)) draw_registration_result(source, target, trans_init) print("Initial alignment") print trans_init evaluation = o3d.registration.evaluate_registration( source, target, threshold, trans_init) print("Init evaluation :", evaluation) print("Apply point-to-point ICP") reg_p2p = o3d.registration.registration_icp( source, target, threshold, trans_init, o3d.registration.TransformationEstimationPointToPoint()) print(reg_p2p) print("Transformation is:") print(reg_p2p.transformation) T3ph = reg_p2p.transformation Tw3p = np.dot(Twh, np.linalg.inv(T3ph)) transform = tf_buffer.lookup_transform("camera3_depth_optical_frame", "camera3_link", rospy.Time()) # 3B p, q = transform_to_pq(transform) To3l = convertRostfTotransform(p, q) Tw3l = np.dot(Tw3p, To3l) print("") draw_registration_result(source, target, reg_p2p.transformation) resT = Tw3l camera = "camera3" Q = Quaternion(matrix=resT) print dir with open(dir + "/{}_transform.yaml".format(camera), 'w') as f: f.write("eye_on_hand: false\n") f.write("robot_base_frame: workspace_link\n") f.write("tracking_base_frame: {}_link\n".format(camera)) f.write("transformation: {") f.write("x: {}, ".format(resT[0, 3])) f.write("y: {}, ".format(resT[1, 3])) f.write("z: {}, ".format(resT[2, 3])) f.write("qx: {}, ".format(Q[1])) f.write("qy: {}, ".format(Q[2])) f.write("qz: {}, ".format(Q[3])) f.write("qw: {} }}\n".format(Q[0])) print "------------written in yaml------------" # print Tw3 print Tw3l # # print("Apply point-to-plane ICP") # reg_p2l = o3d.registration.registration_icp( # source, target, threshold, trans_init, # o3d.registration.TransformationEstimationPointToPlane()) # print(reg_p2l) # print("Transformation is:") # print(reg_p2l.transformation) # dict=t2q(reg_p2l.transformation) print("") # draw_registration_result(source, target, reg_p2l.transformation) # o3d.write_point_cloud("cam3_raw.ply",pcd_o3d_cam3) # o3d.write_point_cloud("cam3_3h_raw.ply",pcd_o3d_cam3_3h) print "written"
def callback_scan(self, data): try: transform = self.tfBuffer.lookup_transform("odom", "base_link", rospy.Time()) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): return trans = transform.transform.translation rot = transform.transform.rotation eular = tf.transformations.euler_from_quaternion( (rot.x, rot.y, rot.z, rot.w), "szxy") quat = tf.transformations.quaternion_from_euler(eular[0], 0, 0, "szxy") transform.transform.translation.z = 0 transform.transform.rotation = Quaternion(quat[0], quat[1], quat[2], quat[3]) self.tf_robot_position.sendTransform( (trans.x, trans.y, 0), (quat[0], quat[1], quat[2], quat[3]), rospy.Time.now(), 'robot_position', 'odom') scan = LaserScan() scan = data projector = LaserProjection() cloud = projector.projectLaser(scan) fixed_frame_cloud = do_transform_cloud(cloud, transform) points = pc2.read_points(fixed_frame_cloud, field_names=['x', 'y', 'z'], skip_nans=True) pole = [[0, 0, 0], [0, 0, 0], [0, 0, 0]] data = [] for x, y, z in points: if x > 5 and x < 7 and y > -2 and y < 2: data.append([x, y, z, 0xff0000]) pole[0][0] += x pole[0][1] += y pole[0][2] += 1 elif x > 2 and x < 4 and y > -2 and y < 0: data.append([x, y, z, 0x00ff00]) pole[1][0] += x pole[1][1] += y pole[1][2] += 1 elif x > 2 and x < 4 and y > 0 and y < 2: data.append([x, y, z, 0x0000ff]) pole[2][0] += x pole[2][1] += y pole[2][2] += 1 else: data.append([x, y, z, 0xffff00]) for p in pole: if p[2] != 0: p[0] /= p[2] p[1] /= p[2] if pole[0][2] < 5 or (pole[1][2] < 5 and pole[2][2] < 5): return trans_diff = [6.0 - pole[0][0], 0.0 - pole[0][1]] if pole[1][2] != 0: rot_diff = math.atan2(-1, -3) - math.atan2(pole[1][1] - pole[0][1], pole[1][0] - pole[0][0]) elif pole[2][2] != 0: rot_diff = math.atan2(1, -3) - math.atan2(pole[2][1] - pole[0][1], pole[2][0] - pole[0][0]) # data_pole = [] for x, y, z, color in data: tx = x + trans_diff[0] - 6.0 ty = y + trans_diff[1] rx = tx * math.cos(rot_diff) - ty * math.sin(rot_diff) + 6.0 ry = tx * math.sin(rot_diff) + ty * math.cos(rot_diff) if random.random() < 0.01: self.data_pole.append([rx, ry, z, color]) # print data_pole HEADER = Header(frame_id='/odom') FIELDS = [ PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1), PointField(name='rgb', offset=12, datatype=PointField.UINT32, count=1), ] filterd_cloud = pc2.create_cloud(HEADER, FIELDS, self.data_pole) self.pc_pub.publish(filterd_cloud)