From e4abf615d3e511a0ecf95d4938bf2e09095c2d07 Mon Sep 17 00:00:00 2001 From: lilpharaoh1 Date: Fri, 2 Sep 2022 09:49:47 +0000 Subject: [PATCH 1/4] init-commit --- range_libc | 1 + src/particle_filter.py | 12 +++++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) create mode 160000 range_libc diff --git a/range_libc b/range_libc new file mode 160000 index 0000000..1251dc3 --- /dev/null +++ b/range_libc @@ -0,0 +1 @@ +Subproject commit 1251dc3c72f8dd72204ca32ac745fa90cb097e4e diff --git a/src/particle_filter.py b/src/particle_filter.py index 05082f8..374beee 100755 --- a/src/particle_filter.py +++ b/src/particle_filter.py @@ -1,5 +1,6 @@ #!/usr/bin/env python + # packages import rospy import numpy as np @@ -122,6 +123,8 @@ def __init__(self): self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1) self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1) + self.pose_pub = rospy.Publisher('/pf_pose', PoseStamped, queue_size=1) + print "Finished initializing, waiting on messages..." def get_omap(self): @@ -343,7 +346,8 @@ def initialize_global(self): permissible_states[:,2] = np.random.random(self.MAX_PARTICLES) * np.pi * 2.0 Utils.map_to_world(permissible_states, self.map_info) - self.particles = permissible_states + # self.particles = permissible_states + self.particles = np.zeros((self.MAX_PARTICLES, 3)) self.weights[:] = 1.0 / self.MAX_PARTICLES self.state_lock.release() @@ -632,6 +636,12 @@ def update(self): # compute the expected value of the robot pose self.inferred_pose = self.expected_pose() + + msg = PoseStamped() + msg.pose.position.x, msg.pose.position.y, msg.pose.position.z = self.inferred_pose + #x = xposistion, y = posistion, z = ORIENTATION + self.pose_pub.publish(msg) + self.state_lock.release() t2 = time.time() From d3b1b0f821bccabb3e88053b04a7568851a6d6af Mon Sep 17 00:00:00 2001 From: lilpharaoh1 Date: Fri, 2 Sep 2022 16:33:16 +0000 Subject: [PATCH 2/4] Made some changes in the launch files, more compatible with the car --- launch/map_server.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/map_server.launch b/launch/map_server.launch index 348e51c..0db5cb8 100644 --- a/launch/map_server.launch +++ b/launch/map_server.launch @@ -1,4 +1,4 @@ - + \ No newline at end of file From 8833c91e87014a2b1665cc91409de7a102b6fafb Mon Sep 17 00:00:00 2001 From: lilpharaoh1 Date: Fri, 2 Sep 2022 16:42:26 +0000 Subject: [PATCH 3/4] Added /pf_init topic, needs testing --- launch/map_server.launch | 1 + src/particle_filter.py | 11 ++++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/launch/map_server.launch b/launch/map_server.launch index 0db5cb8..ffb2754 100644 --- a/launch/map_server.launch +++ b/launch/map_server.launch @@ -1,4 +1,5 @@ + \ No newline at end of file diff --git a/src/particle_filter.py b/src/particle_filter.py index 374beee..56dfb8a 100755 --- a/src/particle_filter.py +++ b/src/particle_filter.py @@ -12,7 +12,7 @@ import utils as Utils # messages -from std_msgs.msg import String, Header, Float32MultiArray +from std_msgs.msg import String, Header, Float32MultiArray, Bool from sensor_msgs.msg import LaserScan from visualization_msgs.msg import Marker from geometry_msgs.msg import Point, Pose, PoseStamped, PoseArray, Quaternion, PolygonStamped,Polygon, Point32, PoseWithCovarianceStamped, PointStamped @@ -124,6 +124,7 @@ def __init__(self): self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1) self.pose_pub = rospy.Publisher('/pf_pose', PoseStamped, queue_size=1) + self.pf_init_pub = rospy.Publisher('/pf_init', Bool, queue_size=1) print "Finished initializing, waiting on messages..." @@ -619,6 +620,10 @@ def update(self): Ensures the state is correctly initialized, and acquires the state lock before proceeding. ''' if self.lidar_initialized and self.odom_initialized and self.map_initialized: + init_msg = Bool() + init_msg.data = True + self.pf_init_pub.publish(init_msg) + self.pf_init_pub if self.state_lock.locked(): print "Concurrency error avoided" else: @@ -655,6 +660,10 @@ def update(self): print "iters per sec:", int(self.timer.fps()), " possible:", int(self.smoothing.mean()) self.visualize() + else: + init_msg = Bool() + init_msg.data = False + self.pf_init_pub.publish(init_msg) import argparse import sys From d2cc0db026466914a8ec1e975319e0b22208dfcf Mon Sep 17 00:00:00 2001 From: lilpharaoh1 Date: Sun, 4 Sep 2022 13:24:12 +0000 Subject: [PATCH 4/4] added preprocess --- launch/localize.launch | 2 ++ launch/map_server.launch | 2 +- src/particle_filter.py | 5 +++-- src/preprocess.py | 24 ++++++++++++++++++++++++ 4 files changed, 30 insertions(+), 3 deletions(-) create mode 100644 src/preprocess.py diff --git a/launch/localize.launch b/launch/localize.launch index 56358a4..0655b2f 100644 --- a/launch/localize.launch +++ b/launch/localize.launch @@ -29,6 +29,7 @@ --> + @@ -38,6 +39,7 @@ + diff --git a/launch/map_server.launch b/launch/map_server.launch index ffb2754..a3cd99f 100644 --- a/launch/map_server.launch +++ b/launch/map_server.launch @@ -1,5 +1,5 @@ - + \ No newline at end of file diff --git a/src/particle_filter.py b/src/particle_filter.py index 56dfb8a..5a011cd 100755 --- a/src/particle_filter.py +++ b/src/particle_filter.py @@ -52,6 +52,7 @@ def __init__(self): self.SHOW_FINE_TIMING = bool(rospy.get_param("~fine_timing", "0")) self.PUBLISH_ODOM = bool(rospy.get_param("~publish_odom", "1")) self.DO_VIZ = bool(rospy.get_param("~viz")) + self.MAP_ADDR = str(rospy.get_param("~map_addr")) # sensor model constants self.Z_SHORT = float(rospy.get_param("~z_short", 0.01)) @@ -123,7 +124,7 @@ def __init__(self): self.pose_sub = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.clicked_pose, queue_size=1) self.click_sub = rospy.Subscriber("/clicked_point", PointStamped, self.clicked_pose, queue_size=1) - self.pose_pub = rospy.Publisher('/pf_pose', PoseStamped, queue_size=1) + self.pf_pose_pub = rospy.Publisher('/pf_pose', PoseStamped, queue_size=1) self.pf_init_pub = rospy.Publisher('/pf_init', Bool, queue_size=1) print "Finished initializing, waiting on messages..." @@ -645,7 +646,7 @@ def update(self): msg = PoseStamped() msg.pose.position.x, msg.pose.position.y, msg.pose.position.z = self.inferred_pose #x = xposistion, y = posistion, z = ORIENTATION - self.pose_pub.publish(msg) + self.pf_pose_pub.publish(msg) self.state_lock.release() t2 = time.time() diff --git a/src/preprocess.py b/src/preprocess.py new file mode 100644 index 0000000..69cc4cc --- /dev/null +++ b/src/preprocess.py @@ -0,0 +1,24 @@ +class MapPreprocessor(): + + def processMap(map_file_name): + #produces a black and white pgm file + + from matplotlib import pyplot + import cv2 + + THRESHOLD = 127 + MAX = 255 + + map_pixel_array = cv2.imread(map_file_name, cv2.IMREAD_GRAYSCALE) + + map_pixel_array_processed = cv2.threshold(map_pixel_array, THRESHOLD, MAX, cv2.THRESH_BINARY)[1] + + # pyplot.imshow(map_pixel_array_processed, cmap = 'gray') + + # pyplot.show() # for viewing output pgm file​ + map_file_name_processed = "../maps/fta_f110/f110-map.pgm" + cv2.imwrite(map_file_name_processed, map_pixel_array_processed) + +if ( __name__ == '__main__' ): + map_file_name = "f110-map.pgm" + MapPreprocessor.processMap(map_file_name) \ No newline at end of file