def getCleanPoseList():
plan = []
try:
plan = getCleanPathPlanner()
return plan.poses
except AttributeError, e:
logMsg = "get plan failed. plan:%s" % (plan)
logWrite('warning', moudleName, logMsg)
return []
def getNewGoal(plan, theNum=30):
tuplePoses = plan.poses
logMsg = "getNewGoal. len(tuplePoses):%s" % (len(tuplePoses))
logWrite('debug', share.moudleName, logMsg)
if len(tuplePoses) > theNum:
return tuplePoses[theNum]
else:
return tuplePoses[len(tuplePoses) - 1]
def pubCleanPlanner(poseList):
pubCleanPlannerOnce(poseList, share.cleanPlannerPub)
if len(poseList) == 0:
logMsg = "poseList empty."
logWrite('warning', moudleName, logMsg)
return False
logMsg = "pubCleanPlannerOnce. len(poseList):%s" % (len(poseList))
logWrite('warning', moudleName, logMsg)
pubCleanPlannerOnce(poseList, share.cleanPlannerPub)
share.cleanPostList = share.cleanPostList + poseList
pubCleanPlannerOnce(share.cleanPostList, share.cleanPlannerAllPub)
def cleanMoveTo(control, fromX, fromY, goalX, goalY, linearVelocity=-1):
##for obstacle_avoid check
# obstacle_avoid is very importent while moving
needRetryWaitingMessage = 0
try:
logMsg = "Waiting for %s topic..." % conf.TOPIC_OBSTACLE_AVOID_SCAN
logWrite('debug', moudleName, logMsg)
rospy.wait_for_message(conf.TOPIC_OBSTACLE_AVOID_SCAN, LaserScan, 1.0)
except rospy.exceptions.ROSException, e:
logMsg = "Waiting for %s topic exceed time. so rotate 90 degrees want to change env , then may be can got topic messages."
logWrite('debug', moudleName, logMsg)
control.rotateAround90Right()
needRetryWaitingMessage = 1
def setGridValue(mapData, valueList):
mapResolution = mapData.info.resolution
mapOrigin = (mapData.info.origin.position.x,
mapData.info.origin.position.y)
mapWidth = mapData.info.width
for pointX, pointY in valueList:
cellTmp = (pointX, pointY)
valueTmp = valueList[cellTmp]
indx = getCellIndex(cellTmp, mapResolution, mapOrigin, mapWidth)
if indx < 0:
logMsg = "setGridValue error with point(%s,%s). because is out of map." % (
pointX, pointY)
logWrite('warning', share.moudleName, logMsg)
continue
mapData.data[indx] = valueTmp
return mapData
def gotoGoalSplit(control, finalGoalX, finalGoalY):
#Wait for the availability of this service
rospy.wait_for_service(conf.SERVICE_MAKE_PLAN)
#Get a proxy to execute the service
make_plan = rospy.ServiceProxy(conf.SERVICE_MAKE_PLAN, GetPlan)
distance = math.sqrt((finalGoalX - share.currentX)**2 +
(finalGoalY - share.currentY)**2)
thresholdDistance = 0.5
logMsg = "gotoGoalSplit. start."
logWrite('debug', share.moudleName, logMsg)
while distance > thresholdDistance:
start = createPoseWithRPY(share.currentX, share.currentY, 0.0, 0, 0, 0,
conf.FRAME_ID_MAP)
goal = createPoseWithRPY(finalGoalX, finalGoalY, 0.0, 0, 0, 0,
conf.FRAME_ID_MAP)
tolerance = 0.5 #meter
planResponse = make_plan(start=start, goal=goal, tolerance=tolerance)
if len(planResponse.plan.poses) <= 0:
return False
newTmpGoal = getNewGoal(planResponse.plan)
logMsg = "gotoGoalSplit. newTmpGoal(%s,%s)" % (
newTmpGoal.pose.position.x, newTmpGoal.pose.position.y)
logWrite('debug', share.moudleName, logMsg)
share.localGoalX = newTmpGoal.pose.position.x
share.localGoalY = newTmpGoal.pose.position.y
if cleanMoveToWithMoveBase(control, share.currentX, share.currentY,
share.localGoalX, share.localGoalY):
distance = math.sqrt((finalGoalX - share.currentX)**2 +
(finalGoalY - share.currentY)**2)
logMsg = "gotoGoalSplit. reached tmp goal."
logWrite('debug', share.moudleName, logMsg)
else:
logMsg = "gotoGoalSplit. can not reached Goal."
logWrite('debug', share.moudleName, logMsg)
return False
break #can not reached Goal
distance = math.sqrt((finalGoalX - share.currentX)**2 +
(finalGoalY - share.currentY)**2)
if distance <= thresholdDistance:
return True
else:
return False
def cleanAction(control):
poseList = getCleanPoseList()
pubCleanPlanner(poseList)
share.reachedEndOfPlan = False
counter = 0
loopStart("for_cleanAction")
for point in poseList:
if count == 0:
continue
if share.stoped:
break
share.localGoalX = point.pose.position.x
share.localGoalY = point.pose.position.y
share.wasLocalGoalDefined = True
#control.goToPositionInAStraightLine(conf.LINEAR_VELOCITY, share.localGoalX, share.localGoalY)
cleanMoveTo(control, share.currentX, share.currentY, share.localGoalX,
share.localGoalY)
if share.bumperEncountered:
logMsg = "share.bumperEncountered encountered in cleanAction."
logWrite('warning', moudleName, logMsg)
# return True
continue
if share.obstacleEncountered:
logMsg = "share.obstacleEncountered encountered in cleanAction."
logWrite('warning', moudleName, logMsg)
# return True
continue
if len(poseList) >= 1 and counter >= (len(poseList) - 1):
logMsg = "reach the end of poseList in cleanAction."
logWrite('warning', moudleName, logMsg)
share.reachedEndOfPlan = True
counter = counter + 1
loopEnd("for_cleanAction")
if len(poseList) == 0:
share.reachedEndOfPlan = True
share.wasLocalGoalDefined = False
return True
def testPointOnMap(x, y, mapData, currentAreaMapPub):
cleanBeginPoint = createPointStamped(Point(x, y, 0), "map")
share.currnetAreaCleanBeginPointpub.publish(cleanBeginPoint)
mapInput = copy.deepcopy(share.mapData)
mapResolution = mapInput.info.resolution
mapOrigin = (mapInput.info.origin.position.x,
mapInput.info.origin.position.y)
mapWidth = mapInput.info.width
mapHeight = mapInput.info.height
currentAreaIndexList = {}
indx = getCellIndex((x, y), mapResolution, mapOrigin, mapWidth)
if indx > 0:
currentAreaIndexList[indx] = True
valueTmp = mapInput.data[indx]
else:
valueTmp = -1
valueNotInCurrentArea = 100
data = []
for i in range(len(mapInput.data)):
if i in currentAreaIndexList:
data.append(valueNotInCurrentArea)
logWrite('debug', share.moudleName, "i:%s = 100." % (i))
else:
data.append(mapInput.data[i])
mapInput.data = data
rate = rospy.Rate(conf.POS_REQUEST_RATE)
for i in range(0, 3):
if not (mapInput is None):
try:
currentAreaMapPub.publish(mapInput)
except rospy.ROSException as e:
logWrite('debug', share.moudleName,
"currentAreaMapPub.publish.publish fail. %s" % (e))
rate.sleep()
continue
logMsg = "testPoint:(%s,%s) index;%s valueTmp:%s " % (1.85, -0.9, indx,
valueTmp)
logWrite('debug', share.moudleName, logMsg)
def getBorderCellList(topLeft, bottomLeft, bottomRight, topRight,
mapResolution, mapOrigin):
(mapOriginX, mapOriginY) = mapOrigin
###########################
# (-2,2) --------------- (2,2)
# | |
# | |
# | |
# | |
# | |
# (-2,-2) --------------- (2,-2)
###########################
borderList = []
###########################
# (-2,2) ---------------
# (-2,1.5) | |
# (-2,1) | |
# (-2,0.5) | |
# ... | |
# (-2,-2) --------------
###########################
#left border
startX = topLeft.point.x
startY = topLeft.point.y
endX = bottomLeft.point.x
endY = bottomLeft.point.y
for y in np.arange(endY, startY, mapResolution):
borderList.append((startX, y))
###########################
# ---------------
# | |
# | |
# | |
# | |
# --------------
# (-2,-2) , (-1.5,-2), (-1,-2), (-0.5 -2) (0,-2) ... (2,-2)
###########################
#bottorm border
startX = bottomLeft.point.x
startY = bottomLeft.point.y
endX = bottomRight.point.x
endY = bottomRight.point.y
for x in np.arange(startX, endX, mapResolution):
borderList.append((x, startY))
###########################
# -------------- (2,2)
# | | (2,1.5)
# | | (2,1)
# | | (2,0.5)
# | | ...
# -------------- (2,-2)
###########################
#right border
startX = bottomRight.point.x
startY = bottomRight.point.y
endX = topRight.point.x
endY = topRight.point.y
for y in np.arange(startY, endY, mapResolution):
logMsg = "==== 11 startX:%s y:%s new:(%s,%s)" % (startX, y, startX -
mapOriginX, y)
logWrite('debug', share.moudleName, logMsg)
borderList.append((startX, y))
###########################
# (-2,2) , (-1.5,2), (-1,2), (-0.5,2) (0,2) ... (2,2)
# ---------------
# | |
# | |
# | |
# | |
# --------------
###########################
#top border
startX = topLeft.point.x
startY = topLeft.point.y
endX = topRight.point.x
endY = topRight.point.y
for x in np.arange(startX, endX, mapResolution):
borderList.append((x, startY))
logMsg = "==== 6. len(borderList):%s" % (len(borderList))
logWrite('debug', share.moudleName, logMsg)
return borderList
def setGridValueBatchWithSameValue(mapData, cellList, value, mapResolution,
mapOrigin, mapWidth):
logMsg = "==== 4 mapResolution:%s mapOriginX:%s mapOriginY:%s mapWidth:%s len(cellList):%s" % (
mapResolution, mapOrigin[0], mapOrigin[1], mapWidth, len(cellList))
logWrite('debug', share.moudleName, logMsg)
appendData = {}
for i in range(len(cellList)):
cellTmp = cellList[i]
#logMsg = "==== 14 cellList:%s,%s "%(cellList[i])
indx = getCellIndex(cellTmp, mapResolution, mapOrigin, mapWidth)
if indx < 0:
logMsg = "setGridValue error with point(%s,%s). because is out of map." % (
cellTmp[0], cellTmp[1])
logWrite('warning', share.moudleName, logMsg)
continue
#logMsg = "==== 14 cellTmp(%s,%s) indx:%s"%(cellTmp[0],cellTmp[1],indx)
#logWrite('debug',share.moudleName,logMsg)
appendData[indx] = True
#appendData[0] = True
#appendData[1] = True
#appendData[2] = True
#appendData[3] = True
#appendData[4] = True
logMsg = "==== 5 len(mapData.data):%s" % (len(mapData.data))
logWrite('debug', share.moudleName, logMsg)
logMsg = "==== y appendData:%s" % (len(appendData))
logWrite('debug', share.moudleName, logMsg)
data = []
for i in range(len(mapData.data)):
if i in appendData:
data.append(value)
#elif mapData.data[i] == -1:
# data.append(100) #unknown -> obstacle
else:
data.append(mapData.data[i])
logMsg = "==== 7 mapWidth:%s" % (mapWidth)
logWrite('debug', share.moudleName, logMsg)
mapData.data = data
logMsg = "==== 6 mapWidth:%s" % (mapWidth)
logWrite('debug', share.moudleName, logMsg)
return mapData
def getCleanPathPlanner():
if not share.receivedNewMap: # for mapData
logMsg = "share.receivedNewMap = False in getCleanPathPlanner()"
logWrite('warning', moudleName, logMsg)
return []
if not share.receivedInitPos: # for start param
logMsg = "share.receivedInitPos = False in getCleanPathPlanner()"
logWrite('warning', moudleName, logMsg)
return []
mapInput = mergeMapData(share.globalMapData, share.mapData)
#####################
## (nav_msgs/OccupancyGrid)
## mapDataPre = share.mapData
##mapInput = share.mapData
#mapInput = share.globalMapData
##mapDataPre = mapInput
#####################
### service MapModifire
##try:
## mapModifierResponse = share.mapModifier(mapModifierThreshold.holeThreshold, mapModifierThreshold.noiseThreshold, mapModifierThreshold.debug, mapDataPre)
## share.MapDataModified = mapData = mapModifierResponse.map
##except rospy.ServiceException, e:
## #mapData = share.mapData
## mapData = mapInput
## logMsg = "call mapModifier service fail. %s"%e
## logWrite('warning',moudleName,logMsg)
#####################
##############################
if share.topLeft == None:
share.topLeft = createPointStamped(Point(-2, 2, 0), "map")
if share.bottomLeft == None:
share.bottomLeft = createPointStamped(Point(-2, -2, 0), "map")
if share.bottomRight == None:
share.bottomRight = createPointStamped(Point(2, -2, 0), "map")
if share.topRight == None:
share.topRight = createPointStamped(Point(2, 2, 0), "map")
logMsg = "====3"
logWrite('debug', share.moudleName, logMsg)
mapResolution = share.mapData.info.resolution
mapOrigin = (share.mapData.info.origin.position.x,
share.mapData.info.origin.position.y)
mapWidth = mapInput.info.width
logMsg = "====6 mapResolution:%s mapOriginX:%s mapOriginY:%s mapWidth:%s" % (
mapResolution, mapOrigin[0], mapOrigin[1], mapWidth)
logWrite('debug', share.moudleName, logMsg)
mapData = updateMapWithborder(share.topLeft, share.bottomLeft,
share.bottomRight, share.topRight, mapInput,
mapResolution, mapOrigin, mapWidth)
logMsg = "====10 len(mapData.data):%s" % (len(mapData.data))
logWrite('debug', share.moudleName, logMsg)
share.MapDataModified = mapData
#############################
####################
##mapInput = share.mapData
#mapInput = share.globalMapData
#mapData = mapInput # to invalid mapModifier
#share.MapDataModified = mapData
# service cleanPlanner
pathPlanner.start = share.currentPose
pathPlanner.start.header.frame_id = mapData.header.frame_id
pathPlanner.goal = goal = pathPlanner.start
logMsg = "====4"
logWrite('debug', share.moudleName, logMsg)
plan = []
try:
cleanPlannerResponse = share.cleanPlanner(
pathPlanner.start, pathPlanner.goal, pathPlanner.erosionRadius,
pathPlanner.robotRadius, pathPlanner.occupancyThreshold, mapData)
# (nav_msgs/Path)
plan = cleanPlannerResponse.plan
except rospy.ServiceException, e:
#mapData = share.mapData
mapData = mapInput
logMsg = "call cleanPlanner service fail.%s" % e
logWrite('warning', moudleName, logMsg)
def pubCleanPlannerOnce(poseList, cleanPlannerPub):
logMsg = "pubCleanPlannerOnce. len(poseList):%s" % (len(poseList))
logWrite('warning', moudleName, logMsg)
markerArray = MarkerArray()
# marker start pose
##########################
pose = Pose()
pose.position.x = pathPlanner.start.pose.position.x
pose.position.y = pathPlanner.start.pose.position.y
pose.orientation.w = 1.0
scale = Vector3(0.2, 0.2, 0.2)
color = ColorRGBA(0, 1, 0, 1) # r,g,b,a
markerId = 0
markerStart = createMarker("plannerStart", Marker.SPHERE, pose, scale,
color, markerId)
markerArray.markers.append(markerStart)
##########################
#cleanPlanner = Marker()
#cleanPlanner.header.frame_id = "s_map"
#cleanPlanner.header.stamp = rospy.Time.now()
#cleanPlanner.ns = 'cleanPlanner'
#cleanPlanner.action = Marker.ADD
#cleanPlanner.pose.orientation.w = 1.0;
#cleanPlanner.id = 0
#cleanPlanner.type = Marker.LINE_STRIP
#cleanPlanner.scale.x = 0.02
#cleanPlanner.scale.y = 0.1
#cleanPlanner.scale.z = 0.1
# cleanPlanner.color.r = 1.0 #red
# cleanPlanner.color.g = 0.0 #green
# cleanPlanner.color.b = 0.0 #blue
#cleanPlanner.color.a = 1.0
lastPose = poseList[0]
count = 0
lastPoint = Point(lastPose.pose.position.x, lastPose.pose.position.y, 0)
loopStart("for_pubCleanPlanner")
for pose in poseList:
count = count + 1
# marker planner pose
##########################
scale = Vector3(0.05, 0.1, 0.1)
color = ColorRGBA(1, 0, 0, 1) # r,g,b,a
poseTmp = Pose()
poseTmp.position.x = lastPoint.x
poseTmp.position.y = lastPoint.y
# z must 0, if do not this, the line will can not see in map, because it
# may be above the map or below the map.
poseTmp.position.z = 0
poseTmp.orientation.w = 1.0
markerId = count
markerArrow = createMarker("plannerPath", Marker.ARROW, poseTmp, scale,
color, markerId)
#arrowHead, arrowEnd
##########
arrowHeadPoint = Point(0, 0, 0)
p = Point(pose.pose.position.x, pose.pose.position.y, 0)
arrowEndPoint = Point()
arrowEndPoint.x = p.x - lastPoint.x
arrowEndPoint.y = p.y - lastPoint.y
arrowEndPoint.z = 0
markerArrow.points.append(arrowHeadPoint)
markerArrow.points.append(arrowEndPoint)
##########
##########################
markerArray.markers.append(markerArrow)
lastPoint = p
loopEnd("for_pubCleanPlanner")
# marker goal pose
##########################
pose = Pose()
pose.position.x = pathPlanner.goal.pose.position.x
pose.position.y = pathPlanner.goal.pose.position.y
pose.orientation.w = 1.0
scale = Vector3(0.2, 0.2, 0.2)
color = ColorRGBA(0, 0, 1, 1) # r,g,b,a
markerId = 0
markerStart = createMarker("plannerStart", Marker.SPHERE, pose, scale,
color, markerId)
markerArray.markers.append(markerStart)
##########################
cleanPlannerPub.publish(markerArray)
except rospy.exceptions.ROSException, e:
logMsg = "Waiting for %s topic exceed time. so rotate 90 degrees want to change env , then may be can got topic messages."
logWrite('debug', moudleName, logMsg)
control.rotateAround90Right()
needRetryWaitingMessage = 1
if needRetryWaitingMessage == 1:
try:
rospy.loginfo("retry Waiting for %s topic...",
conf.TOPIC_OBSTACLE_AVOID_SCAN)
rospy.wait_for_message(conf.TOPIC_OBSTACLE_AVOID_SCAN, LaserScan,
1.0)
except rospy.exceptions.ROSException, e:
logMsg = "bad env without obstacle_avoid_scan. so do not move to new goal,and want to next clean goal."
logWrite('debug', moudleName, logMsg)
return False
if share.nextCleanPosePub:
nextPoint = createPointStamped(Point(goalX, goalY, 0),
conf.FRAME_ID_MAP)
share.nextCleanPosePub.publish(nextPoint)
distance = math.sqrt((goalX - fromX)**2 + (goalY - fromY)**2)
thresholdDistance = pathPlanner.robotRadius * 0.6
if distance < thresholdDistance:
logMsg = "distance is too near, so not move to there. distance:%s threshold:%s from:(%s,%s) goal:(%s,%s) " % (
distance, thresholdDistance, fromX, fromY, goalX, goalY)
logWrite('debug', moudleName, logMsg)
return True