def intHandler(self, signum, frame): register = Registration() register.frame_id = map_topic register.toAdd = False register.resolution = 0.0 register.freePoints = [] self._registration_publisher.publish(register) raise KeyboardInterrupt()
def __init__(self, _map_topic, _algorithm_name): global map_topic map_topic = _map_topic algorithm_name = _algorithm_name signal.signal(signal.SIGINT, self.intHandler) #PARTICLES self.numParticles = 300 self.lock = Lock() # Minimum change (m/radians) before publishing new particle cloud and pose self._PUBLISH_DELTA = rospy.get_param("publish_delta", 0.1) self._particle_filter = pf_localisation.pf.PFLocaliser(self.numParticles, map_topic, _algorithm_name) self._latest_scan = None self._last_published_pose = None self._initial_pose_received = False self._registration_publisher = rospy.Publisher("/regNode", Registration) self._pose_publisher = rospy.Publisher("/estimatedpose_" + map_topic, PoseStamped) self._amcl_pose_publisher = rospy.Publisher("/amcl_pose", PoseWithCovarianceStamped) self._cloud_publisher = rospy.Publisher("/particlecloud_" + map_topic, PoseArray) self._tf_publisher = rospy.Publisher("/tf", tfMessage) self._init_pose_publisher = rospy.Publisher("/initialpose", PoseWithCovarianceStamped) rospy.loginfo("Waiting for a map...") try: ocuccupancy_map = rospy.wait_for_message(map_topic, OccupancyGrid, 20) except: rospy.logerr("Problem getting a map. Check that you have a map_server" " running: rosrun map_server map_server <mapname> " ) sys.exit(1) rospy.loginfo("Map received. %d X %d, %f px/m." % (ocuccupancy_map.info.width, ocuccupancy_map.info.height, ocuccupancy_map.info.resolution)) self._particle_filter.set_map(ocuccupancy_map) self._laser_subscriber = rospy.Subscriber("/base_scan", LaserScan, self._laser_callback, queue_size=1) self._initial_pose_subscriber = rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self._initial_pose_callback) self._odometry_subscriber = rospy.Subscriber("/odom", Odometry, self._odometry_callback, queue_size=1) register = Registration() register.frame_id = map_topic register.toAdd = True register.resolution = self._particle_filter.occupancy_map.info.resolution register.freePoints = self._particle_filter.listFreePoints self._registration_publisher.publish(register)