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]]) #右,上,左,下の４方向
#DIRECTIONS = np.array([[ 0, 1], [ 1, 1], [ 1,0], [ 1,-1], [ 0,-1], [-1,-1], [-1, 0], [-1, 1]]) #８方向用
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 終了")