import path import pathplanning import wall #-----include std libraries----- import math #-----include extends libraries import numpy as np if __name__ == '__main__': # int main()みたいな rospy.init_node("node_name") # ノード設定 #-----object等の定義----- cell_matrix = [17, 17] cell_size = [0.200, 0.200] mouse = mouse.Mouse() walls = wall.wallPublisher("wall_marker", cell_matrix, cell_size) path = path.pathPublisher("path_red", cell_size) #-----User定義変数----- PI = math.pi vl = vr = dire = 0 px = py = 1 action = -1 map_data = walls.getEmptyMazeMap(cell_matrix) map_data_colored = np.zeros(cell_matrix, dtype=int) try: loop = rospy.Rate(10) # 10[loop/s]の設定をする。 while not rospy.is_shutdown(): #-----ここにプログラムを書く----- #-----センサー読み込み----- sensor = mouse.getSensor() < 0.2
(GOAL_X, GOAL_Y) = [63, 63] ARRAY_SIZE = np.array([len(maze), len(maze[0])]) 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 (
#!/usr/bin/env python #coding: utf-8 # 日本語を使えるようにする import roslib.packages pk_path = roslib.packages.get_pkg_dir('micro_mouse') import sys sys.path.append(pk_path + '/script/lib') import rospy # include<ros/ros.h>のようなもの import path if __name__ == "__main__": rospy.init_node("node_name") path1 = path.pathPublisher("path_red", [0.200, 0.200]) path2 = path.pathPublisher("path_green", [0.200, 0.200]) path3 = path.pathPublisher("path_blue", [0.200, 0.200]) path1.setData([[0, 0], [1, 0], [2, 1], [3, 0]]) path2.setData([[0, 0], [0, 1], [1, 2], [0, 3]]) path3.setData([[0, 0], [1, 1], [2, 4], [4, 2]]) loop = rospy.Rate(10) try: while not rospy.is_shutdown(): path1.publish() path2.publish() path3.publish() loop.sleep() pass except KeyboardInterrupt: print("キーボード割り込み!CTRL-C 終了")