def pc2CB(self, msg): self.start_time = rospy.Time.now().to_sec() print "----------------------------------" outer_pts = inner_pts = remaining_pts = [] # Convert ROS PointCloud2 msg to PCL data cloud = pcl_helper.ros_to_pcl(msg) # Passthrough Filter fil_cloud = filtering_helper.do_passthrough_filter( cloud, 'x', self.filter_xmin, self.filter_xmax) fil_cloud = filtering_helper.do_passthrough_filter( fil_cloud, 'y', self.filter_ymin, self.filter_ymax) # Statistical Outlier Filter fil = fil_cloud.make_statistical_outlier_filter() fil.set_mean_k(10) fil.set_std_dev_mul_thresh(0.02) filtered_cloud = fil.filter() # self.load_params_from_yaml(self.filepath) # Setting Outer-Region and Inner-Region self.outer_region, self.inner_region = self.getDetectionRegion( self.fsm_state, self.lifter_status) # Visualize Detection Region self.visualize_region(self.outer_region, self.inner_region) self.middle_time_1 = rospy.Time.now().to_sec() # Grouping raw cloud into 2 groups of points outer_pts, inner_pts = self.getPointsGroupedFromCloud( filtered_cloud, self.outer_region, self.inner_region) self.middle_time_2 = rospy.Time.now().to_sec() remaining_pts = list(set(outer_pts) - set(inner_pts)) print "outer_pts", len(outer_pts) print "inner_pts", len(inner_pts) print "remaining_pts", len(remaining_pts) if (len(remaining_pts) != 0): print "Obstacle detected and stopping AGV" self.stopping() print "start_time", self.start_time print "middle_time_1", self.middle_time_1 print "middle_time_2", self.middle_time_2 print "end_time", rospy.Time.now().to_sec()
def pc2CB(self, msg): # Convert ROS PointCloud2 msg to PCL data cloud = pcl_helper.ros_to_pcl(msg) # Filter the Cloud according limitation on x-axis and y-axis fil_cloud = filtering_helper.do_passthrough_filter( cloud, 'x', -0.5, 0.0) fil_cloud = filtering_helper.do_passthrough_filter( fil_cloud, 'y', -0.5, 0.5) print fil_cloud.size # Visualize the filtered cloud outmsg = pcl_helper.pcl_to_ros(fil_cloud, "base_link") outmsg.header = msg.header self.pc2_pub.publish(outmsg) # Save the cloud if self.init: pcl.save(fil_cloud, "/home/samuel/charger_unit.pcd", None, False) self.init = False print "record finish"
def pc2CB(self, msg): self.start_time = rospy.Time.now().to_sec() print "--------------------------------------------" # Convert ROS PointCloud2 msg to PCL data cloud = pcl_helper.ros_to_pcl(msg) # self.load_params_from_yaml(self.filepath) print "fsm_state", self.fsm_state # Setting Detection-Region for stopping detection_region = self.getDetectionRegion(self.fsm_state, self.lifter_status, self.speed) # Finding Min & Max of detection region x_max, x_min, y_max, y_min = self.getMinMaxXYFromRegion( detection_region) # Passthrough Filter fil_cloud = filtering_helper.do_passthrough_filter( cloud, 'x', x_min, x_max) fil_cloud = filtering_helper.do_passthrough_filter( fil_cloud, 'y', y_min, y_max) # Voxel Grid Filter filtered_cloud = filtering_helper.do_voxel_grid_filter(fil_cloud, 0.01) self.middle_time_1 = rospy.Time.now().to_sec() # Use Euclidean clustering to filter out noise clusters = self.getClusters(filtered_cloud, 0.05, 10, 9000) #0.05, 10 # Setting Front-region for snapshot front_region = self.camera_region # Visualize Detection Region self.visualize_region(detection_region) self.middle_time_2 = rospy.Time.now().to_sec() # Grouping clusters into 2 groups of points detected_pts = self.getPointsInRegionFromClusters( clusters, filtered_cloud, detection_region) # front_pts = self.getPointsInRegionFromPoints(detected_pts, front_region) self.middle_time_3 = rospy.Time.now().to_sec() print "detected_pts", len(detected_pts) # If Obstacle detected, do following: if (len(detected_pts) != 0): self.stopping() print "--------------------------------------" print "| Obstacle detected and stopping AGV |" print "--------------------------------------" # Rosservice call the health_monitoring of obstacle blocking if (rospy.Time.now().to_sec() - self.obs_timer >= self.obs_timeout): print "#######################################################" print "########## calling health_monitoring ##########" print "#######################################################" self.obstacle_health_call() self.obs_timer = rospy.Time.now().to_sec() # Use for snapshot once per obstacle at front if (self.fsm_state != "SUSPEND"): self.error = 1 # Rosservice call to snapshot the front obstacle once # print "front_pts", len(front_pts) # if(len(front_pts)!=0 and self.fsm_state=="SUSPEND" and self.error==1): # if(len(detected_pts)!=0 and self.fsm_state=="SUSPEND" and self.error==1): if (len(detected_pts) != 0 and self.fsm_state == "SUSPEND" and self.fsm_state != "ESTOP" and self.error == 1): self.error = 0 print "######################################" print "########## snapshot ##########" print "######################################" self.snapshot_call() self.prev_detection_region = detection_region print "start_time", self.start_time print "middle_time_1", self.middle_time_1 print "middle_time_2", self.middle_time_2 print "middle_time_3", self.middle_time_3 print "end_time", rospy.Time.now().to_sec()
def pc2CB(self, msg): if self.init: # Convert ROS PointCloud2 msg to PCL data cloud = pcl_helper.ros_to_pcl(msg) # Filter the Cloud fil_cloud = filtering_helper.do_passthrough_filter( cloud, 'x', self.x_min, self.x_max) filtered_cloud = filtering_helper.do_passthrough_filter( fil_cloud, 'y', self.y_min, self.y_max) # Turn cloud into colorless cloud colorless_cloud = pcl_helper.XYZRGB_to_XYZ(filtered_cloud) # Get groups of cluster of points # clusters = self.getClusters(colorless_cloud, 0.05, 20, 300) # Table on Top AGV - 60~300 clusters = self.getClusters(colorless_cloud, 0.05, 20, 350) print "len(clusters)", len(clusters) for i in range(0, len(clusters)): print "len(clusters[", i, "]", len(clusters[i]) # Get mean points from clusters mean_pts = [] for cluster in clusters: mean_pts.append(self.getMeanPoint(cluster, filtered_cloud)) print "len(mean_pts)", len(mean_pts) for i in range(0, len(mean_pts)): print "mean_pts[", i, "]", mean_pts[i] # Remove other points, leave the table points only table_pts = [] table_pts = self.getTablePoints(mean_pts) print "len(table_pts)", len(table_pts) print table_pts for i in range(len(table_pts)): self.vizPoint(i, table_pts[i], ColorRGBA(1, 1, 1, 1), "base_link") # Get transform of "base_link" relative to "map" trans = self.getTransform("map", "base_link") # Finding 2 middle points from 4 table legs and use them as path_pts path_pts = [] if (len(table_pts) == 4): for p1 in table_pts: for p2 in table_pts: if (p1 == p2): break # Register 2 middle_pts of 4 table legs if (0.73 < self.getDistance(p1, p2) < 0.83): path_pts.append(self.getMiddlePoint(p1, p2)) break print "len(path_pts)", len(path_pts) print path_pts # Turn path_pts into map frame for i in range(len(path_pts)): path_pts[i] = tf2_geometry_msgs.do_transform_pose( self.toPoseStamped(path_pts[i], Quaternion(0, 0, 0, 1)), trans).pose.position path_pts[i] = Point(path_pts[i].x, path_pts[i].y, 0) # Record the starting point and heading once if self.record_once: self.start_pt = trans.transform.translation self.record_once = False # Create a list with distance information between initial_pt and path_pts distance_list = [ self.getDistance(p, self.start_pt) for p in path_pts ] # Rearrange the path_pts to be in descending order path_pts = self.getAscend(path_pts, distance_list) # Duplicate the path_pts to prevent from being edited table_pathpts = path_pts[:] print "table_pathpts: ", len(table_pathpts) if (len(table_pathpts) == 2): # Get gradient of the Line of Best Fit m1 = self.getGradientLineOfBestFit(table_pathpts) # Insert another point on the line with d[m] away from the 2nd table_pathpts path_pts.append( self.getPointOnLine(m1, 1.0, table_pathpts[1], self.start_pt)) # Visualize the path_pts self.vizPoint(0, path_pts[0], ColorRGBA(1, 0, 0, 1), "map") # Red self.vizPoint(1, path_pts[1], ColorRGBA(1, 1, 0, 1), "map") # Yellow self.vizPoint(2, path_pts[2], ColorRGBA(1, 0, 1, 1), "map") # Magenta print len(path_pts) # Find the heading of the table (last 2 path_pts) heading_q = self.getOrientation(table_pathpts[0], table_pathpts[1]) # Publish the path self.route_pub.publish(self.getPath2Table(path_pts, heading_q)) print "-----------------"
def pc2CB(self, msg): if self.init: print "start_time", rospy.Time.now().to_sec() # Convert ROS PointCloud2 msg to PCL data cloud = pcl_helper.ros_to_pcl(msg) # Filter the Cloud fil_cloud = filtering_helper.do_passthrough_filter(cloud, 'x', self.x_min, self.x_max) filtered_cloud = filtering_helper.do_passthrough_filter(fil_cloud, 'y', self.y_min, self.y_max) # Turn cloud into colorless cloud # colorless_cloud = pcl_helper.XYZRGB_to_XYZ(filtered_cloud) colorless_cloud = pcl_helper.XYZI_to_XYZ(filtered_cloud) # Get groups of cluster of points # clusters = self.getClusters(colorless_cloud, 0.05, 20, 250) # Table on Top AGV - 60~230 clusters = self.getClusters(colorless_cloud, 0.05, 20, 350) # print "len(clusters)", len(clusters) # for i in range(0,len(clusters)): # print "len(clusters[", i, "]", len(clusters[i]) # Get mean points from clusters mean_pts = [] for cluster in clusters: mean_pts.append(self.getMeanPoint(cluster, filtered_cloud)) # print "len(mean_pts)", len(mean_pts) # for i in range(0,len(mean_pts)): # print "mean_pts[", i, "]", mean_pts[i] # Remove other points, leave the table points only table_pts = [] table_pts = self.getTablePoints(mean_pts) # print "len(table_pts)", len(table_pts) # print table_pts for i in range(len(table_pts)): self.vizPoint(i, table_pts[i], ColorRGBA(1,1,1,1), "base_link") # Get transform of "base_link" relative to "map" trans = self.getTransform("map", "base_link") # Finding 2 middle points from 4 table legs and use them as path_pts path_pts = [] if(len(table_pts)==4): for p1 in table_pts: for p2 in table_pts: if(p1==p2): break # Register 2 middle_pts of 4 table legs if(0.73 < self.getDistance(p1, p2) < 0.83): path_pts.append(self.getMiddlePoint(p1, p2)) break # print "len(path_pts)", len(path_pts) # print path_pts # Path_pts that generated from between 2 legs of table must be fulfill below requirement, else return if(len(path_pts)==2): if not (0.33 < self.getDistance(path_pts[0], path_pts[1]) < 0.48): return # Turn path_pts into map frame for i in range(len(path_pts)): path_pts[i] = tf2_geometry_msgs.do_transform_pose(self.toPoseStamped(path_pts[i], Quaternion(0,0,0,1)), trans).pose.position # Record the starting point once if self.record_once: self.start_pt = trans.transform.translation self.record_once = False # Create a list with distance information between initial_pt and path_pts distance_list = [self.getDistance(p, self.start_pt) for p in path_pts] # Rearrange the path_pts to be in ascending order path_pts = self.getAscend(path_pts, distance_list) # Duplicate the path_pts to prevent from being edited table_pathpts = path_pts[:] if(len(table_pathpts)==2): # Get gradient of the Line of Best Fit m1 = self.getGradientLineOfBestFit(table_pathpts) # Insert another point on the line with d[m] away from the 1st table_pathpts path_pts.insert(0, self.getPointOnLine(m1, 1.0, table_pathpts[0], self.start_pt)) # Insert a point ahead d[m] of table to the begin of path_pts # ahead_pt = self.getPointAheadTable(table_pathpts, 1.0) # path_pts.insert(0, ahead_pt) # Insert a point ahead d[m] of table to the middle of table_pathpts # midway_pt = self.getPointAheadTable(table_pathpts, -0.4) # path_pts.insert(2, midway_pt) # Insert another point on the line with d[m] away from the 1st table_pathpts # path_pts.insert(2, self.getPointOnLine(m1, 0.05, table_pathpts[1], self.start_pt)) # Remove the last point of path_pts # path_pts = path_pts[:len(path_pts)-1] # Add the AGV start_pt to the begin of path_pts # path_pts.insert(0, self.start_pt) # Visualize the path_pts # self.vizPoint(0, path_pts[0], ColorRGBA(1,0,0,1), "map") # Red # self.vizPoint(1, path_pts[1], ColorRGBA(1,1,0,1), "map") # Yellow # self.vizPoint(2, path_pts[2], ColorRGBA(0,0,1,1), "map") # Blue # Find the heading of the table (last 2 path_pts) heading_q = self.getOrientation(table_pathpts[0], table_pathpts[1]) # Publish the path self.route_pub.publish(self.getPath2Table(path_pts, heading_q)) # print "-----------------" print "end_time", rospy.Time.now().to_sec()
def pc2CB(self, msg): if self.init: self.start_time = rospy.Time.now().to_sec() print "-----------------" # Convert ROS PointCloud2 msg to PCL data cloud = pcl_helper.ros_to_pcl(msg) # Get transform of "base_link" relative to "map" trans = self.getTransform("map", "base_link") # Filter the Cloud fil_cloud = filtering_helper.do_passthrough_filter(cloud, 'x', -2.5, 2.5) fil_cloud = filtering_helper.do_passthrough_filter(fil_cloud, 'y', -1.5, 1.5) # Turn cloud into colorless cloud # colorless_cloud = pcl_helper.XYZRGB_to_XYZ(fil_cloud) # Statistical outlier filter # fil = colorless_cloud.make_statistical_outlier_filter() fil = fil_cloud.make_statistical_outlier_filter() fil.set_mean_k(30) fil.set_std_dev_mul_thresh(0.1) #1.0 / 0.1 filtered_cloud = fil.filter() # pc2_msg = pcl_helper.pcl_to_ros(filtered_cloud, "base_link") # self.pc2_pub.publish(pc2_msg) # Get groups of cluster of points clusters = self.getClusters(filtered_cloud, 0.05, 20, 1000) print "len(clusters)", len(clusters) for i in range(len(clusters)): print "clusters["+str(i)+"]", len(clusters[i]) # Not enough clusters to be identified as charger unit if(len(clusters)<3): return self.middle_time_1 = rospy.Time.now().to_sec() # Extract charger_wall & charger_pillars clusters # - use for compute gradient of Line of Best Fit # - use for compute middle_pt wall_cluster, pillar_clusters = self.getChargerClusters(clusters, filtered_cloud) self.middle_time_2 = rospy.Time.now().to_sec() # Cannot find the charger_wall and charger_pillars if(wall_cluster==None or pillar_clusters==[]): print "wall cluster or pillar clusters not found" return print "wall_cluster", len(wall_cluster) self.vizPoint(0, self.getMeanPointFromCluster(wall_cluster, filtered_cloud), ColorRGBA(0,1,0,1), "base_link") for i in range(len(pillar_clusters)): print "pillar_clusters["+str(i)+"]", len(pillar_clusters[i]) self.vizPoint(i+1, self.getMeanPointFromCluster(pillar_clusters[i], filtered_cloud), ColorRGBA(0,1,0,1), "base_link") # Get Points of wall_cluster wall_pts = self.getPointsFromCluster(wall_cluster, filtered_cloud) # Turn wall_pts into map frame for i in range(len(wall_cluster)): wall_pts[i] = tf2_geometry_msgs.do_transform_pose(self.toPoseStamped(wall_pts[i], Quaternion(0,0,0,1)), trans).pose.position # Get gradient of the Line of Best Fit m1 = self.getGradientLineOfBestFit(wall_pts) print "gradient", m1 pillar_pts = [] # Get mean points from pillar_clusters for cluster in pillar_clusters: pillar_pts.append(self.getMeanPointFromCluster(cluster, filtered_cloud)) # Finding 1 middle_pt from 2 charger_pillars and use it as path_pts print pillar_pts if(len(pillar_pts)==2): middle_pt = None for p1 in pillar_pts: for p2 in pillar_pts: if(p1==p2): break # Only register middle_pt of 2 charger_pillars points if(0.5 < self.getDistance(p1, p2) < 0.65): middle_pt = self.getMiddlePoint(p1, p2) break if(middle_pt==None): "middle_pt not found" return # Turn middle_pt into map frame middle_pt = tf2_geometry_msgs.do_transform_pose(self.toPoseStamped(middle_pt, Quaternion(0,0,0,1)), trans).pose.position # Register the middle_pt as one of the path_pts path_pts = [] path_pts.append(middle_pt) # Record the starting point and heading once if self.record_once: self.start_pt = trans.transform.translation self.record_once = False # Insert another point on the normal line with d[m] away from middle_pt path_pts.insert(0, self.getPointOnNormalLine(m1, 1.0, middle_pt, self.start_pt)) # Duplicate the path points to prevent from being edited charger_pathpts = path_pts[:] # Insert another point on the normal line with d[m] away from middle_pt path_pts.insert(0, self.getPointOnNormalLine(m1, 0.3, middle_pt, self.start_pt)) # Create a list with distance information between init_pt and path_pts distance_list = [self.getDistance(p, self.start_pt) for p in path_pts] # Rearrange the path_pts to be in ascending order path_pts = self.getAscend(path_pts, distance_list) # Add the AGV start_pt to the begin of path_pts # path_pts.insert(0, self.start_pt) # Remove last point of path_pts (middle pt of 2 charger feature pts) path_pts = path_pts[:len(path_pts)-1] print "len(path_pts)", len(path_pts) # # Visualize the path_pts self.vizPoint(0, path_pts[0], ColorRGBA(1,0,0,1), "map") # Red self.vizPoint(1, path_pts[1], ColorRGBA(1,1,0,1), "map") # Yellow # self.vizPoint(2, path_pts[2], ColorRGBA(0,0,1,1), "map") # Blue # Find the heading of the charger (last 2 path_pts) heading_q = self.getOrientation(charger_pathpts[0], charger_pathpts[1]) # Publish the path self.route_pub.publish(self.getPath2Charger(path_pts, heading_q)) print "start_time", self.start_time print "middle_time_1", self.middle_time_1 print "middle_time_2", self.middle_time_2 print "end_time", rospy.Time.now().to_sec()
def pc2CB(self, msg): if self.init: # Convert ROS PointCloud2 msg to PCL data cloud = pcl_helper.ros_to_pcl(msg) # Get transform of "base_link" relative to "map" trans = self.getTransform("map", "base_link") # Filter the Cloud fil_cloud = filtering_helper.do_passthrough_filter( cloud, 'x', -3.0, 3.0) fil_cloud = filtering_helper.do_passthrough_filter( fil_cloud, 'y', -3.0, 3.0) # Turn cloud into colorless cloud colorless_cloud = pcl_helper.XYZRGB_to_XYZ(fil_cloud) # source_cloud = colorless_cloud # Get groups of cluster of points clusters = self.getClusters(colorless_cloud, 0.05, 100, 450) # print "len(clusters)", len(clusters) source_cloud = self.getCloudFromCluster(clusters[0], colorless_cloud) middle_pt = self.getMeanPoint(clusters[0], colorless_cloud) self.vizPoint(0, middle_pt, ColorRGBA(1, 1, 1, 1), "base_link") icp = source_cloud.make_IterativeClosestPoint() converged, transform, estimate, fitness = icp.icp( source_cloud, self.target_cloud, 100) # gicp = colorless_cloud.make_GeneralizedIterativeClosestPoint() # converged, transform, estimate, fitness = gicp.gicp(colorless_cloud, self.target_cloud, 100) # icp_nl = colorless_cloud.make_IterativeClosestPointNonLinear() # converged, transform, estimate, fitness = icp_nl.icp_nl(self.target_cloud, colorless_cloud, 100) # if not converged: print "converged", converged print "transform", transform print "estimate", estimate print "fitness", fitness # new_transform = transform.inverse() # print new_transform translation = Vector3(transform[0, 3], transform[1, 3], transform[2, 3]) q = tf.transformations.quaternion_from_matrix(transform) rotation = Quaternion(q[0], q[1], q[2], q[3]) roll, pitch, yaw = tf.transformations.euler_from_matrix(transform) print np.rad2deg(yaw) # print "translation\n", translation # print "rotation\n", rotation # transformation = self.toTransformStamped(translation, rotation, "map", "base_link") print "------------------------------------------------" # Visualize the cloud outmsg = pcl_helper.pcl_to_ros(estimate, "map") outmsg.header = msg.header self.pc2_pub.publish(outmsg)
def pc2CB(self, msg): if self.init: # Convert ROS PointCloud2 msg to PCL data cloud = pcl_helper.ros_to_pcl(msg) # Filter the Cloud fil_cloud = filtering_helper.do_passthrough_filter(cloud, 'x', -2.5, 2.5) filtered_cloud = filtering_helper.do_passthrough_filter(fil_cloud, 'y', -2.5, 2.5) # Turn cloud into colorless cloud colorless_cloud = pcl_helper.XYZRGB_to_XYZ(filtered_cloud) # Get groups of cluster of points clusters = self.getClusters(colorless_cloud, 0.05, 10, 1000) print "len(clusters)", len(clusters) # Extract charger wall cluster - use for compute Line of Best Fit if(len(clusters) != 0): size_clusters = [] for cluster in clusters: size_clusters.append(len(cluster)) # Find the biggest cluster max_id = size_clusters.index(max(size_clusters)) wall_cluster = clusters.pop(max_id) print "len(wall_cluster)", len(wall_cluster) feature_clusters = clusters print "len(feature_clusters)", len(feature_clusters) # Get transform of "base_link" relative to "map" trans = self.getTransform("map", "base_link") # Record the starting point and heading once if self.record_once: self.start_pt = trans.transform.translation self.record_once = False # Get Points of wall cluster wall_pts = self.getPointsFromCluster(wall_cluster, filtered_cloud) # Turn wall_pts into map frame for i in range(len(wall_cluster)): wall_pts[i] = tf2_geometry_msgs.do_transform_pose(self.toPoseStamped(wall_pts[i], Quaternion(0,0,0,1)), trans).pose.position # Get gradient of the Line of Best Fit m1 = self.getGradientLineOfBestFit(wall_pts) print m1 # Get mean points from feature clusters mean_pts = [] for cluster in feature_clusters: mean_pts.append(self.getMeanPointFromCluster(cluster, filtered_cloud)) print "len(mean_pts)", len(mean_pts) for i in range(len(mean_pts)): self.vizPoint(i, mean_pts[i], ColorRGBA(1,1,1,1), "base_link") # Finding 1 middle point from 2 charger feature columns and use it as path_pts if(len(mean_pts)>=2): for p1 in mean_pts: for p2 in mean_pts: if(p1==p2): break # Only register middle_pt of 2 charger feature points if(0.5 < self.getDistance(p1, p2) < 0.65): middle_pt = self.getMiddlePoint(p1, p2) # Turn middle_pt into map frame middle_pt = tf2_geometry_msgs.do_transform_pose(self.toPoseStamped(middle_pt, Quaternion(0,0,0,1)), trans).pose.position # Register the middle_pt as one of the path_pts path_pts = [] path_pts.append(middle_pt) # Insert another point on the normal line with d[m] away from middle_pt path_pts.insert(0, self.getPointOnNormalLine(m1, 1.0, middle_pt, self.start_pt)) # Duplicate the path points to prevent from being edited feature_pathpts = path_pts[:] # Insert another point on the normal line with d[m] away from middle_pt path_pts.insert(1, self.getPointOnNormalLine(m1, 0.3, middle_pt, self.start_pt)) # Create a list with distance information between init_pt and path_pts distance_list = [self.getDistance(p, self.start_pt) for p in path_pts] # Rearrange the path_pts to be in ascending order path_pts = self.getAscend(path_pts, distance_list) print "len(path_pts)", len(path_pts) # Add the AGV start_pt to the begin of path_pts # path_pts.insert(0, self.start_pt) # Remove last point of path_pts (middle pt of 2 charger feature pts) path_pts = path_pts[:len(path_pts)-1] # Visualize the path_pts self.vizPoint(0, path_pts[0], ColorRGBA(1,0,0,1), "map") # Red self.vizPoint(1, path_pts[1], ColorRGBA(1,1,0,1), "map") # Yellow # self.vizPoint(2, path_pts[2], ColorRGBA(0,0,1,1), "map") # Blue # Find the heading of the charger (last 2 path_pts) heading_q = self.getOrientation(feature_pathpts[-2], feature_pathpts[-1]) # Publish the path self.route_pub.publish(self.getPath2Charger(path_pts, heading_q)) print "-----------------"
def main_loop(self): while not rospy.is_shutdown(): print "--------------------------------------------" self.while_start_time = rospy.Time.now().to_sec() # self.load_params_from_yaml(self.filepath) print "fsm_state", self.fsm_state # Setting Detection-Region for stopping detection_region = self.getDetectionRegion(self.fsm_state, self.lifter_status, self.speed) # Finding Min & Max of detection region x_max, x_min, y_max, y_min = self.getMinMaxXYFromRegion(detection_region) # Passthrough Filter fil_cloud = filtering_helper.do_passthrough_filter(self.cloud, 'x', x_min, x_max) fil_cloud = filtering_helper.do_passthrough_filter(fil_cloud, 'y', y_min, y_max) # Use Euclidean clustering to filter out noise clusters = self.getClusters(fil_cloud, 0.05, 10, 9000) # Setting Front-region for screenshot front_region = self.camera_region # Visualize Detection Region self.visualize_region(detection_region) self.middle_time_1 = rospy.Time.now().to_sec() # Grouping clusters into 2 groups of points detected_pts = self.getPointsInRegionFromClusters(clusters, fil_cloud, detection_region) front_pts = self.getPointsInRegionFromPoints(detected_pts, front_region) self.middle_time_2 = rospy.Time.now().to_sec() print "detected_pts", len(detected_pts) # If Obstacle detected, do following: if(len(detected_pts)!=0): self.stopping() print "--------------------------------------" print "| Obstacle detected and stopping AGV |" print "--------------------------------------" # Rosservice call the health_monitoring of obstacle blocking if(rospy.Time.now().to_sec() - self.obs_timer >= self.obs_timeout): print "#######################################################" print "########## calling health_monitoring ##########" print "#######################################################" self.obstacle_health_call() self.obs_timer = rospy.Time.now().to_sec() # Use for screenshot once per obstacle at front if(self.fsm_state!="ERROR"): self.error = 1 # Rosservice call to screenshot the front obstacle once print "front_pts", len(front_pts) if(len(front_pts)!=0 and self.fsm_state=="ERROR" and self.error==1): self.error = 0 print "########################################" print "########## screenshot ##########" print "########################################" self.screenshot_call() self.prev_detection_region = detection_region print "while_start_time", self.while_start_time print "while_middle_time_1", self.middle_time_1 print "while_middle_time_2", self.middle_time_2 print "while_end_time", rospy.Time.now().to_sec() self.rate.sleep()