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()
