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)
