__init__.py
Go to the documentation of this file.
00001 import roslib
00002 roslib.load_manifest('karto_scan_matcher')
00003 
00004 import geometry_msgs.msg as gm
00005 import sensor_msgs.msg as sm
00006 
00007 from karto_scan_matching import *
00008 
00009 ############################################################
00010 # Public interface
00011 ############################################################
00012 
00013 def make_matcher(init_scan, laser_pose, search_size, search_resolution):
00014     """
00015     @param init_scan: Initial scan used only to read parameters of laser scanner
00016     @type init_scan: sensor_msgs.msg.LaserScan
00017     @param laser_pose: Pose of laser wrt base frame
00018     @type laser_pose: Tuple of form (x, y, theta)
00019     @param search_size: Radius of square to do matching in (meters)
00020     @param search_resolution: Resolution of grid search (meters)
00021     @return Opaque object of type KartoScanMatcher, which can be used as an argument to repeated calls to match_scans.
00022     """
00023     scan = scan_from_ros(init_scan)
00024     pose = pose_from_tuple(p)
00025     return KartoScanMatcherCpp(scan, pose, search_size, search_resolution)
00026 
00027 
00028 def match_scans(matcher, scan, init_pose, ref_scans):
00029     """
00030     @param matcher: Object returned by make_matcher
00031     @param scan: Scan to match
00032     @type scan: sensor_msgs.msg.LaserScan
00033     @param init_pose: Initial pose estimate
00034     @type init_pose: Tuple of form (x, y, theta)
00035     @param ref_scans: Reference scans
00036     @type ref_scans: Vector of tuples ((x, y, theta), sensor_msgs.msg.LaserScan)
00037     @return Tuple (opt_pose, response)
00038     """
00039     res = matcher.scanMatch(scan_from_ros(scan), pose_from_tuple(init_pose),
00040                             convert_ref_scans(ref_scans))
00041     return ((res.pose.x, res.pose.y, res.pose.theta), res.response)
00042 
00043 
00044 
00045 
00046 ############################################################
00047 # Internal
00048 ############################################################
00049 
00050 def scan_from_ros(s):
00051     scan = LaserScan()
00052     scan.header.frame_id = s.header.stamp.frame_id
00053     scan.range_min = s.range_min
00054     scan.range_max = s.range_max
00055     scan.angle_min = s.angle_min
00056     scan.angle_max = s.angle_max
00057     scan.angle_increment = s.angle_increment
00058     scan.ranges = s.ranges
00059     return scan
00060     
00061 def pose_from_tuple(p):
00062     pose = Pose2D()
00063     pose.x = p[0]
00064     pose.y = p[1]
00065     pose.z = p[2]
00066     return pose
00067 
00068 def convert_ref_scan(ref_scan):
00069     return ScanWithPose(ref_scan[1], pose_from_tuple(ref_scan[0]))
00070 
00071 def convert_ref_scans(scans):
00072     return map(convert_ref_scan, scans)


karto_scan_matcher
Author(s): Bhaskara Marthi
autogenerated on Wed Apr 23 2014 10:30:02