def _getNewPosition(self, start_position):
"""
Filters the rmhc slam result.
start_position: estimated position based on odometry
"""
# RMHC search is implemented as a C extension for efficiency
slam_position = RMHC_SLAM._getNewPosition(self, start_position)
#check slam values for reliable turn.
#vtheta = (slam_position.theta_degrees - start_position.theta_degrees)/self.time
#if(math.fabs(vtheta) > MAX_DEGREE_PER_S):
# vtheta = vtheta/math.fabs(vtheta) * MAX_DEGREE_PER_S
# slam_position.theta_degrees = start_position.theta_degrees + vtheta * self.time
if not filtering: return slam_position
if self.time == None:
return slam_position
return self.myfilter(slam_position, start_position, self.error, self.time, self.command)
def _getNewPosition(self, start_position):
"""
Filters the rmhc slam result.
start_position: estimated position based on odometry
"""
# RMHC search is implemented as a C extension for efficiency
slam_position = RMHC_SLAM._getNewPosition(self, start_position)
#check slam values for reliable turn.
#vtheta = (slam_position.theta_degrees - start_position.theta_degrees)/self.time
#if(math.fabs(vtheta) > MAX_DEGREE_PER_S):
# vtheta = vtheta/math.fabs(vtheta) * MAX_DEGREE_PER_S
# slam_position.theta_degrees = start_position.theta_degrees + vtheta * self.time
if not filtering: return slam_position
if self.time == None:
return slam_position
return self.myfilter(slam_position, start_position, self.error,
self.time, self.command)
