diff --git a/scripts/assemble_local_grids.py b/scripts/assemble_local_grids.py index a025529d..eb166c0d 100755 --- a/scripts/assemble_local_grids.py +++ b/scripts/assemble_local_grids.py @@ -1,7 +1,7 @@ #!/usr/bin/env python -# Similar to map_assembler node, this minimal python example shows how -# to reconstruct the obstacle map by subscribing only to +# Similar to map_assembler node, this minimal python example shows how +# to reconstruct the obstacle map by subscribing only to # graph and latest data added to map (for constant network bandwidth usage). import rospy @@ -31,26 +31,25 @@ def callback(graph, cloud): nodeId = graph.posesId[-1] pose = graph.poses[-1] size = cloud.width - + posesDict[nodeId] = pose cloudsDict[nodeId] = cloud - - # Update pose of our buffered clouds. + + # Update pose of our buffered clouds. # Check also if the clouds have moved because of a loop closure. If so, we have to update the rendering. maxDiff = 0 - updateWholeMap = False for i in range(0,len(graph.posesId)): if graph.posesId[i] in posesDict: currentPose = posesDict[graph.posesId[i]].position newPose = graph.poses[i].position diff = max([abs(currentPose.x-newPose.x), abs(currentPose.y-newPose.y), abs(currentPose.z-newPose.z)]) - if maxDiff < diff: + if maxDiff < diff: maxDiff = diff else: rospy.loginfo("Old node %d not found in cache, creating an empty cloud.", graph.posesId[i]) posesDict[graph.posesId[i]] = graph.poses[i] cloudsDict[graph.posesId[i]] = PointCloud2() - + # If we don't move, some nodes would be removed from the graph, so remove them from our buffered clouds. newGraph = Set(graph.posesId) totalPoints = 0 @@ -60,8 +59,8 @@ def callback(graph, cloud): cloudsDict.pop(p) else: totalPoints = totalPoints + cloudsDict[p].width - - if maxDiff > 0.1: + + if maxDiff > 0.1: # if any node moved more than 10 cm, request an update of the assembled map so far newAssembledCloud = PointCloud2() rospy.loginfo("Map has been optimized! maxDiff=%.3fm, re-updating the whole map...", maxDiff) @@ -92,11 +91,15 @@ def callback(graph, cloud): assembledCloud.data = assembledCloud.data + transformedCloud.data assembledCloud.width = assembledCloud.width + transformedCloud.width assembledCloud.row_step = assembledCloud.row_step + transformedCloud.row_step - + updateTime = rospy.get_time() - begin - - rospy.loginfo("Received node %d (%d pts) at xyz=%.2f %.2f %.2f, q_xyzw=%.2f %.2f %.2f %.2f (Map: Nodes=%d Points=%d Assembled=%d Update=%.0fms)", nodeId, size, pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w, len(cloudsDict), totalPoints, assembledCloud.width, updateTime*1000) - + + rospy.loginfo("Received node %d (%d pts) at xyz=%.2f %.2f %.2f, q_xyzw=%.2f %.2f %.2f %.2f (Map: Nodes=%d Points=%d Assembled=%d Update=%.0fms)", + nodeId, size, + pose.position.x, pose.position.y, pose.position.z, + pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w, + len(cloudsDict), totalPoints, assembledCloud.width, updateTime*1000) + assembledCloud.header = graph.header pub.publish(assembledCloud)