added missing test_costmap to generate test costmaps after point clicking in rviz

rosbuild/scout_navigation/test_costmap.py

+#!/usr/bin/env python
+
+import sys
+
+import numpy as np
+
+import rospy
+import yaml
+
+from geometry_msgs.msg import *
+from nav_msgs.msg import *
+
+RADIUS = 2 # (m)
+
+def handler(msg):
+    print "clicked on", msg.point
+    size = 2.0*RADIUS/cmap_scale
+    x0 = msg.point.x - RADIUS
+    y0 = msg.point.y - RADIUS
+    x = np.linspace(-3, 3, num=size)
+    z = 100 * np.exp( -(x[:,None]**2 + x[None,:]**2)/2 )
+    occgrid = OccupancyGrid()
+    occgrid.header.frame_id = 'map'
+    occgrid.info.resolution = cmap_scale
+    occgrid.info.width = occgrid.info.height = size
+    occgrid.info.origin.position.x = x0
+    occgrid.info.origin.position.y = y0
+    occgrid.info.origin.position.z = 0
+    occgrid.info.origin.orientation.x = 0
+    occgrid.info.origin.orientation.y = 0
+    occgrid.info.origin.orientation.z = 0
+    occgrid.info.origin.orientation.w = 1
+    # NOTE: this occgrid is flipped upside down, but due to symmetry we don't care
+    occgrid.data = z.flatten()
+    cmap_pub.publish(occgrid)
+
+def main(argv):
+    global cmap_pub, cmap_scale
+    rospy.init_node('test_costmap', anonymous=True)
+    # load map to get scale
+    mapfile = rospy.get_param("navmap", rospy.get_param("map"))
+    with open(mapfile) as fh:
+        meta = yaml.load(fh)
+    cmap_scale = meta['resolution']
+    # setup subscriber and publisher
+    cmap_pub = rospy.Publisher('navigator/costmap', OccupancyGrid)
+    sub = rospy.Subscriber('clicked_point', PointStamped, handler)
+    rospy.loginfo("Node ready")
+    #
+    rospy.spin()
+
+if __name__=='__main__':
+    main(sys.argv)
+
+# EOF