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

Velocities (dxy/dt, dtheta/dt) instead of (dxy/dt, dtheta/dt, 0)

Browse Source
This commit is contained in:
Simon D. Levy
2018-06-03 01:00:43 -04:00
parent d819913cb1
commit 8934a3370f
2 changed files with 7 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/logmovie.py
View File

@@ -68,7 +68,7 @@ def threadfunc(robot, slam, timestamps, lidars, odometries, mapbytes, pose):
# Convert odometry to velocities # Convert odometry to velocities
velocities = robot.computeVelocities(odometries[scanno]) velocities = robot.computeVelocities(odometries[scanno])
# Update SLAM with lidar and velocities # Update SLAM with lidar and velocities
slam.update(lidars[scanno], velocities) slam.update(lidars[scanno], velocities)

12
python/breezyslam/algorithms.py
View File

@@ -81,7 +81,7 @@ class CoreSLAM(object):
self.laser = laser self.laser = laser
# Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry # Initialize velocities (dxyMillimeters, dthetaDegrees, dtSeconds) for odometry
self.velocities = (0, 0, 0) self.velocities = (0, 0)
# Initialize a scan for computing distance to map, and one for updating map # Initialize a scan for computing distance to map, and one for updating map
self.scan_for_distance = pybreezyslam.Scan(laser, 1) self.scan_for_distance = pybreezyslam.Scan(laser, 1)
@@ -105,12 +105,12 @@ class CoreSLAM(object):
self._scan_update(self.scan_for_mapbuild, scans_mm) self._scan_update(self.scan_for_mapbuild, scans_mm)
self._scan_update(self.scan_for_distance, scans_mm) self._scan_update(self.scan_for_distance, scans_mm)
# Update velocities # Convert (dxy,dtheta,dt) to (dxy/dt, dtheta/dt) for scan update
velocity_factor = (1 / velocities[2]) if (velocities[2] > 0) else 0 velocity_factor = (1 / velocities[2]) if (velocities[2] > 0) else 0 # units => units/sec
new_dxy_mm = velocities[0] * velocity_factor new_dxy_mm = velocities[0] * velocity_factor
new_dtheta_degrees = velocities[1] * velocity_factor new_dtheta_degrees = velocities[1] * velocity_factor
self.velocities = (new_dxy_mm, new_dtheta_degrees, 0) self.velocities = (new_dxy_mm, new_dtheta_degrees)
# Implementing class updates map and pointcloud # Implementing class updates map and pointcloud
self._updateMapAndPointcloud(self.velocities, should_update_map) self._updateMapAndPointcloud(self.velocities, should_update_map)
@@ -178,7 +178,7 @@ class SinglePositionSLAM(CoreSLAM):
# Add effect of velocities # Add effect of velocities
start_pos.x_mm += velocities[0] * self._costheta() start_pos.x_mm += velocities[0] * self._costheta()
start_pos.y_mm += velocities[0] * self._sintheta() start_pos.y_mm += velocities[0] * self._sintheta()
start_pos.theta_degrees = start_pos.theta_degrees + velocities[1] start_pos.theta_degrees += velocities[1]
# Add offset from laser # Add offset from laser
start_pos.x_mm += self.laser.offset_mm * self._costheta() start_pos.x_mm += self.laser.offset_mm * self._costheta()