MAP_SIZE = np.array((ARRAY_SIZE - 1) / 2) # mapping data cost_map = np.ones((ARRAY_SIZE), dtype=float) * INF cost_map[START_Y, START_X] = 0 cost_map_colored = np.zeros((ARRAY_SIZE), dtype=int) # node queue nodes = [Node([START_Y, START_X])] DIRECTIONS = np.array([[0, 1], [1, 0], [0, -1], [-1, 0]]) #右,上,左,下の4方向 #DIRECTIONS = np.array([[ 0, 1], [ 1, 1], [ 1,0], [ 1,-1], [ 0,-1], [-1,-1], [-1, 0], [-1, 1]]) #8方向用 cell_matrix = ARRAY_SIZE cell_size = [0.200, 0.200] path1 = path.pathPublisher("path_green", cell_size) walls = wall.wallPublisher("wall_marker", cell_matrix, cell_size) heat = heatmap.heatmapPublisher("heatmap", cell_matrix, cell_size) walls.setData(maze) try: loop = rospy.Rate(10) # 10[loop/s]の設定をする。 while not (rospy.is_shutdown()): #------generate cost map----- while not (len(nodes) == 0): print("--------------------------------------------") for node in nodes: for dire in DIRECTIONS: (ny, nx) = node.pos (search_y, search_x) = node.pos + dire if (0 <= search_x and search_x < ARRAY_SIZE[1]) and ( 0 <= search_y and search_y < ARRAY_SIZE[0]): cost = (calc_cost(ny, nx, search_y, search_x) +
import roslib.packages pk_path = roslib.packages.get_pkg_dir('micro_mouse') import sys sys.path.append(pk_path + '/script/lib') import math import numpy as np import rospy # include<ros/ros.h>のようなもの import heatmap if __name__ == '__main__': # int main()みたいな rospy.init_node("node_name") # ノード設定 cell_matrix = [10, 10] cell_size = [0.200, 0.200] heatmap = heatmap.heatmapPublisher("heatmap_red", cell_matrix, cell_size) map_data = np.arange(100).reshape(10, 10) try: loop = rospy.Rate(10) # 10[loop/s]の設定をする。 while not rospy.is_shutdown(): #-----ここにプログラムを書く----- heatmap.setData(map_data, heatmap.RED, 100) #-----地図情報を表示する----- heatmap.publish() #-----ここまで----- loop.sleep() # 10[loop/s]になるよう調整する。 except KeyboardInterrupt: # try:中にCTRL-Cが押されればココを実行する。 print("キーボード割り込み!CTRL-C 終了")