class MapperNode(object):
def __init__(self):
# Get the ground truth map from stdr; we use this to figure out the dimensions of the map
rospy.init_node('mapper_node', anonymous=True)
self.mapServer = rospy.ServiceProxy('static_map', GetMap)
resp = self.mapServer()
self.entropyFile = open(
'/home/ros_user/cw2_catkin_ws/src/comp0037/comp0037_mapper/src/comp0037_mapper/entropy.txt',
'w')
# Drawing options
self.showOccupancyGrid = rospy.get_param('show_mapper_occupancy_grid',
True)
self.showDeltaOccupancyGrid = rospy.get_param(
'show_mapper_delta_occupancy_grid', True)
# Create the publisher for the regular map messages
self.mapUpdatePublisher = rospy.Publisher('updated_map',
MapUpdate,
queue_size=1)
# Get the map scale
mapScale = rospy.get_param('plan_scale', 5)
# Create the occupancy grid map. This is the "raw" one from the sensor.
self.occupancyGrid = OccupancyGrid(resp.map.info.width,
resp.map.info.height,
resp.map.info.resolution, 0.5)
self.occupancyGrid.setScale(mapScale)
self.occupancyGrid.scaleEmptyMap()
# Create the delta occupancy grid map. This stores the difference since the last time the map was sent out
self.deltaOccupancyGrid = OccupancyGrid(resp.map.info.width,
resp.map.info.height,
resp.map.info.resolution, 0)
self.deltaOccupancyGrid.setScale(mapScale)
self.deltaOccupancyGrid.scaleEmptyMap()
windowHeight = rospy.get_param('maximum_window_height_in_pixels', 600)
if self.showOccupancyGrid is True:
self.occupancyGridDrawer = OccupancyGridDrawer('Mapper Node Occupancy Grid',\
self.occupancyGrid, windowHeight)
self.occupancyGridDrawer.open()
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridForShow = OccupancyGrid(
resp.map.info.width, resp.map.info.height,
resp.map.info.resolution, 0)
self.deltaOccupancyGridForShow.setScale(mapScale)
self.deltaOccupancyGridForShow.scaleEmptyMap()
self.deltaOccupancyGridDrawer = OccupancyGridDrawer('Mapper Node Delta Occupancy Grid',\
self.deltaOccupancyGridForShow, windowHeight)
self.deltaOccupancyGridDrawer.open()
# Flag set to true if graphics can be updated
self.visualisationUpdateRequired = False
# Set up the lock to ensure thread safety
self.dataCopyLock = Lock()
self.noOdometryReceived = True
self.noTwistReceived = True
self.noLaserScanReceived = True
self.enableMapping = True
# Register the supported services
self.changeMapperStateService = rospy.Service('change_mapper_state',
ChangeMapperState,
self.mappingStateService)
self.requestMapUpdateService = rospy.Service(
'request_map_update', RequestMapUpdate,
self.requestMapUpdateService)
# Set up the subscribers. These track the robot position, speed and laser scans.
self.mostRecentOdometry = Odometry()
self.odometrySubscriber = rospy.Subscriber("robot0/odom",
Odometry,
self.odometryCallback,
queue_size=1)
self.mostRecentTwist = Twist()
self.twistSubscriber = rospy.Subscriber('/robot0/cmd_vel',
Twist,
self.twistCallback,
queue_size=1)
self.laserSubscriber = rospy.Subscriber("robot0/laser_0",
LaserScan,
self.laserScanCallback,
queue_size=1)
def odometryCallback(self, msg):
self.dataCopyLock.acquire()
self.mostRecentOdometry = msg
self.noOdometryReceived = False
self.dataCopyLock.release()
def twistCallback(self, msg):
self.dataCopyLock.acquire()
self.mostRecentVelocity = msg
self.noTwistReceived = False
self.dataCopyLock.release()
def mappingStateService(self, changeMapperState):
self.enableMapping = changeMapperState.enableMapping
# rospy.loginfo('Changing the enableMapping state to %d', self.enableMapping)
return ChangeMapperStateResponse()
def requestMapUpdateService(self, request):
# rospy.loginfo('requestMapUpdateService with deltaOccupancyGridRequired %d', request.deltaOccupancyGridRequired)
mapUpdateMessage = self.constructMapUpdateMessage(
request.deltaOccupancyGridRequired)
return RequestMapUpdateResponse(mapUpdateMessage)
# Handle the laser scan callback. First process the scans and update the various maps
def laserScanCallback(self, msg):
# Can't process anything until stuff is enabled
if self.enableMapping is False:
return
# Can't process anything until we have the first scan
if (self.noOdometryReceived is True) or (self.noTwistReceived is True):
return
# Process the scan
gridHasChanged = self.parseScan(msg)
# If nothing has changed, return
if gridHasChanged is False:
return
# Mark that there is a pending update to the visualisation
self.visualisationUpdateRequired = True
# Mark that a laser scan has been received
self.noLaserScanReceived = False
# Construct the map update message and send it out
mapUpdateMessage = self.constructMapUpdateMessage(True)
# rospy.loginfo('publishing map update message')
self.mapUpdatePublisher.publish(mapUpdateMessage)
# Predict the pose of the robot to the current time. This is to
# hopefully make the pose of the robot a bit more accurate. The
# equation is: currentPose = lastPose + dT * lastTwist. Note this
# isn't quite right. e.g. a more proper implementation would store
# a history of velocities and interpolate over them.
def predictPose(self, predictTime):
# Copy the last odometry and velocity
self.dataCopyLock.acquire()
currentPose = copy.deepcopy(self.mostRecentOdometry.pose.pose)
currentPoseTime = self.mostRecentOdometry.header.stamp.to_sec()
currentTwist = copy.deepcopy(self.mostRecentTwist)
self.dataCopyLock.release()
dT = 0 #predictTime - currentPoseTime
quaternion = (currentPose.orientation.x, currentPose.orientation.y,
currentPose.orientation.z, currentPose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
theta = euler[2]
# These are the "ideal motion model" prediction equations from
# stdr which attempt to accurately describe the trajectory of
# the robot if it turns as it moves. The equations are precise
# if the angular and linear velocity is constant over the
# prediction interval.
if (abs(currentTwist.angular.z) < 1e-6):
x = currentPose.position.x + dT * currentTwist.linear.x * math.cos(
theta)
y = currentPose.position.y + dT * currentTwist.linear.x * math.sin(
theta)
else:
x = currentPose.position.x - currentTwist.linear.x / currentTwist.angular.z * sin(theta) + \
currentTwist.linear.x / currentTwist.angular.z * sin(theta + dT * currentTwist.angular.z)
y = currentPose.position.y - currentTwist.linear.x / currentTwist.angular.z * cos(theta) + \
currentTwist.linear.x / currentTwist.angular.z * cos(theta + dT * currentTwist.angular.z)
theta = theta + currentTwist.angular.z * dT
return x, y, theta
def parseScan(self, msg):
# Predict the robot pose to the time the scan was taken
x, y, theta = self.predictPose(msg.header.stamp.to_sec())
# Clear the flag which shows that the map has changed
gridHasChanged = False
# Clear the delta map, to imply that no changes have been detected
self.deltaOccupancyGrid.clearMap(0)
# For each ray, check the range is good. If so, check all the
# cells along the ray and mark cells as either open or
# blocked. To get around numerical issues, we trace along each
# ray in turn and terminate when we hit the first obstacle or at the end of the ray.
for ii in range(
int(
math.floor((msg.angle_max - msg.angle_min) /
msg.angle_increment))):
# rospy.loginfo("{} {} {}".format(msg.ranges[ii],msg.angle_min,msg.angle_max))
detectedRange = msg.ranges[ii]
# If the detection is below the minimum range, assume this ray is busted and continue
if (detectedRange <= msg.range_min):
continue
rayEndsOnObject = True
# If the detection range is beyond the end of the sensor,
# this is the mark which says that nothing was detected
if detectedRange >= msg.range_max:
rayEndsOnObject = False
detectedRange = msg.range_max
# Get the angle of this ray
angle = msg.angle_min + msg.angle_increment * ii + theta
# Get the list of cells which sit on this ray. The ray is
# scaled so that the last sell is at the detected range
# from the sensor.
between = self.ray_trace(detectedRange, x, y, angle, msg)
# If between is empty, something went wrong with the ray cast, so skip
if len(between) == 0:
continue
# Traverse along the ray and set cells. We can only change
# cells from unknown (0.5) to free. If we encounter a
# blocked cell, terminate. Sometimes the ray can slightly
# extend through a blocked cell due to numerical rounding
# issues.
traversedToEnd = True
for point in between:
try:
if self.occupancyGrid.getCell(point[0], point[1]) > 0.5:
traversedToEnd = False
break
if self.occupancyGrid.getCell(point[0], point[1]) == 0.5:
self.occupancyGrid.setCell(point[0], point[1], 0)
self.deltaOccupancyGrid.setCell(
point[0], point[1], 1.0)
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridForShow.setCell(
point[0], point[1], 1.0)
gridHasChanged = True
except IndexError as e:
print(e)
print "between: " + str(point[0]) + ", " + str(point[1])
print "extent: " + str(self.occupancyGrid.getExtent())
# If we got to the end okay, see if we have to mark the
# state of the end cell to occupied or not. To do this, we
# Note that we can change a cell
# from unknown and free to occupied, but we cannot change
# the state from occupied back to anything else. This gets
# around the issue that there can be "blinking" between
# whether a cell is occupied or not.
if (traversedToEnd is True) & (rayEndsOnObject is True):
lastPoint = between[-1]
if self.occupancyGrid.getCell(lastPoint[0],
lastPoint[1]) < 1.0:
self.occupancyGrid.setCell(lastPoint[0], lastPoint[1], 1)
self.deltaOccupancyGrid.setCell(lastPoint[0], lastPoint[1],
1.0)
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridForShow.setCell(
lastPoint[0], lastPoint[1], 1.0)
gridHasChanged = True
return gridHasChanged
def ray_trace(self, dist, x, y, angle, scanmsg):
"""
Function to get a list of points between two points
:param origin: position of the origin in world coordinates
:param dist: distance to end point
:param angle: angle from robot
:param scanmsg: Laser Scan message
:return: list of points in between the origin and end point
"""
startPoint = self.occupancyGrid.getCellCoordinatesFromWorldCoordinates([math.cos(angle) * scanmsg.range_min + x, \
math.sin(angle) * scanmsg.range_min + y])
endPoint = self.occupancyGrid.getCellCoordinatesFromWorldCoordinates([math.cos(angle) * dist + x, \
math.sin(angle) * dist + y])
points = bresenham(endPoint, startPoint)
return points.path
def updateVisualisation(self):
# If anything has changed the state of the occupancy grids,
# visualisationUpdateRequired is set to true. Therefore, the
# graphics are updated. If set to false, we flush anyway to
# make sure that the windows are properly (re)drawn in VNC.
if self.visualisationUpdateRequired is True:
if self.showOccupancyGrid is True:
self.occupancyGridDrawer.update()
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridDrawer.update()
self.deltaOccupancyGridForShow.clearMap(0)
self.visualisationUpdateRequired = False
else:
if self.showOccupancyGrid is True:
self.occupancyGridDrawer.flushAndUpdateWindow()
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridDrawer.flushAndUpdateWindow()
def constructMapUpdateMessage(self, deltaMapRequired):
# Construct the map update message
mapUpdateMessage = MapUpdate()
# rospy.loginfo('constructMapUpdateMessage: invoked')
mapUpdateMessage.header.stamp = rospy.Time().now()
mapUpdateMessage.isPriorMap = self.noLaserScanReceived
mapUpdateMessage.scale = self.occupancyGrid.getScale()
mapUpdateMessage.resolution = self.occupancyGrid.getResolution()
mapUpdateMessage.extentInCells = self.occupancyGrid.getExtentInCells()
mapUpdateMessage.occupancyGrid = self.occupancyGrid.getGridAsVector()
if deltaMapRequired is True:
mapUpdateMessage.deltaOccupancyGrid = self.deltaOccupancyGrid.getGridAsVector(
)
return mapUpdateMessage
def entropyGrid(self):
# entropies = []
totalEntropy = 0
# go through all of the occupancy grid cells
for y in range(0, self.occupancyGrid.getHeightInCells()):
# entropyRow = []
entropyRow = 0
for x in range(0, self.occupancyGrid.getWidthInCells()):
entropyElement = self.occupancyGrid.getCell(x, y)
cell_ent = 0
# in the case where the cell is unknown
# (0.5 * ln(0.5)) * (-2) ~ 0.69315
if entropyElement == 0.5:
cell_ent = 0.69315
# entropyRow.append(cell_ent)
entropyRow += cell_ent
# entropies.append(entropyRow)
totalEntropy += entropyRow
# self.entropyFile.write(str(entropyRow) + "\n")
# self.entropyFile.write("\n\n")
self.entropyFile.write(str(totalEntropy) + "\n")
# return entropies
return totalEntropy
def run(self):
slept = 0
while not rospy.is_shutdown():
self.updateVisualisation()
rospy.sleep(0.1)
slept += 0.1
if slept >= 5:
entropy_grid = self.entropyGrid()
slept = 0
self.entropyFile.close()
class PlannerControllerNode(object):
def __init__(self):
rospy.init_node('planner_controller', anonymous=True)
self.waitForGoal = threading.Condition()
self.waitForDriveCompleted = threading.Condition()
self.goal = None
self.goalReached = False
def createOccupancyGridFromMapServer(self):
# If we are using the ground truth map, get it directly from stdr. Otherwise,
# retrieve it from the mapper node
if rospy.get_param('use_reactive_planner_controller', False) is False:
rospy.loginfo('Using the ground truth map from stdr')
else:
rospy.loginfo('Getting map size information from stdr')
# Get the map service
# rospy.loginfo('Waiting for static_map to become available.')
rospy.wait_for_service('static_map')
self.mapServer = rospy.ServiceProxy('static_map', GetMap)
# rospy.loginfo('Found static_map; requesting map data')
# Query the map status
response = self.mapServer()
map = response.map
# rospy.loginfo('Got map data')
# Allocate the occupancy grid and set the data from the array sent back by the map server
self.occupancyGrid = OccupancyGrid(map.info.width, map.info.height, map.info.resolution)
self.occupancyGrid.setScale(rospy.get_param('plan_scale', 5))
# Copy data over if passive
if rospy.get_param('use_reactive_planner_controller', False) is True:
self.occupancyGrid.scaleEmptyMap()
else:
self.occupancyGrid.setFromDataArrayFromMapServer(map.data)
def mapUpdateCallback(self, msg):
# rospy.loginfo("map update received")
self.plannerController.handleMapUpdateMessage(msg)
def createPlanner(self):
# self.planner = DijkstraPlanner('Dijkstra', self.occupancyGrid)
self.planner = AStarPlanner('A*', self.occupancyGrid)
self.planner.setPauseTime(0)
self.planner.windowHeightInPixels = rospy.get_param('maximum_window_height_in_pixels', 700)
removeGoalCellFromPathIfOccupied = rospy.get_param('remove_goal_cell_from_path_if_occupied', False)
self.planner.setRemoveGoalCellFromPathIfOccupied(removeGoalCellFromPathIfOccupied)
def createRobotController(self):
self.robotController = Move2GoalController(self.occupancyGrid)
def createPlannerController(self):
if rospy.get_param('use_reactive_planner_controller', False) is True:
print("USING REACTIVE PLANNER -----------")
self.plannerController = ReactivePlannerController(self.occupancyGrid, self.planner, self.robotController)
else:
print("USING PASSIVE PLANNER -----------")
self.plannerController = PassivePlannerController(self.occupancyGrid, self.planner, self.robotController)
def handleDriveToGoal(self, goal):
# Report to the main loop that we have a new goal
self.waitForGoal.acquire()
self.goal = goal
self.waitForGoal.notify()
self.waitForGoal.release()
# Wait until the robot has finished driving
self.waitForDriveCompleted.acquire()
self.waitForDriveCompleted.wait()
self.waitForDriveCompleted.release()
return GoalResponse(self.goalReached)
def run(self):
# First set up the occupancy grid
self.createOccupancyGridFromMapServer()
# Create the planner
self.createPlanner()
# Set up the robot controller
self.createRobotController()
# Set up the planner controller, which puts the two together
self.createPlannerController()
# Set up the wait for the service. Note that we can't directly
# handle all the driving operations in the service
# handler. The reason is that the planner can create a GUI,
# and self MUST run in the main thread. The result is pretty
# ugly logic and can lead to deadlocking.
service = rospy.Service('drive_to_goal', Goal, self.handleDriveToGoal)
print 'Spinning to service goal requests'
while not rospy.is_shutdown():
# Wait for a new goal. Allow at most 0.1s, which gives
# time to check if we are shutting down
self.waitForGoal.acquire()
self.waitForGoal.wait(0.1)
self.waitForGoal.release()
# If no goal has been allocated, cycle around
if (self.goal is None):
continue
self.goalReached = self.plannerController.driveToGoal(self.goal)
self.goal = None
# Signal back to the service handler that we are done
self.waitForDriveCompleted.acquire()
self.waitForDriveCompleted.notify()
self.waitForDriveCompleted.release()
class MapperNode(object):
def __init__(self):
rospy.init_node('mapper_node', anonymous=True)
# Check if we use the ground truth map from stdr; if this is enabled, the mapper will ignore
# the laser scans
self.useGroundTruthMap = True
# Set up the lock to ensure thread safety
self.dataCopyLock = Lock()
self.noOdometryReceived = True
self.noTwistReceived = True
self.noLaserScanReceived = True
self.enableMapping = True
# List of active obstacles
self.activatedObstacles = []
self.disactivatedObstacles = []
self.activeObstacles = []
# Get the ground truth map from stdr; we use this to figure out the dimensions of the map
rospy.loginfo('waiting for the static_map service')
rospy.wait_for_service('static_map')
self.mapServer = rospy.ServiceProxy('static_map', GetMap)
resp = self.mapServer()
# Drawing options
self.showOccupancyGrid = rospy.get_param('show_mapper_occupancy_grid',
True)
self.showDeltaOccupancyGrid = rospy.get_param(
'show_mapper_delta_occupancy_grid', True)
# Flag set to true if graphics can be updated
self.visualisationUpdateRequired = False
# Create the publisher for the regular map messages
self.mapUpdatePublisher = rospy.Publisher('updated_map',
MapUpdate,
queue_size=1)
# Get the map scale
mapScale = rospy.get_param('plan_scale', 5)
# Create the occupancy grid
self.occupancyGrid = OccupancyGrid(resp.map.info.width,
resp.map.info.height,
resp.map.info.resolution, 0.5)
self.occupancyGrid.setScale(mapScale)
self.occupancyGrid.scaleEmptyMap()
# Create the delta occupancy grid map. This stores the difference since the last time the map was sent out
self.deltaOccupancyGrid = OccupancyGrid(resp.map.info.width,
resp.map.info.height,
resp.map.info.resolution, 0)
self.deltaOccupancyGrid.setScale(mapScale)
self.deltaOccupancyGrid.scaleEmptyMap()
# If we are using ground truth, create a ground truth occupancy map (used for reference) and set its value to the
# ground truth map.
if self.useGroundTruthMap is True:
self.groundTruthOccupancyGrid = OccupancyGrid(
resp.map.info.width, resp.map.info.height,
resp.map.info.resolution, 0.5)
self.groundTruthOccupancyGrid.setScale(mapScale)
self.groundTruthOccupancyGrid.setFromDataArrayFromMapServer(
resp.map.data)
self.updateOccupancyGridFromGroundTruthAndObstacles()
self.visualisationUpdateRequired = True
self.noLaserScanReceived = False
# Debug code
if False:
uniqueCellValues1 = []
for x in range(self.occupancyGrid.widthInCells):
for y in range(self.occupancyGrid.heightInCells):
if self.groundTruthOccupancyGrid.grid[x][
y] not in uniqueCellValues1:
uniqueCellValues1.append(self.occupancyGrid.grid[x][y])
uniqueCellValues = []
print str(len(resp.map.data))
for x in range(len(resp.map.data)):
g = resp.map.data[x]
if g not in uniqueCellValues:
uniqueCellValues.append(g)
print '**************************************************************************************'
print str(uniqueCellValues)
print str(uniqueCellValues1)
print '**************************************************************************************'
windowHeight = rospy.get_param('maximum_window_height_in_pixels', 600)
if self.showOccupancyGrid is True:
self.occupancyGridDrawer = OccupancyGridDrawer('Mapper Node Occupancy Grid',\
self.occupancyGrid, windowHeight)
self.occupancyGridDrawer.open()
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridForShow = OccupancyGrid(
resp.map.info.width, resp.map.info.height,
resp.map.info.resolution, 0)
self.deltaOccupancyGridForShow.setScale(mapScale)
self.deltaOccupancyGridForShow.scaleEmptyMap()
self.deltaOccupancyGridDrawer = OccupancyGridDrawer('Mapper Node Delta Occupancy Grid',\
self.deltaOccupancyGridForShow, windowHeight)
self.deltaOccupancyGridDrawer.open()
# Set up the subscribers. These track the robot position, speed, threshold and laser scans.
self.mostRecentOdometry = Odometry()
self.odometrySubscriber = rospy.Subscriber("robot0/odom",
Odometry,
self.odometryCallback,
queue_size=1)
self.mostRecentTwist = Twist()
self.twistSubscriber = rospy.Subscriber('/robot0/cmd_vel',
Twist,
self.twistCallback,
queue_size=1)
self.laserSubscriber = rospy.Subscriber("robot0/laser_0",
LaserScan,
self.laserScanCallback,
queue_size=1)
self.addObstacleToMapSubscriber = \
rospy.Subscriber("/add_obstacle_to_map", Int32, self.addObstacleToMapCallback, queue_size=1)
self.removeObstacleFromMapSubscriber = \
rospy.Subscriber("/remove_obstacle_from_map", Int32, self.removeObstacleFromMapCallback, queue_size=1)
# Register the supported services
self.changeMapperStateService = rospy.Service('change_mapper_state',
ChangeMapperState,
self.mappingStateService)
self.requestMapUpdateService = rospy.Service(
'request_map_update', RequestMapUpdate,
self.requestMapUpdateService)
def odometryCallback(self, msg):
self.dataCopyLock.acquire()
self.mostRecentOdometry = msg
self.noOdometryReceived = False
self.dataCopyLock.release()
def twistCallback(self, msg):
self.dataCopyLock.acquire()
self.mostRecentVelocity = msg
self.noTwistReceived = False
self.dataCopyLock.release()
def mappingStateService(self, changeMapperState):
self.enableMapping = changeMapperState.enableMapping
# rospy.loginfo('Changing the enableMapping state to %d', self.enableMapping)
return ChangeMapperStateResponse()
def requestMapUpdateService(self, request):
rospy.loginfo(
'requestMapUpdateService with deltaOccupancyGridRequired %d',
request.deltaOccupancyGridRequired)
mapUpdateMessage = self.constructMapUpdateMessage(
request.deltaOccupancyGridRequired)
return RequestMapUpdateResponse(mapUpdateMessage)
# Handle the laser scan callback. First process the scans and update the various maps
def laserScanCallback(self, msg):
if self.useGroundTruthMap is True:
return
# Can't process anything until stuff is enabled
if self.enableMapping is False:
return
# Can't process anything until we have the first scan
if (self.noOdometryReceived is True) or (self.noTwistReceived is True):
return
# Process the scan
gridHasChanged = self.parseScan(msg)
# If nothing has changed, return
if gridHasChanged is False:
return
# Mark that there is a pending update to the visualisation
self.visualisationUpdateRequired = True
# Mark that a laser scan has been received
self.noLaserScanReceived = False
# Construct the map update message and send it out
mapUpdateMessage = self.constructMapUpdateMessage(True)
rospy.loginfo('publishing map update message')
self.mapUpdatePublisher.publish(mapUpdateMessage)
def predictPose(self, predictTime):
self.dataCopyLock.acquire()
currentPose = copy.deepcopy(self.mostRecentOdometry.pose.pose)
currentPoseTime = self.mostRecentOdometry.header.stamp.to_sec()
currentTwist = copy.deepcopy(self.mostRecentTwist)
self.dataCopyLock.release()
dT = 0 #predictTime - currentPoseTime
quaternion = (currentPose.orientation.x, currentPose.orientation.y,
currentPose.orientation.z, currentPose.orientation.w)
euler = tf.transformations.euler_from_quaternion(quaternion)
theta = euler[2]
# These are the "ideal motion model" prediction equations from
# stdr which attempt to accurately describe the trajectory of
# the robot if it turns as it moves. The equations are precise
# if the angular and linear velocity is constant over the
# prediction interval.
if (abs(currentTwist.angular.z) < 1e-6):
x = currentPose.position.x + dT * currentTwist.linear.x * math.cos(
theta)
y = currentPose.position.y + dT * currentTwist.linear.x * math.sin(
theta)
else:
x = currentPose.position.x - currentTwist.linear.x / currentTwist.angular.z * sin(theta) + \
currentTwist.linear.x / currentTwist.angular.z * sin(theta + dT * currentTwist.angular.z)
y = currentPose.position.y - currentTwist.linear.x / currentTwist.angular.z * cos(theta) + \
currentTwist.linear.x / currentTwist.angular.z * cos(theta + dT * currentTwist.angular.z)
theta = theta + currentTwist.angular.z * dT
return x, y, theta
# This method notes the obstacle ID with the groundtruth list.
def addObstacleToMapCallback(self, obstacleMsg):
id = obstacleMsg.data
rospy.loginfo(
'addObstacleToMapCallback: adding obstacle {}'.format(id))
if id not in self.activeObstacles:
self.activeObstacles.append(id)
rospy.loginfo(
'addObstacleToGroundtruthListCallback: activeObstacles = {}'.
format(self.activeObstacles))
self.updateOccupancyGridFromGroundTruthAndObstacles()
self.visualisationUpdateRequired = True
def removeObstacleFromMapCallback(self, obstacleMsg):
id = obstacleMsg.data
rospy.loginfo(
'removeObstacleFromGroundtruthListCallback: removing obstacle {}'.
format(id))
if id in self.activeObstacles:
self.activeObstacles.remove(id)
rospy.loginfo(
'removeObstacleFromGroundtruthListCallback: activateObstacles = {}'
.format(self.activeObstacles))
self.updateOccupancyGridFromGroundTruthAndObstacles()
self.visualisationUpdateRequired = True
def parseScan(self, msg):
# Predict the robot pose to the time the scan was taken
x, y, theta = self.predictPose(msg.header.stamp.to_sec())
# Clear the flag which shows that the map has changed
gridHasChanged = False
# Clear the delta map, to imply that no changes have been detected
self.deltaOccupancyGrid.clearMap(0)
# For each ray, check the range is good. If so, check all the
# cells along the ray and mark cells as either open or
# blocked. To get around numerical issues, we trace along each
# ray in turn and terminate when we hit the first obstacle or at the end of the ray.
for ii in range(
int(
math.floor((msg.angle_max - msg.angle_min) /
msg.angle_increment))):
# rospy.loginfo("{} {} {}".format(msg.ranges[ii],msg.angle_min,msg.angle_max))
detectedRange = msg.ranges[ii]
# If the detection is below the minimum range, assume this ray is busted and continue
if (detectedRange <= msg.range_min):
continue
rayEndsOnObject = True
# If the detection range is beyond the end of the sensor,
# this is the mark which says that nothing was detected
if detectedRange >= msg.range_max:
rayEndsOnObject = False
detectedRange = msg.range_max
# Get the angle of this ray
angle = msg.angle_min + msg.angle_increment * ii + theta
# Get the list of cells which sit on this ray. The ray is
# scaled so that the last sell is at the detected range
# from the sensor.
between = self.ray_trace(detectedRange, x, y, angle, msg)
# If between is empty, something went wrong with the ray cast, so skip
if len(between) == 0:
continue
# Traverse along the ray and set cells. We can only change
# cells from unknown (0.5) to free. If we encounter a
# blocked cell, terminate. Sometimes the ray can slightly
# extend through a blocked cell due to numerical rounding
# issues.
traversedToEnd = True
for point in between:
try:
# if self.occupancyGrid.getCell(point[0], point[1]) == 0.5:
self.occupancyGrid.setCell(point[0], point[1], 0)
self.deltaOccupancyGrid.setCell(point[0], point[1], 1.0)
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridForShow.setCell(
point[0], point[1], 1.0)
gridHasChanged = True
except IndexError as e:
print(e)
print "between: " + str(point[0]) + ", " + str(point[1])
print "extent: " + str(self.occupancyGrid.getExtent())
# If we got to the end okay, see if we have to mark the
# state of the end cell to occupied or not. To do this, we
# Note that we can change a cell
# from unknown and free to occupied, but we cannot change
# the state from occupied back to anything else. This gets
# around the issue that there can be "blinking" between
# whether a cell is occupied or not.
if (traversedToEnd is True) & (rayEndsOnObject is True):
lastPoint = between[-1]
if self.occupancyGrid.getCell(lastPoint[0],
lastPoint[1]) < 1.0:
self.occupancyGrid.setCell(lastPoint[0], lastPoint[1], 1)
self.deltaOccupancyGrid.setCell(lastPoint[0], lastPoint[1],
1.0)
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridForShow.setCell(
lastPoint[0], lastPoint[1], 1.0)
gridHasChanged = True
return gridHasChanged
def ray_trace(self, dist, x, y, angle, scanmsg):
"""
Function to get a list of points between two points
:param origin: position of the origin in world coordinates
:param dist: distance to end point
:param angle: angle from robot
:param scanmsg: Laser Scan message
:return: list of points in between the origin and end point
"""
startPoint = self.occupancyGrid.getCellCoordinatesFromWorldCoordinates([math.cos(angle) * scanmsg.range_min + x, \
math.sin(angle) * scanmsg.range_min + y])
endPoint = self.occupancyGrid.getCellCoordinatesFromWorldCoordinates([math.cos(angle) * dist + x, \
math.sin(angle) * dist + y])
points = bresenham(endPoint, startPoint)
return points.path
def updateOccupancyGridFromGroundTruthAndObstacles(self):
# Loop through the map and change the occupancy grid so that only
# visible objects are shown. Note that we turn the cell occupancy back
# into an integer to avoid rounding issues in comparisons.
self.occupancyGrid.grid = copy.deepcopy(
self.groundTruthOccupancyGrid.grid)
for x in range(self.occupancyGrid.widthInCells):
for y in range(self.occupancyGrid.heightInCells):
cell = int(self.occupancyGrid.getCell(x, y) * 100)
if cell > 0 and cell < 100:
if cell in self.activeObstacles:
self.occupancyGrid.setCell(x, y, 1)
else:
self.occupancyGrid.setCell(x, y, 0)
mapUpdateMessage = self.constructMapUpdateMessage(True)
rospy.loginfo('publishing map update message')
self.mapUpdatePublisher.publish(mapUpdateMessage)
def updateVisualisation(self):
# If anything has changed the state of the occupancy grids,
# visualisationUpdateRequired is set to true. Therefore, the
# graphics are updated. If set to false, we flush anyway to
# make sure that the windows are properly (re)drawn in VNC.
if self.visualisationUpdateRequired is True:
if self.showOccupancyGrid is True:
self.occupancyGridDrawer.update()
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridDrawer.update()
self.deltaOccupancyGridForShow.clearMap(0)
self.visualisationUpdateRequired = False
else:
if self.showOccupancyGrid is True:
self.occupancyGridDrawer.flushAndUpdateWindow()
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridDrawer.flushAndUpdateWindow()
def constructMapUpdateMessage(self, deltaMapRequired):
# Construct the map update message
mapUpdateMessage = MapUpdate()
#rospy.loginfo('constructMapUpdateMessage: invoked')
mapUpdateMessage.header.stamp = rospy.Time().now()
mapUpdateMessage.isPriorMap = self.noLaserScanReceived
mapUpdateMessage.scale = self.occupancyGrid.getScale()
mapUpdateMessage.resolution = self.occupancyGrid.getResolution()
mapUpdateMessage.extentInCells = self.occupancyGrid.getExtentInCells()
mapUpdateMessage.occupancyGrid = self.occupancyGrid.getGridAsVector()
if deltaMapRequired is True:
mapUpdateMessage.deltaOccupancyGrid = self.deltaOccupancyGrid.getGridAsVector(
)
return mapUpdateMessage
def run(self):
while not rospy.is_shutdown():
self.updateVisualisation()
rospy.sleep(0.1)