Pf init

launch/localize.launch

@@ -29,6 +29,7 @@
 	 -->
 	<include file="$(find particle_filter)/launch/map_server.launch"/>
 
+	<arg name="map_addr" default="(find particle_filter)/maps/fta_f110/f110-map.yaml"/>
 	<arg name="scan_topic" default="/scan"/>
 	<arg name="odometry_topic" default="/vesc/odom"/>
 	<!-- <arg name="odometry_topic" default="/odom"/> -->
@@ -38,6 +39,7 @@
 	<arg name="viz" default="1"/>
 
 	<node pkg="particle_filter" type="particle_filter.py" name="Particle_filter" output="screen">
+		<param name="map_addr" default="$(arg map_addr)"/>
 		<param name="scan_topic" value="$(arg scan_topic)"/>
 		<param name="odometry_topic" value="$(arg odometry_topic)"/>
 		<!-- range data is downsampled by this factor -->

launch/map_server.launch

@@ -1,4 +1,5 @@
 <launch>
-    <arg name="map" default="$(find particle_filter)/maps/basement_fixed.map.yaml"/>
+    <!-- <arg name="map" default="$(find particle_filter)/maps/basement_fixed.map.yaml"/> -->
+    <arg name="map" default="$(find particle_filer)/maps/fta_f110/f110-map.yaml"/>
     <node pkg="map_server" name="map_server" type="map_server" args="$(arg map)" />
 </launch>

src/particle_filter.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python
 
+
 # packages
 import rospy
 import numpy as np
@@ -11,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
@@ -51,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))
@@ -122,6 +124,9 @@ 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.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..."
 
     def get_omap(self):
@@ -343,7 +348,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()
 
@@ -615,6 +621,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:
@@ -632,6 +642,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.pf_pose_pub.publish(msg)
+
                 self.state_lock.release()
                 t2 = time.time()
 
@@ -645,6 +661,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

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)