vato007/breezyslam
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Pass dxy_mm, dtheta_degrees to _updateMapAndPointCloud()

Browse Source
This commit is contained in:
Simon D. Levy
2018-06-03 14:49:07 -04:00
parent 081d33aedf
commit df149435e6
2 changed files with 9 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
python/breezyslam/algorithms.py
View File

@@ -1,6 +1,6 @@
''' '''
BreezySLAM: Simple, efficient SLAM in Python BreezySLAM: Simple, efficient SLAM in Python
#
algorithms.py: SLAM algorithms algorithms.py: SLAM algorithms
Copyright (C) 2014 Simon D. Levy Copyright (C) 2014 Simon D. Levy
@@ -90,21 +90,21 @@ class CoreSLAM(object):
# Initialize the map # Initialize the map
self.map = pybreezyslam.Map(map_size_pixels, map_size_meters) self.map = pybreezyslam.Map(map_size_pixels, map_size_meters)
def update(self, scans_mm, velocities, should_update_map=True): def update(self, scans_mm, poseChange, should_update_map=True):
''' '''
Updates the scan and odometry, and calls the the implementing class's _updateMapAndPointcloud method with Updates the scan and odometry, and calls the the implementing class's _updateMapAndPointcloud method with
the specified velocities. the specified pose change.
scan_mm is a list of Lidar scan values, whose count is specified in the scan_size scan_mm is a list of Lidar scan values, whose count is specified in the scan_size
attribute of the Laser object passed to the CoreSlam constructor attribute of the Laser object passed to the CoreSlam constructor
velocities is a tuple of velocities (dxy_mm, dtheta_degrees, dt_seconds) computed from odometry poseChange is a tuple (dxy_mm, dtheta_degrees, dt_seconds) computed from odometry
should_update_map flags for whether you want to update the map should_update_map flags for whether you want to update the map
''' '''
# Convert (dxy,dtheta,dt) to (dxy/dt, dtheta/dt) for scan update # Convert (dxy,dtheta,dt) to (dxy/dt, dtheta/dt) for scan update
velocity_factor = (1 / velocities[2]) if (velocities[2] > 0) else 0 # units => units/sec velocity_factor = (1 / poseChange[2]) if (poseChange[2] > 0) else 0 # units => units/sec
dxy_mm_dt = velocities[0] * velocity_factor dxy_mm_dt = poseChange[0] * velocity_factor
dtheta_degrees_dt = velocities[1] * velocity_factor dtheta_degrees_dt = poseChange[1] * velocity_factor
self.velocities = (dxy_mm_dt, dtheta_degrees_dt) self.velocities = (dxy_mm_dt, dtheta_degrees_dt)
# Build a scan for computing distance to map, and one for updating map # Build a scan for computing distance to map, and one for updating map
@@ -112,7 +112,7 @@ class CoreSLAM(object):
self._scan_update(self.scan_for_distance, scans_mm) self._scan_update(self.scan_for_distance, scans_mm)
# Implementing class updates map and pointcloud # Implementing class updates map and pointcloud
self._updateMapAndPointcloud(velocities[0], velocities[1], should_update_map) self._updateMapAndPointcloud(poseChange[0], poseChange[1], should_update_map)
def getmap(self, mapbytes): def getmap(self, mapbytes):
''' '''

2
python/pybreezyslam.c
View File

@@ -341,7 +341,7 @@ static PyMethodDef Scan_methods[] =
"Scan.update(scans_mm, hole_width_mm, velocities=None) updates scan.\n"\ "Scan.update(scans_mm, hole_width_mm, velocities=None) updates scan.\n"\
"scans_mm is a list of integers representing scanned distances in mm.\n"\ "scans_mm is a list of integers representing scanned distances in mm.\n"\
"hole_width_mm is the width of holes (obstacles, walls) in millimeters.\n"\ "hole_width_mm is the width of holes (obstacles, walls) in millimeters.\n"\
"velocities is an optional tuple containing at least dxy_mm, dtheta_degrees;\n"\ "velocities is an optional tuple containing (dxy_mm/dt, dtheta_degrees/dt);\n"\
"i.e., robot's (forward, rotational velocity) for improving the quality of the scan." "i.e., robot's (forward, rotational velocity) for improving the quality of the scan."
}, },
{NULL} // Sentinel {NULL} // Sentinel