diff --git a/ros2_ws/build/.built_by b/ros2_ws/build/.built_by
new file mode 100644
index 0000000000000000000000000000000000000000..06e74acb63e6917bd1f0f8853213d49f0c5978e4
--- /dev/null
+++ b/ros2_ws/build/.built_by
@@ -0,0 +1 @@
+colcon
diff --git a/ros2_ws/build/COLCON_IGNORE b/ros2_ws/build/COLCON_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/GameWavefront.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/GameWavefront.py
new file mode 100644
index 0000000000000000000000000000000000000000..aae934fa4259ad8bf8e9b6554fc34074aadac6ed
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/GameWavefront.py
@@ -0,0 +1,375 @@
+# Wavefront Planning
+# Sajad Saeedi, Andrew Davison 2017
+# Implementation is based on the following reference
+#
+# H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki and S. Thrun,
+# Principles of Robot Motion: Theory, Algorithms, and Implementations,
+# MIT Press, Boston, 2005.
+# http://www.cs.cmu.edu/afs/cs/Web/People/motionplanning/
+#
+# From Chapter 4.5 - Wave-Front Planner
+# This planner determines a path via gradient descent on the grid starting from the start.
+# Essentially, the planner determines the path one pixel at a time.
+# The wave-front planner essentially forms a potential function on the grid which has one local minimum and thus is resolution complete.
+# The planner also determines the shortest path, but at the cost of coming dangerously close to obstacles.
+# The major drawback of this method is that the planner has to search the entire space for a path
+
+import pygame, os, math, time, random
+import numpy as np
+try:
+ import pygame
+ from pygame import surfarray
+ from pygame.locals import *
+except ImportError:
+ raise ImportError('Error Importing Pygame/surfarray')
+
+pygame.init()
+
+SCALE = 1
+
+# set the width and height of the screen
+WIDTH = 1500/SCALE
+HEIGHT = 1000/SCALE
+
+# The region we will fill with obstacles
+PLAYFIELDCORNERS = (-3.0, -3.0, 3.0, 3.0)
+
+# Barrier locations
+barriers = []
+# barrier contents are (bx, by, visibilitymask)
+for i in range(100):
+ (bx, by) = (random.uniform(PLAYFIELDCORNERS[0], PLAYFIELDCORNERS[2]), random.uniform(PLAYFIELDCORNERS[1], PLAYFIELDCORNERS[3]))
+ barrier = [bx, by, 0]
+ barriers.append(barrier)
+
+BARRIERRADIUS = 0.1
+
+
+ROBOTRADIUS = 0.15
+W = 2 * ROBOTRADIUS
+SAFEDIST = 0.2
+BARRIERINFILATE = ROBOTRADIUS
+
+MAXVELOCITY = 0.5 #ms^(-1) max speed of each wheel
+MAXACCELERATION = 0.5 #ms^(-2) max rate we can change speed of each wheel
+
+target = (PLAYFIELDCORNERS[2] + 1.0, 0)
+
+k = 160/SCALE # pixels per metre for graphics
+u0 = WIDTH / 2
+v0 = HEIGHT / 2
+
+x = PLAYFIELDCORNERS[0] - 0.5
+y = 0.0
+theta = 0.0
+
+vL = 0.00
+vR = 0.00
+
+size = [int(WIDTH), int(HEIGHT)]
+screen = pygame.display.set_mode(size)
+black = (20,20,40)
+
+# This makes the normal mouse pointer invisible in graphics window
+pygame.mouse.set_visible(0)
+
+# time delta
+dt = 0.1
+
+
+def surfdemo_show(array_img, name):
+ "displays a surface, waits for user to continue"
+ screen = pygame.display.set_mode(array_img.shape[:2], 0, 32)
+ surfarray.blit_array(screen, array_img)
+ pygame.display.flip()
+ pygame.display.set_caption(name)
+ while 1:
+ e = pygame.event.wait()
+ if e.type == MOUSEBUTTONDOWN: break
+ if e.type == KEYDOWN: break
+
+
+def predictPosition(vL, vR, x, y, theta, dt):
+
+ # Position update in time dt
+
+ # Special cases
+ # Straight line motion
+ if (vL == vR):
+ xnew = x + vL * dt * math.cos(theta)
+ ynew = y + vL * dt * math.sin(theta)
+ thetanew = theta
+ # Pure rotation motion
+ elif (vL == -vR):
+ xnew = x
+ ynew = y
+ thetanew = theta + ((vR - vL) * dt / W)
+ else:
+ # Rotation and arc angle of general circular motion
+ R = W / 2.0 * (vR + vL) / (vR - vL)
+ deltatheta = (vR - vL) * dt / W
+ xnew = x + R * (math.sin(deltatheta + theta) - math.sin(theta))
+ ynew = y - R * (math.cos(deltatheta + theta) - math.cos(theta))
+ thetanew = theta + deltatheta
+
+ return (xnew, ynew, thetanew)
+
+
+def drawBarriers(barriers):
+ for barrier in barriers:
+ if(barrier[2] == 0):
+ pygame.draw.circle(screen, (0,20,80), (int(u0 + k * barrier[0]), int(v0 - k * barrier[1])), int(k * BARRIERRADIUS), 0)
+ else:
+ pygame.draw.circle(screen, (0,120,255), (int(u0 + k * barrier[0]), int(v0 - k * barrier[1])), int(k * BARRIERRADIUS), 0)
+ return
+
+def dialtebarrieres(imgin, barriers):
+ imgout = imgin
+ for barrier in barriers:
+ if(barrier[2] == 0):
+ center_u = int(u0 + k * barrier[0])
+ center_v = int(v0 - k * barrier[1])
+ RAD = BARRIERRADIUS+BARRIERINFILATE
+ radius = int(k * (RAD))
+ radius2 = int(k * (BARRIERRADIUS))
+ points = [(x,y) for x in range(center_u-radius, center_u+radius) for y in range(center_v-radius, center_v+radius)]
+ for pt in points:
+ vu = center_u - pt[0]
+ vv = center_v - pt[1]
+ distance = math.sqrt(vv*vv + vu*vu)
+ if (distance < radius and distance > radius2 and pt[0] < WIDTH-1 and pt[1] < HEIGHT-1):
+ imgout[pt[0],pt[1],0] = 0
+ imgout[pt[0],pt[1],1] = 40
+ imgout[pt[0],pt[1],2] = 80
+ return imgout
+
+def MakeWaveFront(imgarray, start_uv, target_uv):
+ print ("building wavefront, please wait ... ")
+ heap = []
+ newheap = []
+
+ u = target_uv[0]
+ v = target_uv[1]
+
+ imgarray[:,:,0] = 0 # use channel 0 for planning
+ imgarray[u, v, 0] = 2
+
+ lastwave = 3 # start by setting nodes around target to 3
+ moves = [(u + 1, v), (u - 1, v), (u, v - 1), (u, v + 1)]
+ for move in moves:
+ imgarray[move[0], move[1], 0] = 3
+ heap.append(move)
+
+ path = False
+ max_search = int(np.sqrt(WIDTH*WIDTH + HEIGHT*HEIGHT))
+ for currentwave in range(4, max_search):
+ lastwave = lastwave + 1
+ while(heap != []):
+ position = heap.pop()
+ (u, v) = position
+ moves = [(u + 1, v), (u - 1, v), (u, v + 1), (u, v - 1)]
+ for move in moves:
+ if(imgarray[move[0], move[1], 2] != 80):
+ if(imgarray[move[0], move[1], 0] == 0 and imgarray[position[0], position[1], 0] == currentwave - 1 and move[0] < WIDTH-1 and move[1] < HEIGHT-1 and move[0]>2 and move[1]>2):
+ imgarray[move[0], move[1], 0] = currentwave
+ newheap.append(move)
+ if(move == start_uv):
+ path = True
+ break
+ if(path == True):
+ break
+ if(path == True):
+ break
+ heap = newheap
+ newheap = []
+ return imgarray, currentwave
+
+
+def FindPath(imgarray, start_uv, currentwave):
+ trajectory = []
+ nextpt = start_uv
+ path = []
+ for backwave in range(currentwave-1,2,-1):
+ path.append(nextpt)
+ (u,v) = nextpt
+ values = []
+ val = []
+ moves = [(u + 1, v), (u - 1, v), (u, v + 1), (u, v - 1), (u + 1, v + 1), (u - 1, v - 1), (u + 1, v - 1), (u - 1, v - 1)]
+ for move in moves:
+ val.append(imgarray[move[0], move[1], 0])
+ if(imgarray[move[0], move[1], 0] == backwave):
+ values.append(imgarray[move[0], move[1], 0])
+ minimum = min(values)
+ indices = [i for i, v in enumerate(val) if v == minimum]
+ nextid = random.choice(indices)
+ nextpt = moves[nextid]
+ return path
+
+def GetControl(x,y,theta, waypoint):
+ wpu = waypoint[0]
+ wpv = waypoint[1]
+
+ vt = 0.2
+ u = u0 + k * x
+ v = v0 - k * y
+ vector = (wpu - u, -wpv + v)
+ vectorangle = math.atan2(vector[1], vector[0])
+ psi = vectorangle - theta
+
+ if(abs(psi) > (2*3.14/180)):
+ if(psi > 0):
+ vR = vt/2
+ vL = -vt/2
+ else:
+ vR = -vt/2
+ vL = vt/2
+ else:
+ wlx = x - (W/2.0) * math.sin(theta)
+ wly = y + (W/2.0) * math.cos(theta)
+ ulx = u0 + k * wlx
+ vlx = v0 - k * wly
+ dl = math.sqrt((ulx-wpu)*(ulx-wpu) + (vlx-wpv)*(vlx-wpv))
+
+ wrx = x + (W/2.0) * math.sin(theta)
+ wry = y - (W/2.0) * math.cos(theta)
+ urx = u0 + k * wrx
+ vrx = v0 - k * wry
+ dr = math.sqrt((urx-wpu)*(urx-wpu) + (vrx-wpv)*(vrx-wpv))
+
+ vR = 1*(2*vt)/(1+(dl/dr))
+ vL = 1*(2*vt - vR)
+
+ return vL, vR
+
+def AnimateRobot(x,y,theta):
+ # Draw robot
+ u = u0 + k * x
+ v = v0 - k * y
+ pygame.draw.circle(screen, (255,255,255), (int(u), int(v)), int(k * ROBOTRADIUS), 3)
+ # Draw wheels
+ # left wheel centre
+ wlx = x - (W/2.0) * math.sin(theta)
+ wly = y + (W/2.0) * math.cos(theta)
+ ulx = u0 + k * wlx
+ vlx = v0 - k * wly
+ WHEELBLOB = 0.04
+ pygame.draw.circle(screen, (0,0,255), (int(ulx), int(vlx)), int(k * WHEELBLOB))
+ # right wheel centre
+ wrx = x + (W/2.0) * math.sin(theta)
+ wry = y - (W/2.0) * math.cos(theta)
+ urx = u0 + k * wrx
+ vrx = v0 - k * wry
+ pygame.draw.circle(screen, (0,0,255), (int(urx), int(vrx)), int(k * WHEELBLOB))
+
+ time.sleep(dt / 5)
+ pygame.display.flip()
+ #time.sleep(0.2)
+
+def AnimateNavigation(barriers, waypint, path):
+ screen.fill(black)
+ drawBarriers(barriers)
+
+ for pt in path:
+ screen.set_at(pt, (255,255,255))
+
+ pygame.draw.circle(screen, (255,100,0), target_uv, int(k * ROBOTRADIUS), 0)
+ pygame.draw.circle(screen, (255,100,0), (int(u0 + k * target[0]), int(v0 - k * target[1])), int(k * ROBOTRADIUS), 0)
+ pygame.draw.circle(screen, (255,0,255), (int(waypint[0]), int(waypint[1])), int(5))
+
+def AnimatePath(imgarray, path, start_uv):
+ for pt in path:
+ imgarray[pt[0], pt[1], 0] = 0
+ imgarray[pt[0], pt[1], 1] = 255
+ imgarray[pt[0], pt[1], 2] = 255
+
+ #imgarray[:,:,0] /= imgarray[:,:,0].max()/255.0
+ scalefactor = imgarray[:,:,0].max()/255.0
+ imgarray[:,:,0] = imgarray[:,:,0]/scalefactor
+ imgarray[:,:,0].astype(int)
+ imgarray[start_uv[0], start_uv[1], 0] = 0
+ imgarray[start_uv[0], start_uv[1], 1] = 255
+ imgarray[start_uv[0], start_uv[1], 2] = 255
+
+ surfdemo_show(imgarray, 'Wavefront Path Planning')
+
+
+def GetWaypoint(x,y,theta, path, waypointIndex, waypointSeperation, target):
+ reset = False
+ waypoint = path[waypointIndex]
+ u = u0 + k * x
+ v = v0 - k * y
+ distance_to_wp = math.sqrt((waypoint[0]-u)**2+(waypoint[1]-v)**2) # todo compare distance in metrics
+ if(distance_to_wp < 3):
+ waypointIndex = waypointSeperation + waypointIndex
+ if(waypointIndex > len(path)):
+ waypointIndex = len(path) - 1
+
+ return waypoint, reset, waypointIndex
+
+def IsAtTarget(x,y,target):
+ disttotarget = math.sqrt((x - target[0])**2 + (y - target[1])**2)
+ if (disttotarget < 0.04):
+ return True
+ else:
+ return False
+
+
+while(1):
+ start_uv = (int(u0 + k * x), int(v0 - k * y))
+ target_uv = (int(u0 + k * target[0]), int(v0 - k * target[1]))
+ print ("start is: ", start_uv)
+ print ("goal is: ", target_uv)
+
+ # prepare map of the world for plannign
+ screen.fill(black)
+ drawBarriers(barriers)
+ pygame.draw.circle(screen, (255,100,0), target_uv, int(k * ROBOTRADIUS), 0)
+ pygame.draw.circle(screen, (255,255,0), start_uv, int(k * ROBOTRADIUS), 0)
+ imgscreen8 = pygame.surfarray.array3d(screen)
+ imgscreen16 = np.array(imgscreen8, dtype=np.uint16)
+ drawBarriers(barriers)
+ imgarray = dialtebarrieres(imgscreen16, barriers)
+ pygame.display.flip()
+ pygame.display.set_caption('Wavefront Path Planning')
+
+ # build wavefront, given the map, start point, and target point
+ imgarray, currentwave = MakeWaveFront(imgarray, start_uv, target_uv)
+ print ("press a key to start navaigation ... ")
+
+ # find the path using the wavefront
+ path = FindPath(imgarray, start_uv, currentwave)
+
+ # normlaize and show the path on the map
+ AnimatePath(imgarray, path, start_uv)
+
+ # set start point
+ x = PLAYFIELDCORNERS[0] - 0.5
+ y = 0
+ theta = 0
+ waypointSeperation = 40;
+ waypointIndex = waypointSeperation;
+
+ # loop to navigate the path from start to target
+ while(1):
+ # get a waypoint to follow the path
+ (waypoint, reset, waypointIndex)= GetWaypoint(x,y,theta, path, waypointIndex, waypointSeperation, target)
+
+ # reset the simulation, if robot is at target
+ if (IsAtTarget(x,y,target)):
+ target = (target[0], random.uniform(PLAYFIELDCORNERS[1], PLAYFIELDCORNERS[3]))
+ x = PLAYFIELDCORNERS[0] - 0.5
+ y = 0.0
+ theta = 0.0
+ break
+
+
+ # calculate control signals to get to the next waypoint
+ (vL, vR) = GetControl(x,y,theta, waypoint)
+
+ # Actually now move and update position based on chosen vL and vR
+ (x, y, theta) = predictPosition(vL, vR, x, y, theta, dt)
+
+ # animate
+ AnimateNavigation(barriers, waypoint, path)
+ AnimateRobot(x,y,theta)
+
\ No newline at end of file
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/WavefrontChatGpt.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/WavefrontChatGpt.py
new file mode 100644
index 0000000000000000000000000000000000000000..a358dfa0282ce93f94817eca2941d5fa2e991539
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/WavefrontChatGpt.py
@@ -0,0 +1,53 @@
+import numpy as np
+import cv2
+import matplotlib.pyplot as plt
+import time
+
+class WaveFrontPlanner:
+ __wall = 999
+ __goal = 1
+ __nothing = '000'
+ __path = 'PATH'
+
+ def __init__(self, mask_list):
+ if not isinstance(mask_list, np.ndarray):
+ raise ValueError("Invalid input type. 'mask_list' must be a numpy array.")
+ self.__mapOfWorld = np.array([['999' if j > 200 else '000' for j in row] for row in mask_list])
+
+ def minSurroundingNodeValue(self, x, y):
+ values = [self.__mapOfWorld[x + 1][y], self.__mapOfWorld[x - 1][y],
+ self.__mapOfWorld[x][y + 1], self.__mapOfWorld[x][y - 1]]
+ min_value = min(value for value in values if value != self.__nothing)
+ min_index = values.index(min_value) + 1
+ return min_value, min_index
+
+ def waveFrontAlgorithm(self):
+ goal_coordinates = np.where(self.__mapOfWorld == self.__goal)
+ goal_x, goal_y = goal_coordinates[0][0], goal_coordinates[1][0]
+ queue = [(goal_x, goal_y)]
+
+ while queue:
+ x, y = queue.pop(0)
+
+ if self.__mapOfWorld[x][y] == self.__nothing:
+ min_value, min_index = self.minSurroundingNodeValue(x, y)
+ self.__mapOfWorld[x][y] = min_value + 1
+ queue.append((x + 1, y)) # Move down
+ queue.append((x - 1, y)) # Move up
+ queue.append((x, y + 1)) # Move right
+ queue.append((x, y - 1)) # Move left
+
+ self.__mapOfWorld[goal_x][goal_y] = self.__goal
+
+ def visualizeMap(self):
+ plt.imshow(cv2.flip(self.__mapOfWorld, 0), cmap='hot', interpolation='nearest')
+ plt.colorbar()
+ plt.show()
+
+# Beispielverwendung:
+mask_list = np.zeros((200, 200))
+mask_list[50:150, 50:150] = 1
+
+planner = WaveFrontPlanner(mask_list)
+planner.waveFrontAlgorithm()
+planner.visualizeMap()
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/WavefrontPlanner.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/WavefrontPlanner.py
new file mode 100644
index 0000000000000000000000000000000000000000..13cebc92bac4ee4b8096a05c1e3007b8f8741a69
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/WavefrontPlanner.py
@@ -0,0 +1,397 @@
+############################################################################
+# WAVEFRONT ALGORITHM
+# Adapted to Python Code By Darin Velarde
+# Fri Jan 29 13:56:53 PST 2010
+# from C code from John Palmisano
+# (www.societyofrobots.com)
+############################################################################
+import cv2
+import numpy as np
+import matplotlib.pyplot as plt
+import time
+
+class waveFrontPlanner:
+ ############################################################################
+ # WAVEFRONT ALGORITHM
+ # Adapted to Python Code By Darin Velarde
+ # Fri Jan 29 13:56:53 PST 2010
+ # from C code from John Palmisano
+ # (www.societyofrobots.com)
+ ############################################################################
+
+ def __init__(self, mapOfWorld, slow=False):
+ self.__slow = slow
+ self.__mapOfWorld = mapOfWorld
+ if str(type(mapOfWorld)).find("numpy") != -1:
+ #If its a numpy array
+ self.__height, self.__width = self.__mapOfWorld.shape
+ else:
+ self.__height, self.__width = len(self.__mapOfWorld), len(self.__mapOfWorld[0])
+
+ self.__nothing = 000
+ self.__wall = 999
+ self.__goal = 1
+ self.__path = "PATH"
+
+ self.__finalPath = []
+
+ #Robot value
+ self.__robot = 254
+ #Robot default Location
+ self.__robot_x = 0
+ self.__robot_y = 0
+
+ #default goal location
+ self.__goal_x = 8
+ self.__goal_y = 9
+
+ #temp variables
+ self.__temp_A = 0
+ self.__temp_B = 0
+ self.__counter = 0
+ self.__steps = 0 #determine how processor intensive the algorithm was
+
+ #when searching for a node with a lower value
+ self.__minimum_node = 250
+ self.__min_node_location = 250
+ self.__new_state = 1
+ self.__old_state = 1
+ self.__reset_min = 250 #above this number is a special (wall or robot)
+ ###########################################################################
+
+ def setRobotPosition(self, x, y):
+ """
+ Sets the robot's current position
+
+ """
+
+ self.__robot_x = x
+ self.__robot_y = y
+ ###########################################################################
+
+ def setGoalPosition(self, x, y):
+ """
+ Sets the goal position.
+
+ """
+
+ self.__goal_x = x
+ self.__goal_y = y
+ ###########################################################################
+
+ def robotPosition(self):
+ return (self.__robot_x, self.__robot_y)
+ ###########################################################################
+
+ def goalPosition(self):
+ return (self.__goal_x, self.__goal_y)
+ ###########################################################################
+
+ def run(self, prnt=False):
+ """
+ The entry point for the robot algorithm to use wavefront propagation.
+
+ """
+
+ path = []
+ while self.__mapOfWorld[self.__robot_x][self.__robot_y] != self.__goal:
+ if self.__steps > 10000:
+ print ("Cannot find a path.")
+ return
+ #find new location to go to
+ self.__new_state = self.propagateWavefront()
+ #update location of robot
+ if self.__new_state == 1:
+ self.__robot_x -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" % (self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 2:
+ self.__robot_y += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 3:
+ self.__robot_x += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 4:
+ self.__robot_y -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ self.__old_state = self.__new_state
+ msg = "Found the goal in %i steps:\n" % self.__steps
+ msg += "mapOfWorld size= %i %i\n\n" % (self.__height, self.__width)
+ if prnt:
+ print(msg)
+ self.printMap()
+ return path
+ ###########################################################################
+
+ def propagateWavefront(self, prnt=False):
+ self.unpropagate()
+ #Old robot location was deleted, store new robot location in mapOfWorld
+ self.__mapOfWorld[self.__robot_x][self.__robot_y] = self.__robot
+ self.__path = self.__robot
+ #start location to begin scan at goal location
+ self.__mapOfWorld[self.__goal_x][self.__goal_y] = self.__goal
+ counter = 0
+ while counter < 200: #allows for recycling until robot is found
+ x = 0
+ y = 0
+ #time.sleep(0.00001)
+ #while the mapOfWorld hasnt been fully scanned
+ while x < self.__height and y < self.__width:
+ #if this location is a wall or the goal, just ignore it
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal:
+ #a full trail to the robot has been located, finished!
+ minLoc = self.minSurroundingNodeValue(x, y)
+ if minLoc < self.__reset_min and \
+ self.__mapOfWorld[x][y] == self.__robot:
+ if prnt:
+ print("Finished Wavefront:\n")
+ self.printMap()
+ # Tell the robot to move after this return.
+ return self.__min_node_location
+ #record a value in to this node
+ elif self.__minimum_node != self.__reset_min:
+ #if this isnt here, 'nothing' will go in the location
+ self.__mapOfWorld[x][y] = self.__minimum_node + 1
+ #go to next node and/or row
+ y += 1
+ if y == self.__width and x != self.__height:
+ x += 1
+ y = 0
+ #print self.__robot_x, self.__robot_y
+ if prnt:
+ print("Sweep #: %i\n" % (counter + 1))
+ self.printMap()
+ self.__steps += 1
+ counter += 1
+ return 0
+ ###########################################################################
+
+ def unpropagate(self):
+ """
+ clears old path to determine new path
+ stay within boundary
+
+ """
+
+ for x in range(0, self.__height):
+ for y in range(0, self.__width):
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal and \
+ self.__mapOfWorld[x][y] != self.__path:
+ #if this location is a wall or goal, just ignore it
+ self.__mapOfWorld[x][y] = self.__nothing #clear that space
+ ###########################################################################
+
+ def minSurroundingNodeValue(self, x, y):
+ """
+ this method looks at a node and returns the lowest value around that
+ node.
+
+ """
+
+ #reset minimum
+ self.__minimum_node = self.__reset_min
+ #down
+ if x < self.__height -1:
+ if self.__mapOfWorld[x + 1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x + 1][y] != self.__nothing:
+ #find the lowest number node, and exclude empty nodes (0's)
+ self.__minimum_node = self.__mapOfWorld[x + 1][y]
+ self.__min_node_location = 3
+ #up
+ if x > 0:
+ if self.__mapOfWorld[x-1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x-1][y] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x-1][y]
+ self.__min_node_location = 1
+ #right
+ if y < self.__width -1:
+ if self.__mapOfWorld[x][y + 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y + 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y + 1]
+ self.__min_node_location = 2
+ #left
+ if y > 0:
+ if self.__mapOfWorld[x][y - 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y - 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y-1]
+ self.__min_node_location = 4
+ return self.__minimum_node
+ ###########################################################################
+
+ def printMap(self):
+ """
+ Prints out the map of this instance of the class.
+
+ """
+
+ msg = ''
+ for temp_B in range(0, self.__height):
+ for temp_A in range(0, self.__width):
+ if self.__mapOfWorld[temp_B][temp_A] == self.__wall:
+ msg += "%04s" % "[#]"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__robot:
+ msg += "%04s" % "-"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__goal:
+ msg += "%04s" % "G"
+ else:
+ msg += "%04s" % str(self.__mapOfWorld[temp_B][temp_A])
+ msg += "\n\n"
+ msg += "\n\n"
+ print(msg)
+ #
+ if self.__slow == True:
+ time.sleep(0.05)
+############################################################################
+
+class mapCreate:
+ ############################################################################
+
+ def create_map(self, scale,img):
+
+ # Map erstellen
+
+ # Img Objekt
+ #cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+ #success, img = cap.read()
+
+ #Karte von Bild
+ #succ, img = cv2.imread(2)
+ print('Original Dimensions : ',img.shape)
+
+ scale_percent = 100-scale # percent of original size
+ width = int(img.shape[1] * scale_percent / 100)
+ height = int(img.shape[0] * scale_percent / 100)
+ dim = (width, height)
+
+ # resize image
+ resized = cv2.resize(img, dim, interpolation = cv2.INTER_AREA)
+ print('Resized Dimensions : ',resized.shape)
+
+ gray = cv2.cvtColor(resized, cv2.COLOR_RGB2GRAY)
+ mask = cv2.inRange(gray, np.array([90]), np.array([255]))
+
+ mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+ mask_list= np.ndarray.tolist(mask)
+
+ #Markiere alle leeren Zellen mit 000 und alle Zellen mit Hinderniss mit 999
+ for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['999' if j > 200 else '000' for j in mask_list[i]]
+
+ mapOfWorld = np.array(mapOfWorld, dtype = int)
+ mapOfWorldSized = mapOfWorld.copy()
+
+ e = 3
+ #Hindernisse Größer machen
+ for i in range(0,mapOfWorld.shape[0]):
+ for j in range(0,mapOfWorld.shape[1]):
+ for r in range(0,e):
+ try:
+ if (mapOfWorldSized[i][j] == 999):
+ mapOfWorld[i][j+e] = 999
+ mapOfWorld[i+e][j] = 999
+ mapOfWorld[i-e][j] = 999
+ mapOfWorld[i][j-e] = 999
+ mapOfWorld[i+e][j+e] = 999
+ mapOfWorld[i+e][j-e] = 999
+ mapOfWorld[i-e][j+e] = 999
+ mapOfWorld[i-e][j-e] = 999
+ except Exception:
+ continue
+
+ return mapOfWorld
+ ############################################################################
+
+ def scale_koord(self, sx, sy, gx, gy,scale):
+ scale_percent = 100-scale # percent of original size
+
+ sx = int(sx*scale_percent/100)
+ sy = int(sy*scale_percent/100)
+ gx = int(gx*scale_percent/100)
+ gy = int(gy*scale_percent/100)
+
+ return sx,sy,gx,gy
+ ############################################################################
+
+ def rescale_koord(self, path,scale):
+ scale_percent = 100-scale # percent of original size100
+
+ path = np.array(path)
+
+ for i in range(len(path)):
+ path[i] = path[i]*100/scale_percent
+
+ return path
+ ############################################################################
+
+ def calc_trans(self, x, y, height):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+###############################################################################
+
+if __name__ == "__main__":
+ """
+ X is vertical, Y is horizontal
+
+ """
+
+ Map = mapCreate() #Map Objekt erzeugen
+
+ #Verkleinerungsfaktor in %
+ scale = 85
+
+ img = cv2.imread("D:/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/karte.jpg", cv2.COLOR_RGB2GRAY)
+ #cap = cv2.VideoCapture(2)
+ mapWorld = Map.create_map(scale, img)
+ height, width = img.shape[:2]
+
+ #Angaben in Weltkoordinaten
+ sy = 300 #X-Koordinate im Weltkoordinaten System
+ sx = 200 #Y-Koordinate im Weltkoordinaten System
+
+ gy = 700 #X-Koordinate im Weltkordinaten System
+ gx = 240 #Y-Koordinate im Weltkoordinaten System
+
+ sx,sy,gx,gy = Map.scale_koord(sx,sy,gx,gy,scale) #Kordinaten Tauschen X,Y
+
+ start = time.time()
+ planner = waveFrontPlanner(mapWorld)
+ planner.setGoalPosition(gx,gy)
+ planner.setRobotPosition(sx,sy)
+ path = planner.run(False)
+ end = time.time()
+ print("Took %f seconds to run wavefront simulation" % (end - start))
+
+ path = Map.rescale_koord(path, scale)
+ #print(path)
+#%% Plot
+ #Programm Koordinaten
+ imgTrans = cv2.transpose(img) # X und Y-Achse im Bild tauschen
+ imgPlot, ax1 = plt.subplots()
+
+ ax1.set_title('Programm Koordinaten')
+ ax1.imshow(imgTrans)
+ ax1.set_xlabel('[px]')
+ ax1.set_ylabel('[px]')
+ ax1.scatter(path[:,0], path[:,1], color='r')
+
+ #Bild Koordinaten
+ imgPlot2, ax2 = plt.subplots()
+ ax2.set_title('Bild Koordinaten')
+ ax2.set_xlabel('[px]')
+ ax2.set_ylabel('[px]')
+ ax2.imshow(img)
+ ax2.scatter(path[:,1], path[:,0], color='r')
+
+#%% Sphero Pfad
+ pathWeltX, pathWeltY = Map.calc_trans(path[:,1], path[:,0], height)
\ No newline at end of file
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/__init__.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/_constants.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/_constants.py
new file mode 100644
index 0000000000000000000000000000000000000000..fa8a0e875eed9f484945c1eeaf2fda3fd98503a4
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/_constants.py
@@ -0,0 +1,72 @@
+'''
+Known Peripheral UUIDs, obtained by querying using the Bluepy module:
+=====================================================================
+Anti DOS Characteristic <00020005-574f-4f20-5370-6865726f2121>
+Battery Level Characteristic <Battery Level>
+Peripheral Preferred Connection Parameters Characteristic <Peripheral Preferred Connection Parameters>
+API V2 Characteristic <00010002-574f-4f20-5370-6865726f2121>
+DFU Control Characteristic <00020002-574f-4f20-5370-6865726f2121>
+Name Characteristic <Device Name>
+Appearance Characteristic <Appearance>
+DFU Info Characteristic <00020004-574f-4f20-5370-6865726f2121>
+Service Changed Characteristic <Service Changed>
+Unknown1 Characteristic <00020003-574f-4f20-5370-6865726f2121>
+Unknown2 Characteristic <00010003-574f-4f20-5370-6865726f2121>
+
+The rest of the values saved in the dictionaries below, were borrowed from
+@igbopie's javacript library, which is available at https://github.com/igbopie/spherov2.js
+
+'''
+
+deviceID = {"apiProcessor": 0x10, # 16
+ "systemInfo": 0x11, # 17
+ "powerInfo": 0x13, # 19
+ "driving": 0x16, # 22
+ "animatronics": 0x17, # 23
+ "sensor": 0x18, # 24
+ "something": 0x19, # 25
+ "userIO": 0x1a, # 26
+ "somethingAPI": 0x1f} # 31
+
+SystemInfoCommands = {"mainApplicationVersion": 0x00, # 00
+ "bootloaderVersion": 0x01, # 01
+ "something": 0x06, # 06
+ "something2": 0x13, # 19
+ "something6": 0x12, # 18
+ "something7": 0x28} # 40
+
+sendPacketConstants = {"StartOfPacket": 0x8d, # 141
+ "EndOfPacket": 0xd8} # 216
+
+userIOCommandIDs = {"allLEDs": 0x0e} # 14
+
+flags= {"isResponse": 0x01, # 0x01
+ "requestsResponse": 0x02, # 0x02
+ "requestsOnlyErrorResponse": 0x04, # 0x04
+ "resetsInactivityTimeout": 0x08} # 0x08
+
+powerCommandIDs={"deepSleep": 0x00, # 0
+ "sleep": 0x01, # 01
+ "batteryVoltage": 0x03, # 03
+ "wake": 0x0D, # 13
+ "something": 0x05, # 05
+ "something2": 0x10, # 16
+ "something3": 0x04, # 04
+ "something4": 0x1E} # 30
+
+drivingCommands={"rawMotor": 0x01, # 1
+ "resetHeading": 0x06, # 6
+ "driveAsSphero": 0x04, # 4
+ "driveAsRc": 0x02, # 2
+ "driveWithHeading": 0x07, # 7
+ "stabilization": 0x0C} # 12
+
+sensorCommands={'sensorMask': 0x00, # 00
+ 'sensorResponse': 0x02, # 02
+ 'configureCollision': 0x11, # 17
+ 'collisionDetectedAsync': 0x12, # 18
+ 'resetLocator': 0x13, # 19
+ 'enableCollisionAsync': 0x14, # 20
+ 'sensor1': 0x0F, # 15
+ 'sensor2': 0x17, # 23
+ 'configureSensorStream': 0x0C} # 12
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/a_star.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/a_star.py
new file mode 100644
index 0000000000000000000000000000000000000000..6d203504327dd356614b08d9d8f169f63aa2ad55
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/a_star.py
@@ -0,0 +1,282 @@
+"""
+
+A* grid planning
+
+author: Atsushi Sakai(@Atsushi_twi)
+ Nikos Kanargias (nkana@tee.gr)
+
+See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
+
+"""
+
+import math
+
+import matplotlib.pyplot as plt
+
+show_animation = True
+
+
+class AStarPlanner:
+
+ def __init__(self, ox, oy, resolution, rr):
+ """
+ Initialize grid map for a star planning
+
+ ox: x position list of Obstacles [m]
+ oy: y position list of Obstacles [m]
+ resolution: grid resolution [m]
+ rr: robot radius[m]
+ """
+
+ self.resolution = resolution
+ self.rr = rr
+ self.min_x, self.min_y = 0, 0
+ self.max_x, self.max_y = 0, 0
+ self.obstacle_map = None
+ self.x_width, self.y_width = 0, 0
+ self.motion = self.get_motion_model()
+ self.calc_obstacle_map(ox, oy)
+
+ class Node:
+ def __init__(self, x, y, cost, parent_index):
+ self.x = x # index of grid
+ self.y = y # index of grid
+ self.cost = cost
+ self.parent_index = parent_index
+
+ def __str__(self):
+ return str(self.x) + "," + str(self.y) + "," + str(
+ self.cost) + "," + str(self.parent_index)
+
+ def planning(self, sx, sy, gx, gy):
+ """
+ A star path search
+
+ input:
+ s_x: start x position [m]
+ s_y: start y position [m]
+ gx: goal x position [m]
+ gy: goal y position [m]
+
+ output:
+ rx: x position list of the final path
+ ry: y position list of the final path
+ """
+
+ start_node = self.Node(self.calc_xy_index(sx, self.min_x),
+ self.calc_xy_index(sy, self.min_y), 0.0, -1)
+ goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
+ self.calc_xy_index(gy, self.min_y), 0.0, -1)
+
+ open_set, closed_set = dict(), dict()
+ open_set[self.calc_grid_index(start_node)] = start_node
+
+ while True:
+ if len(open_set) == 0:
+ print("Open set is empty..")
+ break
+
+ c_id = min(
+ open_set,
+ key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node,
+ open_set[
+ o]))
+ current = open_set[c_id]
+
+ # show graph
+ if show_animation: # pragma: no cover
+ plt.plot(self.calc_grid_position(current.x, self.min_x),
+ self.calc_grid_position(current.y, self.min_y), "xc")
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect('key_release_event',
+ lambda event: [exit(
+ 0) if event.key == 'escape' else None])
+ if len(closed_set.keys()) % 10 == 0:
+ plt.pause(0.001)
+
+ if current.x == goal_node.x and current.y == goal_node.y:
+ print("Find goal")
+ goal_node.parent_index = current.parent_index
+ goal_node.cost = current.cost
+ break
+
+ # Remove the item from the open set
+ del open_set[c_id]
+
+ # Add it to the closed set
+ closed_set[c_id] = current
+
+ # expand_grid search grid based on motion model
+ for i, _ in enumerate(self.motion):
+ node = self.Node(current.x + self.motion[i][0],
+ current.y + self.motion[i][1],
+ current.cost + self.motion[i][2], c_id)
+ n_id = self.calc_grid_index(node)
+
+ # If the node is not safe, do nothing
+ if not self.verify_node(node):
+ continue
+
+ if n_id in closed_set:
+ continue
+
+ if n_id not in open_set:
+ open_set[n_id] = node # discovered a new node
+ else:
+ if open_set[n_id].cost > node.cost:
+ # This path is the best until now. record it
+ open_set[n_id] = node
+
+ rx, ry = self.calc_final_path(goal_node, closed_set)
+
+ return rx, ry
+
+ def calc_final_path(self, goal_node, closed_set):
+ # generate final course
+ rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [
+ self.calc_grid_position(goal_node.y, self.min_y)]
+ parent_index = goal_node.parent_index
+ while parent_index != -1:
+ n = closed_set[parent_index]
+ rx.append(self.calc_grid_position(n.x, self.min_x))
+ ry.append(self.calc_grid_position(n.y, self.min_y))
+ parent_index = n.parent_index
+
+ return rx, ry
+
+ @staticmethod
+ def calc_heuristic(n1, n2):
+ w = 1.0 # weight of heuristic
+ d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
+ return d
+
+ def calc_grid_position(self, index, min_position):
+ """
+ calc grid position
+
+ :param index:
+ :param min_position:
+ :return:
+ """
+ pos = index * self.resolution + min_position
+ return pos
+
+ def calc_xy_index(self, position, min_pos):
+ return round((position - min_pos) / self.resolution)
+
+ def calc_grid_index(self, node):
+ return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
+
+ def verify_node(self, node):
+ px = self.calc_grid_position(node.x, self.min_x)
+ py = self.calc_grid_position(node.y, self.min_y)
+
+ if px < self.min_x:
+ return False
+ elif py < self.min_y:
+ return False
+ elif px >= self.max_x:
+ return False
+ elif py >= self.max_y:
+ return False
+
+ # collision check
+ if self.obstacle_map[node.x][node.y]:
+ return False
+
+ return True
+
+ def calc_obstacle_map(self, ox, oy):
+
+ self.min_x = round(min(ox))
+ self.min_y = round(min(oy))
+ self.max_x = round(max(ox))
+ self.max_y = round(max(oy))
+ print("min_x:", self.min_x)
+ print("min_y:", self.min_y)
+ print("max_x:", self.max_x)
+ print("max_y:", self.max_y)
+
+ self.x_width = round((self.max_x - self.min_x) / self.resolution)
+ self.y_width = round((self.max_y - self.min_y) / self.resolution)
+ print("x_width:", self.x_width)
+ print("y_width:", self.y_width)
+
+ # obstacle map generation
+ self.obstacle_map = [[False for _ in range(self.y_width)]
+ for _ in range(self.x_width)]
+ for ix in range(self.x_width):
+ x = self.calc_grid_position(ix, self.min_x)
+ for iy in range(self.y_width):
+ y = self.calc_grid_position(iy, self.min_y)
+ for iox, ioy in zip(ox, oy):
+ d = math.hypot(iox - x, ioy - y)
+ if d <= self.rr:
+ self.obstacle_map[ix][iy] = True
+ break
+
+ @staticmethod
+ def get_motion_model():
+ # dx, dy, cost
+ motion = [[1, 0, 1],
+ [0, 1, 1],
+ [-1, 0, 1],
+ [0, -1, 1],
+ [-1, -1, math.sqrt(2)],
+ [-1, 1, math.sqrt(2)],
+ [1, -1, math.sqrt(2)],
+ [1, 1, math.sqrt(2)]]
+
+ return motion
+
+
+def main():
+ print(__file__ + " start!!")
+
+ # start and goal position
+ sx = 10.0 # [m]
+ sy = 10.0 # [m]
+ gx = 50.0 # [m]
+ gy = 50.0 # [m]
+ grid_size = 2.0 # [m]
+ robot_radius = 1.0 # [m]
+
+ # set obstacle positions
+ ox, oy = [], []
+ for i in range(-10, 60):
+ ox.append(i)
+ oy.append(-10.0)
+ for i in range(-10, 60):
+ ox.append(60.0)
+ oy.append(i)
+ for i in range(-10, 61):
+ ox.append(i)
+ oy.append(60.0)
+ for i in range(-10, 61):
+ ox.append(-10.0)
+ oy.append(i)
+ for i in range(-10, 40):
+ ox.append(20.0)
+ oy.append(i)
+ for i in range(0, 40):
+ ox.append(40.0)
+ oy.append(60.0 - i)
+
+ if show_animation: # pragma: no cover
+ plt.plot(ox, oy, ".k")
+ plt.plot(sx, sy, "og")
+ plt.plot(gx, gy, "xb")
+ plt.grid(True)
+ plt.axis("equal")
+
+ a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
+ rx, ry = a_star.planning(sx, sy, gx, gy)
+
+ if show_animation: # pragma: no cover
+ plt.plot(rx, ry, "-r")
+ plt.pause(0.001)
+ plt.show()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/blobErkennung.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/blobErkennung.py
new file mode 100644
index 0000000000000000000000000000000000000000..20036f56c3bb1f237bb1ec84a06c18afa70b5308
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/blobErkennung.py
@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+
+# Standard imports
+import cv2
+import numpy as np;
+
+# Img Objekt
+#cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+cap = cv2.VideoCapture(4)
+
+# Set up the detector with default parameters.
+detector = cv2.SimpleBlobDetector()
+
+# Setup SimpleBlobDetector parameters.
+params = cv2.SimpleBlobDetector_Params()
+
+# Change thresholds
+#params.minThreshold = 5
+#params.maxThreshold = 500
+
+#FIlter by Color
+params.filterByColor = True
+params.blobColor = 255
+
+# Filter by Area.
+params.filterByArea = True
+params.minArea = 20
+params.maxArea = 200
+
+# Filter by Circularity
+#params.filterByCircularity = True
+#params.minCircularity = 0.5
+#params.maxCircularity = 1
+
+# Filter by Convexity
+#params.filterByConvexity = True
+#params.minConvexity = 0.7
+#params.maxConvexity = 1
+
+# Filter by Inertia
+params.filterByInertia = True
+params.minInertiaRatio = 0.5
+
+# Create a detector with the parameters
+detector = cv2.SimpleBlobDetector_create(params)
+
+success, img = cap.read()
+height, width = img.shape[:2]
+
+def calc_trans(x,y):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+
+while True:
+
+ success, img = cap.read()
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+
+ # Detect blobs.
+ keypoints = detector.detect(gray)
+
+ #print(keypoints)
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+ #s = keyPoint.sizekeyPoints
+
+ # Draw detected blobs as red circles.
+ # cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures
+ # the size of the circle corresponds to the size of blob
+
+ im_with_keypoints = cv2.drawKeypoints(gray, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
+
+ # Show blobs
+ cv2.imshow("Keypoints", im_with_keypoints)
+
+ if cv2.waitKey(10) & 0xFF == ord('q'):
+ break
\ No newline at end of file
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/dynamicPathPlanning.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/dynamicPathPlanning.py
new file mode 100644
index 0000000000000000000000000000000000000000..6607c85a415e48eac5483d8ecc329954fbf76051
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/dynamicPathPlanning.py
@@ -0,0 +1,504 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import robotController as rc
+import mapGeneration as mg
+import random
+from controller import Robot
+
+#======[Scenario Control functions]============================================
+
+#This function causes the robot to wait some time steps before starting it's operation
+#DO NOT CHANGE THIS FUNCTION OR IT'S CALL IN THE CODE
+def waitRandomTimeSteps(turtle, stop=50):
+ initialPose = np.zeros(7)
+ initialPose[0:2] = findRobotPosition(turtle)
+ turtle.setPoseGoal(initialPose)
+ turtle.drive_goalPose()
+ nRand = random.randint(1, stop)
+ for i in range(nRand):
+ turtle.robot.step(turtle.TIME_STEP)
+ turtle.update()
+
+#=========[Example functions for finding objects in the map]===================
+
+def findTargetPosition(turtlebot):
+ mapInd = turtlebot.findInMap_FinalTarget()
+ if(len(mapInd) != 0):
+ pos = turtlebot.getPositionFromMapIndex(mapInd[0])
+ else:
+ pos = []
+ return np.array(pos, dtype=float).tolist()
+
+def findRobotPosition(turtlebot):
+ mapInds = turtlebot.findInMap_Robot()
+ robotPos = np.zeros((len(mapInds), 2))
+ for i in range(len(mapInds)):
+ pos = turtlebot.getPositionFromMapIndex(mapInds[i])
+ robotPos[i, ...] = pos
+ XYInd = np.average(robotPos, axis=0)
+ return np.array(XYInd, dtype=float).tolist()
+
+def findObstaclePositions(turtlebot):
+ mapInds = turtlebot.findInMap_Obstacles()
+ obsPos = np.zeros((len(mapInds), 2))
+ for i in range(len(mapInds)):
+ pos = turtlebot.getPositionFromMapIndex(mapInds[i])
+ obsPos[i, ...] = pos
+ return np.array(obsPos, dtype=float).tolist()
+
+#=========[Framework for own implementations of path tracking and planning]====
+
+def checkForPossibleCollisions(turtlebot, turtlePos, collisionCntr):
+ #Fill this with your collision detection between the planed path and
+ #moving obstacles
+ hitBox = 0
+
+ faktor = 0.1
+
+ turtlePos1 = [turtlePos[0] + faktor, turtlePos[1] + faktor]
+ turtlePos2 = [turtlePos[0] + faktor, turtlePos[1] - faktor]
+ turtlePos3 = [turtlePos[0] - faktor, turtlePos[1] + faktor]
+ turtlePos4 = [turtlePos[0] - faktor, turtlePos[1] - faktor]
+
+ turtlePos5 = [turtlePos[0] + faktor, turtlePos[1] ]
+ turtlePos6 = [turtlePos[0] , turtlePos[1] + faktor]
+ turtlePos7 = [turtlePos[0] - faktor, turtlePos[1] ]
+ turtlePos8 = [turtlePos[0] , turtlePos[1] - faktor]
+
+ turtlePosBool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos))
+ turtlePos1Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos1))
+ turtlePos2Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos2))
+ turtlePos3Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos3))
+ turtlePos4Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos4))
+ turtlePos5Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos5))
+ turtlePos6Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos6))
+ turtlePos7Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos7))
+ turtlePos8Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos8))
+
+ hitBox = turtlePosBool | turtlePos1Bool | turtlePos2Bool | turtlePos3Bool | turtlePos4Bool | turtlePos5Bool | turtlePos6Bool | turtlePos7Bool | turtlePos8Bool
+
+ if(hitBox):
+ collisionCntr += 1
+ return True
+
+ if(~hitBox):
+ return False
+
+def staticPathTracking(turtlebot, path, indCntr):
+ #This delta value determines the precision of the position control during path tracking
+ #A high value leads towards a larger curve radius at corners in the path,
+ # which might result in a smoothing of the path and keeping the velocity high.
+ #A lower value sets a higher precision for the precision control and thus,
+ # the robot might slow down when reaching a path point, but it stays very
+ # close to the original path
+ delta = 0.05
+ if(indCntr == (len(path)-1)):
+ delta = 0.01
+ #Control the turtlebot to drive towards the target position
+ isNearGoal = turtlebot.drive_goalPose(delta=delta)
+ #If the robot is close to the goal select the next goal position
+ if (isNearGoal):
+ indCntr += 1
+ i = min(indCntr, len(path))
+ if i == len(path):
+ indCntr = -1
+ else:
+ #print("Aktuelle Position Pfad: ", path[i])
+ turtlebot.setPoseGoal(path[i])
+ return indCntr
+
+def planInitialPath(turtle):
+
+ initialObstacles = findObstaclePositions(turtle)
+ initialPose = findRobotPosition(turtle)
+ initialFinalTarget = findTargetPosition(turtle)
+
+ plt.scatter(np.array(initialObstacles)[..., 0], np.array(initialObstacles)[..., 1])
+ plt.scatter(np.array(initialPose)[..., 0], np.array(initialPose)[..., 1] )
+ plt.scatter(np.array(initialFinalTarget)[..., 0], np.array(initialFinalTarget)[..., 1])
+ plt.show()
+ plt.pause(0.001)
+
+ from matplotlib import pyplot as ppl
+ from matplotlib import cm
+ import random, sys, math, os.path
+ from matplotlib.pyplot import imread
+
+ #Implementation of RRT
+
+ MAP_IMG = './karte.png' #Black and white image for a map
+ MIN_NUM_VERT = 20 # Minimum number of vertex in the graph
+ MAX_NUM_VERT = 1500 # Maximum number of vertex in the graph
+ STEP_DISTANCE = 20 # Maximum distance between two vertex
+ SEED = None # For random numbers
+
+ def rapidlyExploringRandomTree(ax, img, start, goal, seed=None):
+ hundreds = 100
+ seed = random.seed(seed)
+ #print("Zufallsseed: ", seed)
+ points = []
+ graph = []
+ points.append(start)
+ graph.append((start, []))
+ print('Generating and conecting random points')
+ occupied = True
+ phaseTwo = False
+
+ # Phase two values (points 5 step distances around the goal point)
+ minX = max(goal[0] - 5 * STEP_DISTANCE, 0)
+ maxX = min(goal[0] + 5 * STEP_DISTANCE, len(img[0]) - 1)
+ minY = max(goal[1] - 5 * STEP_DISTANCE, 0)
+ maxY = min(goal[1] + 5 * STEP_DISTANCE, len(img) - 1)
+
+ i = 0
+ while (goal not in points) and (len(points) < MAX_NUM_VERT):
+ if (i % 100) == 0:
+ print(i, 'points randomly generated')
+
+ if (len(points) % hundreds) == 0:
+ print(len(points), 'vertex generated')
+ hundreds = hundreds + 100
+
+ while (occupied):
+ if phaseTwo and (random.random() > 0.8):
+ point = [random.randint(minX, maxX), random.randint(minY, maxY)]
+ else:
+ point = [random.randint(0, len(img[0]) - 1), random.randint(0, len(img) - 1)]
+
+ if (img[point[1]][point[0]][0] * 255 == 255):
+ occupied = False
+
+ occupied = True
+
+ nearest = findNearestPoint(points, point)
+ newPoints = connectPoints(point, nearest, img)
+ addToGraph(ax, graph, newPoints, point)
+ newPoints.pop(0) # The first element is already in the points list
+ points.extend(newPoints)
+ ppl.draw()
+ i = i + 1
+
+ if len(points) >= MIN_NUM_VERT:
+ if not phaseTwo:
+ print('Phase Two')
+ phaseTwo = True
+
+ if phaseTwo:
+ nearest = findNearestPoint(points, goal)
+ newPoints = connectPoints(goal, nearest, img)
+ addToGraph(ax, graph, newPoints, goal)
+ newPoints.pop(0)
+ points.extend(newPoints)
+ ppl.draw()
+
+ if goal in points:
+ print('Goal found, total vertex in graph:', len(points), 'total random points generated:', i)
+
+ path = searchPath(graph, start, [start])
+
+ for i in range(len(path) - 1):
+ ax.plot([path[i][0], path[i + 1][0]], [path[i][1], path[i + 1][1]], color='g', linestyle='-',
+ linewidth=2)
+ ppl.draw()
+
+ print('Showing resulting map')
+ print('Final path:', path)
+ print('The final path is made from:', len(path), 'connected points')
+ else:
+ path = None
+ print('Reached maximum number of vertex and goal was not found')
+ print('Total vertex in graph:', len(points), 'total random points generated:', i)
+ print('Showing resulting map')
+
+ ppl.show()
+ return path
+
+ def searchPath(graph, point, path):
+ for i in graph:
+ if point == i[0]:
+ p = i
+
+ if p[0] == graph[-1][0]:
+ return path
+
+ for link in p[1]:
+ path.append(link)
+ finalPath = searchPath(graph, link, path)
+
+ if finalPath != None:
+ return finalPath
+ else:
+ path.pop()
+
+ def addToGraph(ax, graph, newPoints, point):
+ if len(newPoints) > 1: # If there is anything to add to the graph
+ for p in range(len(newPoints) - 1):
+ nearest = [nearest for nearest in graph if (nearest[0] == [newPoints[p][0], newPoints[p][1]])]
+ nearest[0][1].append(newPoints[p + 1])
+ graph.append((newPoints[p + 1], []))
+
+ if not p == 0:
+ ax.plot(newPoints[p][0], newPoints[p][1], '+k') # First point is already painted
+ ax.plot([newPoints[p][0], newPoints[p + 1][0]], [newPoints[p][1], newPoints[p + 1][1]], color='k',
+ linestyle='-', linewidth=1)
+
+ if point in newPoints:
+ ax.plot(point[0], point[1], '.g') # Last point is green
+ else:
+ ax.plot(newPoints[p + 1][0], newPoints[p + 1][1], '+k') # Last point is not green
+
+ def connectPoints(a, b, img):
+ newPoints = []
+ newPoints.append([b[0], b[1]])
+ step = [(a[0] - b[0]) / float(STEP_DISTANCE), (a[1] - b[1]) / float(STEP_DISTANCE)]
+
+ # Set small steps to check for walls
+ pointsNeeded = int(math.floor(max(math.fabs(step[0]), math.fabs(step[1]))))
+
+ if math.fabs(step[0]) > math.fabs(step[1]):
+ if step[0] >= 0:
+ step = [1, step[1] / math.fabs(step[0])]
+ else:
+ step = [-1, step[1] / math.fabs(step[0])]
+
+ else:
+ if step[1] >= 0:
+ step = [step[0] / math.fabs(step[1]), 1]
+ else:
+ step = [step[0] / math.fabs(step[1]), -1]
+
+ blocked = False
+ for i in range(pointsNeeded + 1): # Creates points between graph and solitary point
+ for j in range(STEP_DISTANCE): # Check if there are walls between points
+ coordX = round(newPoints[i][0] + step[0] * j)
+ coordY = round(newPoints[i][1] + step[1] * j)
+
+ if coordX == a[0] and coordY == a[1]:
+ break
+ if coordY >= len(img) or coordX >= len(img[0]):
+ break
+ if img[int(coordY)][int(coordX)][0] * 255 < 255:
+ blocked = True
+ if blocked:
+ break
+
+ if blocked:
+ break
+ if not (coordX == a[0] and coordY == a[1]):
+ newPoints.append(
+ [newPoints[i][0] + (step[0] * STEP_DISTANCE), newPoints[i][1] + (step[1] * STEP_DISTANCE)])
+
+ if not blocked:
+ newPoints.append([a[0], a[1]])
+ return newPoints
+
+ def findNearestPoint(points, point):
+ best = (sys.maxsize, sys.maxsize, sys.maxsize)
+ for p in points:
+ if p == point:
+ continue
+ dist = math.sqrt((p[0] - point[0]) ** 2 + (p[1] - point[1]) ** 2)
+ if dist < best[2]:
+ best = (p[0], p[1], dist)
+ return (best[0], best[1])
+
+ karte = np.ones([80, 80, 3], dtype=int)
+
+ # Koordinatentransformation
+ initialObstacles = [[int(x[0] * 10), int(x[1] * 10)] for x in initialObstacles]
+ initialPose = [initialPose[0] * 10, initialPose[1] * 10]
+ initialFinalTarget = [initialFinalTarget[0] * 10, initialFinalTarget[1] * 10]
+
+ initialObstacles = [[int(x[0] + 40), int(x[1] - 40)] for x in initialObstacles]
+ initialPose = [initialPose[0] + 40, initialPose[1] - 40]
+ initialFinalTarget = [initialFinalTarget[0] + 40, initialFinalTarget[1] - 40]
+
+ initialObstacles = [[int(x[0]), int(x[1] * (-1))] for x in initialObstacles]
+ initialPose = [int(initialPose[0]), int(initialPose[1] * -1)]
+ initialFinalTarget = [int(initialFinalTarget[0]), int(initialFinalTarget[1] * -1)]
+
+ # Karte Hindernisse einzeichnen
+ rot = np.array(0, dtype=int)
+ gruen = np.array(0, dtype=int)
+ blau = np.array(0, dtype=int)
+
+ for k in initialObstacles:
+ karte[k[1], k[0], 0] = rot;
+ karte[k[1], k[0], 1] = gruen;
+ karte[k[1], k[0], 2] = blau;
+
+ karte[k[1] + 1, k[0] + 0, 0] = rot;
+ karte[k[1] + 1, k[0] + 0, 1] = gruen;
+ karte[k[1] + 1, k[0] + 0, 2] = blau;
+
+ karte[k[1] - 0, k[0] + 1, 0] = rot;
+ karte[k[1] - 0, k[0] + 1, 1] = gruen;
+ karte[k[1] - 0, k[0] + 1, 2] = blau;
+
+ karte[k[1] + 0, k[0] - 1, 0] = rot;
+ karte[k[1] + 0, k[0] - 1, 1] = gruen;
+ karte[k[1] + 0, k[0] - 1, 2] = blau;
+
+ karte[k[1] - 1, k[0] - 0, 0] = rot;
+ karte[k[1] - 1, k[0] - 0, 1] = gruen;
+ karte[k[1] - 1, k[0] - 0, 2] = blau;
+
+ #Plotten als PNG speichern
+ from PIL import Image
+ im = Image.fromarray((karte * 255).astype(np.uint8))
+ im.save('karte.png')
+
+ print('Loading map... with file \'', MAP_IMG, '\'')
+ img = imread(MAP_IMG)
+ fig = ppl.gcf()
+ fig.clf()
+ ax = fig.add_subplot(1, 1, 1)
+ ax.imshow(img, cmap=cm.Greys_r)
+ ax.axis('image')
+ ppl.draw()
+ print('Map is', len(img[0]), 'x', len(img))
+ start = initialPose
+ goal = initialFinalTarget
+
+ path = rapidlyExploringRandomTree(ax, img, start, goal, seed=SEED)
+
+ # Ruecktransformation
+ path1 = [[x[0], x[1] * (-1)] for x in path]
+ path2 = [[(x[0] - 40), (x[1] + 40)] for x in path1]
+ path3 = [[(x[0] / 10), (x[1] / 10)] for x in path2]
+
+ path3 = [[(x[0]), x[1], 0, 0, 0, 0, 0] for x in path3]
+
+ print("Umgerechneter Pfad", path3)
+
+ if len(sys.argv) > 2:
+ print('Only one argument is needed')
+ elif len(sys.argv) > 1:
+ if os.path.isfile(sys.argv[1]):
+ MAP_IMG = sys.argv[1]
+ else:
+ print(sys.argv[1], 'is not a file')
+
+ pathIndCntr = 0
+
+ return path3, pathIndCntr
+
+#=============[Evaluation functions]===========================================
+collisionCntr = 0
+recalcCntr = 0
+def updateEval_RobotCollisions(detector):
+ global collisionCntr
+ collision = np.nan_to_num(detector.getValue(), nan=0)
+ collisionCntr += int(collision)
+ return bool(collision)
+
+#======================================[MAIN Execution]========================
+if __name__ == "__main__":
+ plotting = False
+ nPlot = 80
+ festgefahren = 0
+ #Only increase this value if the simulation is running slowly
+ TIME_STEP = 64
+
+ #%%=============[DO NOT CHANGE THIS PART OF THE SCRIPT]==================
+ loopCntr = 0 #
+ turtle = rc.turtlebotController(TIME_STEP,
+ poseGoal_tcs=np.array([-0.863, -2.0, 0.0, 0.0, 0.0, 1.0, 3.14159])) #
+ detector = turtle.robot.getDevice("collisionDetector") #
+ detector.enable(TIME_STEP) #
+ #
+ #The map will be initialized after the first timestep (Transmission) #
+ turtle.robot.step(turtle.TIME_STEP) #
+ turtle.update() #
+
+ #The map will have the dynamic objects after the second timestep #
+ turtle.robot.step(turtle.TIME_STEP) #
+ turtle.update() #
+ ScenarioFinalTarget = findTargetPosition(turtle) #
+ #
+ if plotting:
+ mg.plotting_GM_plotly(turtle.mapGenerator)
+ #DO NOT CHANGE OR REMOVE THIS FUNCTION CALL AND POSITION #
+ waitRandomTimeSteps(turtle) #
+ #===============[From here you can change the script]====================
+ #%%
+ #here you should insert a function, which plans a path; the functions has to return a path and related index
+ try:
+ path, pathIndCntr = planInitialPath(turtle)
+ except TypeError:
+ print("Erste Pfadplanung nicht erfolgreich, neustart...")
+ try:
+ path, pathIndCntr = planInitialPath(turtle)
+ except TypeError:
+ print("Zweite Pfadplanung nicht erfolgreich, neustart...")
+ try:
+ path, pathIndCntr = planInitialPath(turtle)
+ except TypeError:
+ print("Dritte Pfadplanung nicht erfolgreich, neustart...")
+ exit()
+
+ turtle.setPoseGoal(path[pathIndCntr])
+ startTime = turtle.robot.getTime()
+
+ inhibitCntr = 0
+ while turtle.robot.step(turtle.TIME_STEP) != -1:
+ #%%===[DO NOT CHANGE THIS PART OF THE LOOP]==========================
+ loopCntr += 1 #
+ turtle.update()
+
+ collision = checkForPossibleCollisions(turtle, findRobotPosition(turtle), collisionCntr)
+
+ if ( (plotting) & (loopCntr % nPlot == 0)): #
+ mg.plotting_GM_plotly(turtle.mapGenerator) #
+ #====[From here you can change the loop]=============================
+ #%%
+ #Follow the static path towards the index pathIndCntr in the path
+ inhibitCntr = max(0, inhibitCntr - 1)
+
+ if(collision == False):
+ pathIndCntr = staticPathTracking(turtle, path, pathIndCntr)
+
+ if(collision == True):
+ print("Knallt")
+ turtle.stop()
+ festgefahren += 1
+
+ if(festgefahren == 10):
+ #Berechnen neue Pos
+ vorherigePos = path[pathIndCntr-1]
+ neueRichtung = [vorherigePos[0] / 2, vorherigePos[1] / 2, 0, 0, 0, 0, 0]
+ path.insert(pathIndCntr, neueRichtung)
+
+ #Fahre neue Pos an bis du da bist
+ #Wenn du da bist festgefahren 0 und hoffentlich keine Collsion mehr
+ recalcCntr += 1
+ inhibitCntr = 15
+ festgefahren = 0
+ collision = False
+
+ #This is the exit condition. Changing this is not recommended
+ if(pathIndCntr == -1):
+ break
+ #End while
+ #%%
+ print("Simulation ended")
+ print("Timesteps with collisions: ", collisionCntr)
+ print("Recalculations done: ", recalcCntr)
+ print("Simulationtime: ", turtle.robot.getTime() - startTime)
+ print("")
+ print("Recalculation")
+ print("Timesteps with collisions: ", collisionCntr)
+ print("Recalculations done: ", recalcCntr)
+ print("Simulationtime: ", turtle.robot.getTime() - startTime)
+
+ lastStateObstacles = np.array(findObstaclePositions(turtle))
+ lastStateRobotPose = np.array(findRobotPosition(turtle))
+
+ trackRobot = np.array(turtle.trackRobot)
+ plt.scatter(lastStateObstacles[..., 0], lastStateObstacles[..., 1])
+ plt.scatter(lastStateRobotPose[..., 0], lastStateRobotPose[..., 1])
+
+ plt.plot(trackRobot[..., 0], trackRobot[..., 1])
+ plt.scatter(turtle.poseGoal[0], turtle.poseGoal[1])
+ plt.scatter(turtle.mapGenerator.finalTarget[0], turtle.mapGenerator.finalTarget[1])
+ plt.show()
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/map.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/map.py
new file mode 100644
index 0000000000000000000000000000000000000000..379645de42c7898a6d3e846c21e3c8bf996ac70a
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/map.py
@@ -0,0 +1,24 @@
+#Map erstellen
+
+# Standard imports
+import cv2
+import numpy as np;
+
+# Img Objekt
+cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+img = cap.read()
+#height, width = img.shape[:2]
+
+success, img = cap.read()
+gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
+mask = cv2.inRange(gray, np.array([90]), np.array([255]))
+
+mask[mask == 255] = 87
+
+mask = np.array(mask, dtype=np.uint8).view('S2').squeeze()
+
+mask[mask == b'WW'] = 'W'
+
+print(mask)
+
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/maperstellen.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/maperstellen.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node copy.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node copy.py
new file mode 100644
index 0000000000000000000000000000000000000000..29f2473b20a9c782704e15851518da8ec90ee823
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node copy.py
@@ -0,0 +1,183 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import json
+from sensor_msgs.msg import Imu
+import threading
+from std_msgs.msg import String
+from geometry_msgs.msg import Quaternion, Vector3
+from box import Box
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import datetime
+
+
+class MyNode(Node):
+
+ def __init__(self):
+ super().__init__("sphero_mini")
+ self.publisher_ = self.create_publisher(String,"Imu",5)
+ #self.get_logger().info("Hello from ROS2")
+
+ def connect(self):
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ #print(i)
+
+ #self.publisher_.publish(i)
+
+
+
+def main(args = None):
+
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ rate = node.create_rate(2)
+
+ try:
+ while rclpy.ok():
+
+ sphero.configureSensorMask(
+ IMU_yaw=True,
+ IMU_pitch=True,
+ IMU_roll=True,
+ IMU_acc_y=True,
+ IMU_acc_z=True,
+ IMU_acc_x=True,
+ IMU_gyro_x=True,
+ IMU_gyro_y=True,
+ #IMU_gyro_z=True
+ )
+ sphero.configureSensorStream()
+
+ sensors_values = node.get_sensors_data(sphero)
+ #rclpy.logdebug(sensors_values)
+ node.publish_imu(sensors_values, node)
+ sphero.setLEDColor(red = 0, green = 255, blue = 0) # Turn LEDs green
+
+ # Aiming:
+ sphero.setLEDColor(red = 0, green = 0, blue = 0) # Turn main LED off
+ sphero.stabilization(False) # Turn off stabilization
+ sphero.setBackLEDIntensity(255) # turn back LED on
+ sphero.wait(3) # Non-blocking pau`se
+ sphero.resetHeading() # Reset heading
+ sphero.stabilization(True) # Turn on stabilization
+ sphero.setBackLEDIntensity(0) # Turn back LED off
+
+ # Move around:
+ sphero.setLEDColor(red = 0, green = 0, blue = 255) # Turn main LED blue
+ sphero.roll(100, 0) # roll forwards (heading = 0) at speed = 50
+
+ sphero.wait(3) # Keep rolling for three seconds
+
+ sphero.roll(0, 0) # stop
+ sphero.wait(1) # Allow time to stop
+
+ sphero.setLEDColor(red = 0, green = 255, blue = 0) # Turn main LED green
+ sphero.roll(-100, 0) # Keep facing forwards but roll backwards at speed = 50
+ sphero.wait(3) # Keep rolling for three seconds
+
+ sphero.roll(0, 0) # stop
+ sphero.wait(1) # Allow time to stop
+
+ rate.sleep()
+
+ except KeyboardInterrupt:
+ pass
+
+ rclpy.shutdown()
+ thread.join()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node.py
new file mode 100644
index 0000000000000000000000000000000000000000..8906b23876454b93bf060523bda262e61363cdd2
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node.py
@@ -0,0 +1,766 @@
+#!/usr/bin/env python3
+############################################################################
+#%% Imports
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+import threading
+from geometry_msgs.msg import Quaternion, Vector3
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import cv2
+import time
+import queue
+import matplotlib.pyplot as plt
+############################################################################
+#%% Blob Detector / Globale Variablen
+detector = cv2.SimpleBlobDetector()
+
+# Setup SimpleBlobDetector parameters.
+params = cv2.SimpleBlobDetector_Params()
+
+# Change thresholds
+#params.minThreshold = 5
+#params.maxThreshold = 500
+
+#FIlter by Color
+params.filterByColor = True
+params.blobColor = 255
+
+# Filter by Area.
+params.filterByArea = True
+params.minArea = 10
+params.maxArea = 200
+
+# Filter by Circularity
+#params.filterByCircularity = True
+#params.minCircularity = 0.5
+#params.maxCircularity = 1
+
+# Filter by Convexity
+#params.filterByConvexity = True
+#params.minConvexity = 0.7
+#params.maxConvexity = 1
+
+# Filter by Inertia
+#params.filterByInertia = True
+#params.minInertiaRatio = 0.01
+
+# Create a detector with the parameters
+detector = cv2.SimpleBlobDetector_create(params)
+############################################################################
+#%% Class Video Capture
+# Bufferless VideoCapture
+# Quelle ?
+class VideoCapture:
+ def __init__(self, name):
+ self.cap = cv2.VideoCapture(name)
+ self.q = queue.Queue()
+ t = threading.Thread(target=self._reader)
+ t.daemon = True
+ t.start()
+ # read frames as soon as they are available, keeping only most recent one
+ def _reader(self):
+ while True:
+ ret, frame = self.cap.read()
+ #cv2.imshow("Cap", frame)
+
+ if not ret:
+ break
+ if not self.q.empty():
+ try:
+ self.q.get_nowait() # discard previous (unprocessed) frame
+ except queue.Empty:
+ pass
+ self.q.put(frame)
+
+ def read(self):
+ return self.q.get()
+############################################################################
+#%% Class Sphero Node
+class MyNode(Node):
+ def __init__(self):
+ super().__init__("sphero_mini")
+
+ def connect(self):
+ #Automatisch MAC-Adresse laden
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ def get_pos(self, cap, height):
+ #zeitanfang = time.time()
+
+ success = False
+
+ while(success == False):
+ try:
+ img = cap.read()
+
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+ keypoints = detector.detect(gray)
+
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+ success = True
+
+ xTrans, yTrans = self.calc_trans(x,y, height)
+ except Exception:
+ continue
+
+ #print("Blob X: %f Blob Y: %f" %(x,y))
+ #zeitende = time.time()
+ #print("Dauer Programmausführung:",(zeitende-zeitanfang))
+ #print("Get_Pos: X,Y:", xTrans,yTrans)
+
+ return xTrans,yTrans
+
+ def calc_offset(self, sphero, cap, height):
+ sphero.roll(100,0)
+ sphero.wait(1)
+ sphero.roll(0,0)
+ sphero.wait(0.5)
+
+ ref = self.get_pos(cap, height)
+ print("calc_offset: Ref Punkt x,y", ref)
+
+ sphero.wait(1)
+ sphero.roll(100,180)
+ sphero.wait(1)
+ sphero.roll(0,0)
+
+ startpunkt = self.get_pos(cap, height)
+ print("calc_offset: Startpunkt x,y", startpunkt)
+
+ ref = np.array(ref)
+ startpunkt = np.array(startpunkt)
+
+ start_ref = ref-startpunkt
+
+ phi = np.arctan2(start_ref[1],start_ref[0])
+ phi = np.degrees(phi)
+ phi = int(-phi)
+
+ print("Phi ohne alles:", phi)
+
+ phi = self.phi_in_range(phi)
+
+ print("Calc_Offset: ", phi)
+
+ return phi
+
+ def calc_av(self,istPos, sollPos, cap, height):
+ startPos = istPos
+
+ startPos = np.array(startPos)
+ sollPos = np.array(sollPos)
+
+ pktSpheroKord = sollPos - startPos
+
+ phi = np.arctan2(pktSpheroKord[1], pktSpheroKord[0])
+ phi = np.degrees(phi)
+
+ phiZiel = int(phi)
+
+ phiZiel = self.phi_in_range(phiZiel)
+
+ v = 100
+
+ #print("Calc_AV: a,v", phiZiel, v)
+
+ return phiZiel, v
+
+ def drive_to(self,sphero, targetPos, aOffset, cap, height):
+ dmin = 20
+ pos = self.get_pos(cap, height)
+ pos = np.array(pos)
+
+ diff = targetPos - pos
+
+ d = np.linalg.norm(diff, ord = 2)
+
+ ar = []
+ ar.append(0)
+
+ i = 1
+
+ while d > dmin:
+ a,v = self.calc_av(pos,targetPos, cap, height)
+
+ aR = -aOffset - a #Fallunterscheidung?
+ ar.append((self.phi_in_range(aR)))
+
+ #Fahrbefehl
+ if(ar[i] != ar[i-1]):
+ sphero.roll(v, ar[i])
+ sphero.wait(0.05)
+ else:
+ sphero.wait(0.05)
+
+ #Aktuelle Pos
+ pos = self.get_pos(cap, height)
+ pos = np.array(pos)
+
+ #Abweichung
+ diff = targetPos - pos
+ diff = np.array(diff)
+ d = np.linalg.norm(diff, ord = 2)
+
+ i = i + 1
+
+ sphero.roll(0,0)
+ print("Ziel Erreicht")
+
+ return
+
+ def calc_trans(self,x,y, height):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+
+ def phi_in_range(self,phi):
+ while(phi < 0):
+ phi = phi + 360
+ while(phi > 360):
+ phi = phi - 360
+ return phi
+############################################################################
+#%% Class Wavefrontplanner
+class waveFrontPlanner:
+ ############################################################################
+ # WAVEFRONT ALGORITHM
+ # Adapted to Python Code By Darin Velarde
+ # Fri Jan 29 13:56:53 PST 2010
+ # from C code from John Palmisano
+ # (www.societyofrobots.com)
+ ############################################################################
+
+ def __init__(self, mapOfWorld, slow=False):
+ self.__slow = slow
+ self.__mapOfWorld = mapOfWorld
+ if str(type(mapOfWorld)).find("numpy") != -1:
+ #If its a numpy array
+ self.__height, self.__width = self.__mapOfWorld.shape
+ else:
+ self.__height, self.__width = len(self.__mapOfWorld), len(self.__mapOfWorld[0])
+
+ self.__nothing = 000
+ self.__wall = 999
+ self.__goal = 1
+ self.__path = "PATH"
+
+ self.__finalPath = []
+
+ #Robot value
+ self.__robot = 254
+ #Robot default Location
+ self.__robot_x = 0
+ self.__robot_y = 0
+
+ #default goal location
+ self.__goal_x = 8
+ self.__goal_y = 9
+
+ #temp variables
+ self.__temp_A = 0
+ self.__temp_B = 0
+ self.__counter = 0
+ self.__steps = 0 #determine how processor intensive the algorithm was
+
+ #when searching for a node with a lower value
+ self.__minimum_node = 250
+ self.__min_node_location = 250
+ self.__new_state = 1
+ self.__old_state = 1
+ self.__reset_min = 250 #above this number is a special (wall or robot)
+ ###########################################################################
+
+ def setRobotPosition(self, x, y):
+ """
+ Sets the robot's current position
+
+ """
+
+ self.__robot_x = x
+ self.__robot_y = y
+ ###########################################################################
+
+ def setGoalPosition(self, x, y):
+ """
+ Sets the goal position.
+
+ """
+
+ self.__goal_x = x
+ self.__goal_y = y
+ ###########################################################################
+
+ def robotPosition(self):
+ return (self.__robot_x, self.__robot_y)
+ ###########################################################################
+
+ def goalPosition(self):
+ return (self.__goal_x, self.__goal_y)
+ ###########################################################################
+
+ def run(self, prnt=False):
+ """
+ The entry point for the robot algorithm to use wavefront propagation.
+
+ """
+
+ path = []
+ while self.__mapOfWorld[self.__robot_x][self.__robot_y] != self.__goal:
+ if self.__steps > 10000:
+ print ("Cannot find a path.")
+ return
+ #find new location to go to
+ self.__new_state = self.propagateWavefront()
+ #update location of robot
+ if self.__new_state == 1:
+ self.__robot_x -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" % (self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 2:
+ self.__robot_y += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 3:
+ self.__robot_x += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 4:
+ self.__robot_y -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ self.__old_state = self.__new_state
+ msg = "Found the goal in %i steps:\n" % self.__steps
+ msg += "mapOfWorld size= %i %i\n\n" % (self.__height, self.__width)
+ if prnt:
+ print(msg)
+ self.printMap()
+ return path
+ ###########################################################################
+
+ def propagateWavefront(self, prnt=False):
+ self.unpropagate()
+ #Old robot location was deleted, store new robot location in mapOfWorld
+ self.__mapOfWorld[self.__robot_x][self.__robot_y] = self.__robot
+ self.__path = self.__robot
+ #start location to begin scan at goal location
+ self.__mapOfWorld[self.__goal_x][self.__goal_y] = self.__goal
+ counter = 0
+ while counter < 200: #allows for recycling until robot is found
+ x = 0
+ y = 0
+ #time.sleep(0.00001)
+ #while the mapOfWorld hasnt been fully scanned
+ while x < self.__height and y < self.__width:
+ #if this location is a wall or the goal, just ignore it
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal:
+ #a full trail to the robot has been located, finished!
+ minLoc = self.minSurroundingNodeValue(x, y)
+ if minLoc < self.__reset_min and \
+ self.__mapOfWorld[x][y] == self.__robot:
+ if prnt:
+ print("Finished Wavefront:\n")
+ self.printMap()
+ # Tell the robot to move after this return.
+ return self.__min_node_location
+ #record a value in to this node
+ elif self.__minimum_node != self.__reset_min:
+ #if this isnt here, 'nothing' will go in the location
+ self.__mapOfWorld[x][y] = self.__minimum_node + 1
+ #go to next node and/or row
+ y += 1
+ if y == self.__width and x != self.__height:
+ x += 1
+ y = 0
+ #print self.__robot_x, self.__robot_y
+ if prnt:
+ print("Sweep #: %i\n" % (counter + 1))
+ self.printMap()
+ self.__steps += 1
+ counter += 1
+ return 0
+ ###########################################################################
+
+ def unpropagate(self):
+ """
+ clears old path to determine new path
+ stay within boundary
+
+ """
+
+ for x in range(0, self.__height):
+ for y in range(0, self.__width):
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal and \
+ self.__mapOfWorld[x][y] != self.__path:
+ #if this location is a wall or goal, just ignore it
+ self.__mapOfWorld[x][y] = self.__nothing #clear that space
+ ###########################################################################
+
+ def minSurroundingNodeValue(self, x, y):
+ """
+ this method looks at a node and returns the lowest value around that
+ node.
+
+ """
+
+ #reset minimum
+ self.__minimum_node = self.__reset_min
+ #down
+ if x < self.__height -1:
+ if self.__mapOfWorld[x + 1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x + 1][y] != self.__nothing:
+ #find the lowest number node, and exclude empty nodes (0's)
+ self.__minimum_node = self.__mapOfWorld[x + 1][y]
+ self.__min_node_location = 3
+ #up
+ if x > 0:
+ if self.__mapOfWorld[x-1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x-1][y] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x-1][y]
+ self.__min_node_location = 1
+ #right
+ if y < self.__width -1:
+ if self.__mapOfWorld[x][y + 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y + 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y + 1]
+ self.__min_node_location = 2
+ #left
+ if y > 0:
+ if self.__mapOfWorld[x][y - 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y - 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y-1]
+ self.__min_node_location = 4
+ return self.__minimum_node
+ ###########################################################################
+
+ def printMap(self):
+ """
+ Prints out the map of this instance of the class.
+
+ """
+
+ msg = ''
+ for temp_B in range(0, self.__height):
+ for temp_A in range(0, self.__width):
+ if self.__mapOfWorld[temp_B][temp_A] == self.__wall:
+ msg += "%04s" % "[#]"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__robot:
+ msg += "%04s" % "-"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__goal:
+ msg += "%04s" % "G"
+ else:
+ msg += "%04s" % str(self.__mapOfWorld[temp_B][temp_A])
+ msg += "\n\n"
+ msg += "\n\n"
+ print(msg)
+ #
+ if self.__slow == True:
+ time.sleep(0.05)
+
+ def plotMap(self,img,path):
+ #Programm Koordinaten
+ print("Plotten")
+
+ imgTrans = cv2.transpose(img) # X und Y-Achse im Bild tauschen
+ imgPlot, ax1 = plt.subplots()
+
+ ax1.set_title('Programm Koordinaten')
+ ax1.imshow(imgTrans)
+ ax1.set_xlabel('[px]')
+ ax1.set_ylabel('[px]')
+ ax1.scatter(path[:,0], path[:,1], color='r')
+
+ #Bild Koordinaten
+ imgPlot2, ax2 = plt.subplots()
+ ax2.set_title('Bild Koordinaten')
+ ax2.set_xlabel('[px]')
+ ax2.set_ylabel('[px]')
+ ax2.imshow(img)
+ ax2.scatter(path[:,1], path[:,0], color='r')
+
+ plt.show()
+
+ return
+
+ def getPathWelt(self,path,height,Mapobj):
+
+ pathWeltX, pathWeltY = Mapobj.calc_trans_welt(path[:,1], path[:,0], height)
+ pathEckenX = []
+ pathEckenY = []
+
+ for i,px in enumerate(pathWeltX):
+ if (i < len(pathWeltX)-1) & (i > 0):
+ if px != pathWeltX[i+1]:
+ if pathWeltY[i]!=pathWeltY[i-1]:
+ pathEckenX.append(px)
+ pathEckenY.append(pathWeltY[i])
+ if pathWeltY[i] != pathWeltY[i+1]:
+ if pathWeltX[i]!=pathWeltX[i-1]:
+ pathEckenX.append(px)
+ pathEckenY.append(pathWeltY[i])
+
+ pathEckenX.append(pathWeltX[-1])
+ pathEckenY.append(pathWeltY[-1])
+
+
+ uebergabePath = []
+ for i in range(0,len(pathEckenX)):
+ uebergabePath.append((pathEckenX[i],pathEckenY[i]))
+
+ print("Ecken: ", uebergabePath)
+
+ return uebergabePath
+
+############################################################################
+#%% Class Map
+class mapCreate:
+ ############################################################################
+
+ def create_map(self, scale, cap):
+
+ # Map erstellen
+
+ # Img Objekt
+ #cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+ img = cap.read()
+
+ #Karte von Bild
+ #img = cv2.imread("D:/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/map/map.jpg")
+ print('Original Dimensions : ',img.shape)
+
+ scale_percent = 100-scale # percent of original size
+ width = int(img.shape[1] * scale_percent / 100)
+ height = int(img.shape[0] * scale_percent / 100)
+ dim = (width, height)
+
+ # resize image
+ resized = cv2.resize(img, dim, interpolation = cv2.INTER_AREA)
+ print('Resized Dimensions : ',resized.shape)
+
+ gray = cv2.cvtColor(resized, cv2.COLOR_RGB2GRAY)
+ mask = cv2.inRange(gray, np.array([50]), np.array([255]))
+
+ plt.plot(mask)
+
+ mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+ mask_list= np.ndarray.tolist(mask)
+
+ #Markiere alle leeren Zellen mit 000 und alle Zellen mit Hinderniss mit 999
+ for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['999' if j > 200 else '000' for j in mask_list[i]]
+
+ mapOfWorld = np.array(mapOfWorld, dtype = int)
+ mapOfWorldSized = mapOfWorld.copy()
+
+ e = 1
+ #Hindernisse Größer machen
+ for i in range(0,mapOfWorld.shape[0]):
+ for j in range(0,mapOfWorld.shape[1]):
+ for r in range(0,e):
+ try:
+ if (mapOfWorldSized[i][j] == 999):
+ mapOfWorld[i][j+e] = 999
+ mapOfWorld[i+e][j] = 999
+ mapOfWorld[i-e][j] = 999
+ mapOfWorld[i][j-e] = 999
+ mapOfWorld[i+e][j+e] = 999
+ mapOfWorld[i+e][j-e] = 999
+ mapOfWorld[i-e][j+e] = 999
+ mapOfWorld[i-e][j-e] = 999
+ except Exception:
+ continue
+
+ return mapOfWorld,img
+ ############################################################################
+
+ def scale_koord(self, sx, sy, gx, gy,scale):
+ scale_percent = 100-scale # percent of original size
+
+ sx = int(sx*scale_percent/100)
+ sy = int(sy*scale_percent/100)
+ gx = int(gx*scale_percent/100)
+ gy = int(gy*scale_percent/100)
+
+ return sx,sy,gx,gy
+ ############################################################################
+
+ def rescale_koord(self, path,scale):
+ scale_percent = 100-scale # percent of original size100
+
+ path = np.array(path)
+
+ for i in range(len(path)):
+ path[i] = path[i]*100/scale_percent
+
+ return path
+ ############################################################################
+
+ def calc_trans_welt(self, x, y, height):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+###############################################################################
+#%% Main Funktion
+def main(args = None):
+ #Verbindung herstellen, Node erstellen
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ #rate = node.create_rate(5)
+
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ #cap = VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+ cap = VideoCapture(4)
+ Map = mapCreate()
+
+ img = cap.read()
+ height, width = img.shape[:2]
+
+ scale = 90
+
+ sphero.setLEDColor(255,0,0)
+ sphero.wait(1)
+ sphero.setBackLEDIntensity(0)
+ sphero.stabilization(True)
+
+ aOffset = node.calc_offset(sphero, cap, height)
+
+ sphero.wait(1)
+
+ #while(True):
+
+ mapWorld,img = Map.create_map(scale, cap)
+ height, width = img.shape[:2]
+
+ #Befehle für WaveFrontPlanner/Maperstellung
+ sy,sx = node.get_pos(cap,height) #Aktuelle Roboter Pos in Welt
+ sy, sx = node.calc_trans(sy,sx,height)
+
+ gy = int(input("Bitte geben Sie die X-Zielkoordinate im Bild Koordinatensystem ein:")) #X-Koordinate im Weltkoordinaten System
+ gx = int(input("Bitte geben Sie die Y-Zielkoordinate im Bild Koordinatensystem ein:")) #Y-Koordinate im Weltkordinaten System
+
+ sx,sy,gx,gy = Map.scale_koord(sx,sy,gx,gy,scale) #Kordinaten Tauschen X,Y
+
+ start = time.time()
+ planner = waveFrontPlanner(mapWorld)
+ planner.setGoalPosition(gx,gy)
+ planner.setRobotPosition(sx,sy)
+ path = planner.run(False)
+ end = time.time()
+ print("Took %f seconds to run wavefront simulation" % (end - start))
+
+ path = Map.rescale_koord(path, scale)
+
+ planner.plotMap(img,path)
+
+ pathWelt = planner.getPathWelt(path,height,Map)
+
+ for p in pathWelt:
+ node.drive_to(sphero,p,aOffset,cap, height)
+ sphero.wait(1)
+
+ print("Geschafft")
+
+ #if cv2.waitKey(10) & 0xFF == ord('q'):
+ # break
+
+ rclpy.shutdown()
+############################################################################
+#%% Main
+if __name__ == '__main__':
+ main()
+############################################################################
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_alt.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_alt.py
new file mode 100644
index 0000000000000000000000000000000000000000..3b4ea7911e77fbc716fc3c714888fe11f6c6d3ab
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_alt.py
@@ -0,0 +1,323 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import json
+from sensor_msgs.msg import Imu
+import threading
+from std_msgs.msg import String
+from geometry_msgs.msg import Quaternion, Vector3
+from box import Box
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import datetime
+import cv2
+import time
+import queue, threading, time
+
+# Set up the detector with default parameters.
+detector = cv2.SimpleBlobDetector()
+
+# Setup SimpleBlobDetector parameters.
+params = cv2.SimpleBlobDetector_Params()
+
+# Change thresholds
+#params.minThreshold = 5
+#params.maxThreshold = 500
+
+#FIlter by Color
+params.filterByColor = True
+params.blobColor = 255
+
+# Filter by Area.
+params.filterByArea = True
+params.minArea = 10
+params.maxArea = 200
+
+# Filter by Circularity
+#params.filterByCircularity = True
+#params.minCircularity = 0.5
+#params.maxCircularity = 1
+
+# Filter by Convexity
+#params.filterByConvexity = True
+#params.minConvexity = 0.7
+#params.maxConvexity = 1
+
+# Filter by Inertia
+#params.filterByInertia = True
+#params.minInertiaRatio = 0.01
+
+# Create a detector with the parameters
+detector = cv2.SimpleBlobDetector_create(params)
+
+# bufferless VideoCapture
+class VideoCapture:
+ def __init__(self, name):
+ self.cap = cv2.VideoCapture(name)
+ self.q = queue.Queue()
+ t = threading.Thread(target=self._reader)
+ t.daemon = True
+ t.start()
+ # read frames as soon as they are available, keeping only most recent one
+ def _reader(self):
+ while True:
+ ret, frame = self.cap.read()
+ if not ret:
+ break
+ if not self.q.empty():
+ try:
+ self.q.get_nowait() # discard previous (unprocessed) frame
+ except queue.Empty:
+ pass
+ self.q.put(frame)
+
+ def read(self):
+ return self.q.get()
+
+class MyNode(Node):
+
+ def __init__(self):
+ super().__init__("sphero_mini")
+
+ def connect(self):
+ #Automatisch MAC-Adresse laden
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ def get_pos(self, cap, height):
+ zeitanfang = time.time()
+
+ img = cap.read()
+
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+ keypoints = detector.detect(gray)
+
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+
+ #print("Blob X: %f Blob Y: %f" %(x,y))
+
+ zeitende = time.time()
+ #print("Dauer Programmausführung:",(zeitende-zeitanfang))
+
+ xTrans, yTrans = self.calc_trans(x,y, height)
+
+ #print("Get_Pos: X,Y:", xTrans,yTrans)
+
+ return xTrans,yTrans
+
+ def calc_offset(self, sphero, cap, height):
+ sphero.roll(100,0)
+ sphero.wait(2)
+ sphero.roll(0,0)
+ sphero.wait(0.5)
+
+ ref = self.get_pos(cap, height)
+ print("calc_offset: Ref Punkt x,y", ref)
+
+ sphero.wait(1)
+ sphero.roll(100,180)
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+ startpunkt = self.get_pos(cap, height)
+ print("calc_offset: Startpunkt x,y", startpunkt)
+
+ ref = np.array(ref)
+ startpunkt = np.array(startpunkt)
+
+ start_ref = ref-startpunkt
+
+ phi = np.arctan2(start_ref[1],start_ref[0])
+ phi = np.degrees(phi)
+ phi = int(phi)
+
+ print("Phi ohne alles:", phi)
+
+ if(phi < 0):
+ phi = phi + 360
+
+ if(phi > 360):
+ phi = phi - 360
+
+ print("Calc_Offset: ", phi)
+
+ return phi
+
+ def calc_av(self, sollPos, cap, height):
+
+ startPos = self.get_pos(cap, height)
+
+ startPos = np.array(startPos)
+ sollPos = np.array(sollPos)
+
+ pktSpheroKord = sollPos - startPos
+
+ phi = np.arctan2(pktSpheroKord[1], pktSpheroKord[0])
+ phi = np.degrees(phi)
+
+ phiZiel = int(phi)
+
+ if(phiZiel < 0):
+ phiZiel = phiZiel + 360
+
+ if(phi > 360):
+ phi = phi - 360
+
+ v = 100
+
+ print("Calc_AV: a,v", phiZiel, v)
+
+ return phiZiel, v
+
+ def drive_to(self,sphero, targetPos, aOffset, cap, height):
+ dmin = 50
+ pos = self.get_pos(cap, height)
+ pos = np.array(pos)
+
+ diff = targetPos - pos
+
+ d = np.linalg.norm(diff, ord = 2)
+
+ while d > dmin:
+ a,v = self.calc_av(targetPos, cap, height)
+ ar = aOffset + a #Fallunterscheidung?
+
+ pos = self.get_pos(cap, height)
+ pos = np.array(pos)
+ diff = targetPos - pos
+ diff = np.array(diff)
+ d = np.linalg.norm(diff, ord = 2)
+
+ sphero.roll(v, ar)
+ time.sleep(0.1)
+
+ sphero.roll(0,ar)
+
+ return
+
+ def calc_trans(self,x,y, height):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+
+def main(args = None):
+ #Verbindung herstellen, Node erstellen
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ rate = node.create_rate(5)
+
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ cap = VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+ img = cap.read()
+ height, width = img.shape[:2]
+
+ sphero.setLEDColor(255,0,0)
+ sphero.wait(1)
+ sphero.setBackLEDIntensity(100)
+ sphero.stabilization(True)
+
+ aOffset = node.calc_offset(sphero, cap, height)
+
+ sphero.roll(100,0+aOffset)
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+ sphero.wait(5)
+
+ targetPos = 117,430
+
+ node.drive_to(sphero,targetPos,aOffset,cap, height)
+
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
+
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_komisch.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_komisch.py
new file mode 100644
index 0000000000000000000000000000000000000000..7c5dc3e024792433ecfc8652b43a8c8b3281bc30
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_komisch.py
@@ -0,0 +1,305 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import json
+from sensor_msgs.msg import Imu
+import threading
+from std_msgs.msg import String
+from geometry_msgs.msg import Quaternion, Vector3
+from box import Box
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import datetime
+import cv2
+import time
+
+
+# Img Objekt
+cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+# Set up the detector with default parameters.
+detector = cv2.SimpleBlobDetector()
+
+# Setup SimpleBlobDetector parameters.
+params = cv2.SimpleBlobDetector_Params()
+
+# Change thresholds
+#params.minThreshold = 5
+#params.maxThreshold = 500
+
+#FIlter by Color
+params.filterByColor = True
+params.blobColor = 255
+
+# Filter by Area.
+#params.filterByArea = True
+#params.minArea = 100
+#params.maxArea = 1000
+
+# Filter by Circularity
+#params.filterByCircularity = True
+#params.minCircularity = 0.5
+#params.maxCircularity = 1
+
+# Filter by Convexity
+#params.filterByConvexity = True
+#params.minConvexity = 0.7
+#params.maxConvexity = 1
+
+# Filter by Inertia
+#params.filterByInertia = True
+#params.minInertiaRatio = 0.01
+
+# Create a detector with the parameters
+detector = cv2.SimpleBlobDetector_create(params)
+
+class MyNode(Node):
+ #Konstrukter Node Objekt
+ def __init__(self):
+ super().__init__("sphero_mini")
+ #self.publisher_ = self.create_publisher(String,"Imu",5)
+
+ def connect(self):
+ #Automatisch MAC-Adresse laden
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ def positionauslesen(self):
+ zeitanfang = time.time()
+
+ x = (-1)
+ y = (-1)
+
+ while x < 0 and y < 0:
+
+ success, img = cap.read()
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+ # Detect blobs.
+ keypoints = detector.detect(gray)
+
+ #print(keypoints)
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+
+ #print("Blob X: %f Blob Y: %f" %(x,y))
+ zeitende = time.time()
+ print("Dauer Programmausführung:",(zeitende-zeitanfang))
+ return (x,y)
+
+ def ref_winkel(self, sphero):
+
+ startpunkt = self.positionauslesen()
+
+ sphero.roll(100,0)
+ sphero.wait(2)
+ sphero.roll(0,0)
+ sphero.wait(0.5)
+
+ ref = self.positionauslesen()
+
+ sphero.wait(0.5)
+ sphero.roll(100,180)
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+ soll = (startpunkt[0]+100,startpunkt[1])
+
+ soll = np.array(soll)
+ ref = np.array(ref)
+ startpunkt = np.array(startpunkt)
+
+ start_ref = ref-startpunkt
+ start_soll = soll-startpunkt
+
+ y = start_ref[0] * start_soll[1] - start_ref[1] * start_soll[0]
+ x = start_ref[0] * start_soll[0] + start_ref[1] * start_soll[1]
+
+ phi = np.arctan2(y,x)
+ phi = np.degrees(phi)
+ phi = int(phi)
+
+ if(phi < 0 ):
+ phi = phi + 360
+
+ return phi
+
+ def fahre_ziel(self, sphero, x,y, phiAbw):
+
+ startPos = self.positionauslesen()
+ sollPos = (x,y)
+
+ startPos = np.array(startPos)
+ sollPos = np.array(sollPos)
+
+ pktSpheroKord = sollPos - startPos
+
+ phi = np.arctan2(pktSpheroKord[1], pktSpheroKord[0])
+ phi = np.degrees(phi)
+
+ phi = int(phi)
+
+ phiZiel = phiAbw - phi
+
+ if(phiZiel < 0 ):
+ phiZiel = phiZiel + 360
+
+ print("Winkel Sphero System ", phi)
+
+ #sollPos = (x,y)
+ #startPos = self.positionauslesen()
+ #refPos = (startPos[0]+100,startPos[1])
+
+ #sollPos = np.array(sollPos)
+ #refPos = np.array(refPos)
+ #startPos = np.array(startPos)
+
+ #start_ref = refPos-startPos
+ #start_soll = sollPos-startPos
+
+ #y = start_ref[0] * start_soll[1] - start_ref[1] * start_soll[0]
+ #x = start_ref[0] * start_soll[0] + start_ref[1] * start_soll[1]
+
+ #phi = np.arctan2(y,x)
+ #phi = np.degrees(phi)
+ #phiZiel = phiAbw - (int(phi) * -1)
+
+ print("Zielwinkel: ", phiZiel)
+
+ while(not(np.allclose(startPos, sollPos, atol = 50))):
+
+ sphero.roll(100, (phiZiel))
+
+ aktPos = self.positionauslesen()
+ aktPos = np.array(aktPos)
+
+ time.sleep(0.1)
+
+ print("Ziel erreicht")
+
+ sphero.roll(0, (0))
+
+ return
+
+
+def main(args = None):
+ #Verbindung herstellen, Node erstellen
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ rate = node.create_rate(5)
+
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ sphero.setLEDColor(255,0,0)
+ sphero.wait(1)
+ sphero.setBackLEDIntensity(100)
+ sphero.stabilization(True)
+
+ #Differenzwinkel auslesen
+ phiAbw = node.ref_winkel(sphero)
+ print("Abweichender Winkel Initialfahrt:", phiAbw)
+
+ sphero.wait(5)
+ sphero.roll(100, phiAbw)
+
+ sphero.wait(5)
+ node.fahre_ziel(sphero, 94,94, phiAbw)
+
+ sphero.wait(5)
+ node.fahre_ziel(sphero, 134,462, phiAbw)
+
+ sphero.wait(5)
+ node.fahre_ziel(sphero, 581,372, phiAbw)
+
+ sphero.wait(5)
+ node.fahre_ziel(sphero, 331,308, phiAbw)
+
+ rclpy.spin()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
+
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_refwinkel.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_refwinkel.py
new file mode 100644
index 0000000000000000000000000000000000000000..78066a4c2fa4da3a7a78b3faca6b5427ae979e42
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_refwinkel.py
@@ -0,0 +1,243 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import json
+from sensor_msgs.msg import Imu
+import threading
+from std_msgs.msg import String
+from geometry_msgs.msg import Quaternion, Vector3
+from box import Box
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import datetime
+
+class MyNode(Node):
+ #Konstrukter Node Objekt
+ def __init__(self):
+ super().__init__("sphero_mini")
+ #self.publisher_ = self.create_publisher(String,"Imu",5)
+
+ def connect(self):
+ #Automatisch MAC-Adresse laden
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ def positionauslesen(self):
+ # Standard imports
+ import cv2
+ import numpy as np;
+
+ # Img Objekt
+ cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+ # Set up the detector with default parameters.
+ detector = cv2.SimpleBlobDetector()
+
+ # Setup SimpleBlobDetector parameters.
+ params = cv2.SimpleBlobDetector_Params()
+
+ # Change thresholds
+ #params.minThreshold = 5
+ #params.maxThreshold = 500
+
+ #FIlter by Color
+ params.filterByColor = True
+ params.blobColor = 255
+
+ # Filter by Area.
+ #params.filterByArea = True
+ #params.minArea = 100
+ #params.maxArea = 1000
+
+ # Filter by Circularity
+ #params.filterByCircularity = True
+ #params.minCircularity = 0.5
+ #params.maxCircularity = 1
+
+ # Filter by Convexity
+ #params.filterByConvexity = True
+ #params.minConvexity = 0.7
+ #params.maxConvexity = 1
+
+ # Filter by Inertia
+ #params.filterByInertia = True
+ #params.minInertiaRatio = 0.01
+
+ # Create a detector with the parameters
+ detector = cv2.SimpleBlobDetector_create(params)
+
+ success, img = cap.read()
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+
+ # Detect blobs.
+ keypoints = detector.detect(gray)
+
+ #print(keypoints)
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+ #keypointss = keyPoint.sizekeyPoints
+
+ #print("Blob X: %f Blob Y: %f" %(x,y))
+
+ # Draw detected blobs as red circles.
+ # cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures
+ # the size of the circle corresponds to the size of blob
+
+ im_with_keypoints = cv2.drawKeypoints(gray, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
+
+ # Show blobs
+ cv2.imshow("Keypoints", im_with_keypoints)
+
+ return (x,y)
+
+ def ref_winkel(self, sphero, x,y):
+
+ startpunkt = self.positionauslesen()
+
+ sphero.roll(100,0)
+ sphero.wait(2)
+ sphero.roll(0,0)
+ sphero.wait(0.5)
+
+ ref = self.positionauslesen()
+
+ #print(ref)
+
+ sphero.wait(0.5)
+ sphero.roll(100,180)
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+ soll = (x,y)
+
+ soll = np.array(soll)
+ ref = np.array(ref)
+ startpunkt = np.array(startpunkt)
+
+ start_ref = ref-startpunkt
+ start_soll = soll-startpunkt
+ phi = np.arccos(np.dot(start_ref,start_soll) / (np.linalg.norm(start_ref)*np.linalg.norm(start_soll)))
+ return phi
+
+def main(args = None):
+ #Verbindung herstellen, Node erstellen
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ rate = node.create_rate(5)
+
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ #Simple Moves, Erste Bewegung erkennen
+ #try:oll = (0,0)
+ # while rclpy.ok():
+
+ #sphero.positionauslesen()
+ #soll = 0,0
+ phi = node.ref_winkel(sphero, 0,0)
+
+ phi = np.degrees(phi)
+
+ phi = int(phi)
+
+ print("Abweihender Winkel:", phi)
+
+ sphero.wait(0.5)
+ sphero.roll(100, (0 + phi))
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+# except KeyboardInterrupt:
+ # pass
+
+ rclpy.shutdown()
+ # thread.join()
+
+if __name__ == '__main__':
+ main()
+
+ #!/usr/bin/env python3
+
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_testfahrt.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_testfahrt.py
new file mode 100644
index 0000000000000000000000000000000000000000..29f2473b20a9c782704e15851518da8ec90ee823
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node_testfahrt.py
@@ -0,0 +1,183 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import json
+from sensor_msgs.msg import Imu
+import threading
+from std_msgs.msg import String
+from geometry_msgs.msg import Quaternion, Vector3
+from box import Box
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import datetime
+
+
+class MyNode(Node):
+
+ def __init__(self):
+ super().__init__("sphero_mini")
+ self.publisher_ = self.create_publisher(String,"Imu",5)
+ #self.get_logger().info("Hello from ROS2")
+
+ def connect(self):
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ #print(i)
+
+ #self.publisher_.publish(i)
+
+
+
+def main(args = None):
+
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ rate = node.create_rate(2)
+
+ try:
+ while rclpy.ok():
+
+ sphero.configureSensorMask(
+ IMU_yaw=True,
+ IMU_pitch=True,
+ IMU_roll=True,
+ IMU_acc_y=True,
+ IMU_acc_z=True,
+ IMU_acc_x=True,
+ IMU_gyro_x=True,
+ IMU_gyro_y=True,
+ #IMU_gyro_z=True
+ )
+ sphero.configureSensorStream()
+
+ sensors_values = node.get_sensors_data(sphero)
+ #rclpy.logdebug(sensors_values)
+ node.publish_imu(sensors_values, node)
+ sphero.setLEDColor(red = 0, green = 255, blue = 0) # Turn LEDs green
+
+ # Aiming:
+ sphero.setLEDColor(red = 0, green = 0, blue = 0) # Turn main LED off
+ sphero.stabilization(False) # Turn off stabilization
+ sphero.setBackLEDIntensity(255) # turn back LED on
+ sphero.wait(3) # Non-blocking pau`se
+ sphero.resetHeading() # Reset heading
+ sphero.stabilization(True) # Turn on stabilization
+ sphero.setBackLEDIntensity(0) # Turn back LED off
+
+ # Move around:
+ sphero.setLEDColor(red = 0, green = 0, blue = 255) # Turn main LED blue
+ sphero.roll(100, 0) # roll forwards (heading = 0) at speed = 50
+
+ sphero.wait(3) # Keep rolling for three seconds
+
+ sphero.roll(0, 0) # stop
+ sphero.wait(1) # Allow time to stop
+
+ sphero.setLEDColor(red = 0, green = 255, blue = 0) # Turn main LED green
+ sphero.roll(-100, 0) # Keep facing forwards but roll backwards at speed = 50
+ sphero.wait(3) # Keep rolling for three seconds
+
+ sphero.roll(0, 0) # stop
+ sphero.wait(1) # Allow time to stop
+
+ rate.sleep()
+
+ except KeyboardInterrupt:
+ pass
+
+ rclpy.shutdown()
+ thread.join()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/pfadplanung.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/pfadplanung.py
new file mode 100644
index 0000000000000000000000000000000000000000..d07a2a00e56b7626e47d9ae9ba004c28b8fa3a94
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/pfadplanung.py
@@ -0,0 +1,223 @@
+
+def planInitialPath(start, goal):
+
+ from matplotlib import pyplot as ppl
+ from matplotlib import cm
+ import random, sys, math, os.path
+ from matplotlib.pyplot import imread
+
+ #Implementation of RRT
+
+ MAP_IMG = './map.jpg' #Black and white image for a map
+ MIN_NUM_VERT = 20 # Minimum number of vertex in the graph
+ MAX_NUM_VERT = 1500 # Maximum number of vertex in the graph
+ STEP_DISTANCE = 10 # Maximum distance between two vertex
+ SEED = None # For random numbers
+
+ def rapidlyExploringRandomTree(ax, img, start, goal, seed=None):
+ hundreds = 100
+ seed = random.seed(seed)
+ #print("Zufallsseed: ", seed)
+ points = []
+ graph = []
+ points.append(start)
+ graph.append((start, []))
+ print('Generating and conecting random points')
+ occupied = True
+ phaseTwo = False
+
+ # Phase two values (points 5 step distances around the goal point)
+ minX = max(goal[0] - 5 * STEP_DISTANCE, 0)
+ maxX = min(goal[0] + 5 * STEP_DISTANCE, len(img[0]) - 1)
+ minY = max(goal[1] - 5 * STEP_DISTANCE, 0)
+ maxY = min(goal[1] + 5 * STEP_DISTANCE, len(img) - 1)
+
+ i = 0
+ while (goal not in points) and (len(points) < MAX_NUM_VERT):
+ if (i % 100) == 0:
+ print(i, 'points randomly generated')
+
+ if (len(points) % hundreds) == 0:
+ print(len(points), 'vertex generated')
+ hundreds = hundreds + 100
+
+ while (occupied):
+ if phaseTwo and (random.random() > 0.8):
+ point = [random.randint(minX, maxX), random.randint(minY, maxY)]
+ else:
+ point = [random.randint(0, len(img[0]) - 1), random.randint(0, len(img) - 1)]
+
+ if (img[point[1]][point[0]][0] * 255 == 255):
+ occupied = False
+
+ occupied = True
+
+ nearest = findNearestPoint(points, point)
+ newPoints = connectPoints(point, nearest, img)
+ addToGraph(ax, graph, newPoints, point)
+ newPoints.pop(0) # The first element is already in the points list
+ points.extend(newPoints)
+ ppl.draw()
+ i = i + 1
+
+ if len(points) >= MIN_NUM_VERT:
+ if not phaseTwo:
+ print('Phase Two')
+ phaseTwo = True
+
+ if phaseTwo:
+ nearest = findNearestPoint(points, goal)
+ newPoints = connectPoints(goal, nearest, img)
+ addToGraph(ax, graph, newPoints, goal)
+ newPoints.pop(0)
+ points.extend(newPoints)
+ ppl.draw()
+
+ if goal in points:
+ print('Goal found, total vertex in graph:', len(points), 'total random points generated:', i)
+
+ path = searchPath(graph, start, [start])
+
+ for i in range(len(path) - 1):
+ ax.plot([path[i][0], path[i + 1][0]], [path[i][1], path[i + 1][1]], color='g', linestyle='-',
+ linewidth=2)
+ ppl.draw()
+
+ print('Showing resulting map')
+ print('Final path:', path)
+ print('The final path is made from:', len(path), 'connected points')
+ else:
+ path = None
+ print('Reached maximum number of vertex and goal was not found')
+ print('Total vertex in graph:', len(points), 'total random points generated:', i)
+ print('Showing resulting map')
+
+ ppl.show()
+ return path
+
+ def searchPath(graph, point, path):
+ for i in graph:
+ if point == i[0]:
+ p = i
+
+ if p[0] == graph[-1][0]:
+ return path
+
+ for link in p[1]:
+ path.append(link)
+ finalPath = searchPath(graph, link, path)
+
+ if finalPath != None:
+ return finalPath
+ else:
+ path.pop()
+
+ def addToGraph(ax, graph, newPoints, point):
+ if len(newPoints) > 1: # If there is anything to add to the graph
+ for p in range(len(newPoints) - 1):
+ nearest = [nearest for nearest in graph if (nearest[0] == [newPoints[p][0], newPoints[p][1]])]
+ nearest[0][1].append(newPoints[p + 1])
+ graph.append((newPoints[p + 1], []))
+
+ if not p == 0:
+ ax.plot(newPoints[p][0], newPoints[p][1], '+k') # First point is already painted
+ ax.plot([newPoints[p][0], newPoints[p + 1][0]], [newPoints[p][1], newPoints[p + 1][1]], color='k',
+ linestyle='-', linewidth=1)
+
+ if point in newPoints:
+ ax.plot(point[0], point[1], '.g') # Last point is green
+ else:
+ ax.plot(newPoints[p + 1][0], newPoints[p + 1][1], '+k') # Last point is not green
+
+ def connectPoints(a, b, img):
+ newPoints = []
+ newPoints.append([b[0], b[1]])
+ step = [(a[0] - b[0]) / float(STEP_DISTANCE), (a[1] - b[1]) / float(STEP_DISTANCE)]
+
+ # Set small steps to check for walls
+ pointsNeeded = int(math.floor(max(math.fabs(step[0]), math.fabs(step[1]))))
+
+ if math.fabs(step[0]) > math.fabs(step[1]):
+ if step[0] >= 0:
+ step = [1, step[1] / math.fabs(step[0])]
+ else:
+ step = [-1, step[1] / math.fabs(step[0])]
+
+ else:
+ if step[1] >= 0:
+ step = [step[0] / math.fabs(step[1]), 1]
+ else:
+ step = [step[0] / math.fabs(step[1]), -1]
+
+ blocked = False
+ for i in range(pointsNeeded + 1): # Creates points between graph and solitary point
+ for j in range(STEP_DISTANCE): # Check if there are walls between points
+ coordX = round(newPoints[i][0] + step[0] * j)
+ coordY = round(newPoints[i][1] + step[1] * j)
+
+ if coordX == a[0] and coordY == a[1]:
+ break
+ if coordY >= len(img) or coordX >= len(img[0]):
+ break
+ if img[int(coordY)][int(coordX)][0] * 255 < 255:
+ blocked = True
+ if blocked:
+ break
+
+ if blocked:
+ break
+ if not (coordX == a[0] and coordY == a[1]):
+ newPoints.append(
+ [newPoints[i][0] + (step[0] * STEP_DISTANCE), newPoints[i][1] + (step[1] * STEP_DISTANCE)])
+
+ if not blocked:
+ newPoints.append([a[0], a[1]])
+ return newPoints
+
+ def findNearestPoint(points, point):
+ best = (sys.maxsize, sys.maxsize, sys.maxsize)
+ for p in points:
+ if p == point:
+ continue
+ dist = math.sqrt((p[0] - point[0]) ** 2 + (p[1] - point[1]) ** 2)
+ if dist < best[2]:
+ best = (p[0], p[1], dist)
+ return (best[0], best[1])
+
+ # Standard imports
+ import cv2
+ import numpy as np;
+
+ # Img Objekt
+ cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+ val, frame = cap.read()
+ inverted_image = np.invert(frame)
+ cv2.imwrite("map.jpg", inverted_image)
+
+ print('Loading map... with file \'', MAP_IMG, '\'')
+ img = imread(MAP_IMG)
+ fig = ppl.gcf()
+ fig.clf()
+ ax = fig.add_subplot(1, 1, 1)
+ ax.imshow(img, cmap=cm.Greys_r)
+ ax.axis('image')
+ ppl.draw()
+ print('Map is', len(img[0]), 'x', len(img))
+
+ path = rapidlyExploringRandomTree(ax, img, start, goal, seed=SEED)
+
+ if len(sys.argv) > 2:
+ print('Only one argument is needed')
+ elif len(sys.argv) > 1:
+ if os.path.isfile(sys.argv[1]):
+ MAP_IMG = sys.argv[1]
+ else:
+ print(sys.argv[1], 'is not a file')
+
+ pathIndCntr = 0
+
+ return path, pathIndCntr
+
+start = (200,200)
+ziel = (450, 450)
+planInitialPath(start, ziel)
\ No newline at end of file
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/potential_field_planning (1).py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/potential_field_planning (1).py
new file mode 100644
index 0000000000000000000000000000000000000000..8f136b5ee3fbee7f9cd0d40fd368a4e10822030b
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/potential_field_planning (1).py
@@ -0,0 +1,199 @@
+"""
+
+Potential Field based path planner
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+Ref:
+https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
+
+"""
+
+from collections import deque
+import numpy as np
+import matplotlib.pyplot as plt
+
+# Parameters
+KP = 5.0 # attractive potential gain
+ETA = 100.0 # repulsive potential gain
+AREA_WIDTH = 30.0 # potential area width [m]
+# the number of previous positions used to check oscillations
+OSCILLATIONS_DETECTION_LENGTH = 3
+
+show_animation = True
+
+
+def calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy):
+ minx = min(min(ox), sx, gx) - AREA_WIDTH / 2.0
+ miny = min(min(oy), sy, gy) - AREA_WIDTH / 2.0
+ maxx = max(max(ox), sx, gx) + AREA_WIDTH / 2.0
+ maxy = max(max(oy), sy, gy) + AREA_WIDTH / 2.0
+ xw = int(round((maxx - minx) / reso))
+ yw = int(round((maxy - miny) / reso))
+
+ # calc each potential
+ pmap = [[0.0 for i in range(yw)] for i in range(xw)]
+
+ for ix in range(xw):
+ x = ix * reso + minx
+
+ for iy in range(yw):
+ y = iy * reso + miny
+ ug = calc_attractive_potential(x, y, gx, gy)
+ uo = calc_repulsive_potential(x, y, ox, oy, rr)
+ uf = ug + uo
+ pmap[ix][iy] = uf
+
+ return pmap, minx, miny
+
+
+def calc_attractive_potential(x, y, gx, gy):
+ return 0.5 * KP * np.hypot(x - gx, y - gy)
+
+
+def calc_repulsive_potential(x, y, ox, oy, rr):
+ # search nearest obstacle
+ minid = -1
+ dmin = float("inf")
+ for i, _ in enumerate(ox):
+ d = np.hypot(x - ox[i], y - oy[i])
+ if dmin >= d:
+ dmin = d
+ minid = i
+
+ # calc repulsive potential
+ dq = np.hypot(x - ox[minid], y - oy[minid])
+
+ if dq <= rr:
+ if dq <= 0.1:
+ dq = 0.1
+
+ return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2
+ else:
+ return 0.0
+
+
+def get_motion_model():
+ # dx, dy
+ motion = [[1, 0],
+ [0, 1],
+ [-1, 0],
+ [0, -1],
+ [-1, -1],
+ [-1, 1],
+ [1, -1],
+ [1, 1]]
+
+ return motion
+
+
+def oscillations_detection(previous_ids, ix, iy):
+ previous_ids.append((ix, iy))
+
+ if (len(previous_ids) > OSCILLATIONS_DETECTION_LENGTH):
+ previous_ids.popleft()
+
+ # check if contains any duplicates by copying into a set
+ previous_ids_set = set()
+ for index in previous_ids:
+ if index in previous_ids_set:
+ return True
+ else:
+ previous_ids_set.add(index)
+ return False
+
+
+def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
+
+ # calc potential field
+ pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy)
+
+ # search path
+ d = np.hypot(sx - gx, sy - gy)
+ ix = round((sx - minx) / reso)
+ iy = round((sy - miny) / reso)
+ gix = round((gx - minx) / reso)
+ giy = round((gy - miny) / reso)
+
+ if show_animation:
+ draw_heatmap(pmap)
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect('key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
+ plt.plot(ix, iy, "*k")
+ plt.plot(gix, giy, "*m")
+
+ rx, ry = [sx], [sy]
+ motion = get_motion_model()
+ previous_ids = deque()
+
+ while d >= reso:
+ minp = float("inf")
+ minix, miniy = -1, -1
+ for i, _ in enumerate(motion):
+ inx = int(ix + motion[i][0])
+ iny = int(iy + motion[i][1])
+ if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0:
+ p = float("inf") # outside area
+ print("outside potential!")
+ else:
+ p = pmap[inx][iny]
+ if minp > p:
+ minp = p
+ minix = inx
+ miniy = iny
+ ix = minix
+ iy = miniy
+ xp = ix * reso + minx
+ yp = iy * reso + miny
+ d = np.hypot(gx - xp, gy - yp)
+ rx.append(xp)
+ ry.append(yp)
+
+ if (oscillations_detection(previous_ids, ix, iy)):
+ print("Oscillation detected at ({},{})!".format(ix, iy))
+ break
+
+ if show_animation:
+ plt.plot(ix, iy, ".r")
+ plt.pause(0.01)
+
+ print("Goal!!")
+
+ return rx, ry
+
+
+def draw_heatmap(data):
+ data = np.array(data).T
+ plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)
+
+
+def main():
+ print("potential_field_planning start")
+
+ sx = 0.0 # start x position [m]
+ sy = 10.0 # start y positon [m]
+ gx = 30.0 # goal x position [m]
+ gy = 30.0 # goal y position [m]
+ grid_size = 0.5 # potential grid size [m]
+ robot_radius = 5.0 # robot radius [m]
+
+ ox = [15.0, 5.0, 20.0, 25.0] # obstacle x position list [m]
+ oy = [25.0, 15.0, 26.0, 25.0] # obstacle y position list [m]
+
+ if show_animation:
+ plt.grid(True)
+ plt.axis("equal")
+
+ # path generation
+ _, _ = potential_field_planning(
+ sx, sy, gx, gy, ox, oy, grid_size, robot_radius)
+
+ if show_animation:
+ plt.show()
+
+
+if __name__ == '__main__':
+ print(__file__ + " start!!")
+ main()
+ print(__file__ + " Done!!")
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/potential_field_planning.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/potential_field_planning.py
new file mode 100644
index 0000000000000000000000000000000000000000..c28279d83d3ea64eaee8592afabe70a8fe5372d1
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/potential_field_planning.py
@@ -0,0 +1,248 @@
+"""
+
+Potential Field based path planner
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+Ref:
+https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
+
+"""
+
+from collections import deque
+import numpy as np
+import matplotlib.pyplot as plt
+
+# Parameters
+KP = 10.0 # attractive potential gain
+ETA = 300.0 # repulsive potential gain
+AREA_WIDTH = 704.0 # potential area width [m]
+# the number of previous positions used to check oscillations
+OSCILLATIONS_DETECTION_LENGTH = 3
+
+show_animation = True
+
+px = []
+py = []
+
+def calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy):
+ minx = min(min(ox), sx, gx) - AREA_WIDTH / 2.0
+ miny = min(min(oy), sy, gy) - AREA_WIDTH / 2.0
+ maxx = max(max(ox), sx, gx) + AREA_WIDTH / 2.0
+ maxy = max(max(oy), sy, gy) + AREA_WIDTH / 2.0
+ xw = int(round((maxx - minx) / reso))
+ yw = int(round((maxy - miny) / reso))
+
+ # calc each potential
+ pmap = [[0.0 for i in range(yw)] for i in range(xw)]
+
+ for ix in range(xw):
+ x = ix * reso + minx
+
+ for iy in range(yw):
+ y = iy * reso + miny
+ ug = calc_attractive_potential(x, y, gx, gy)
+ uo = calc_repulsive_potential(x, y, ox, oy, rr)
+ uf = ug + uo
+ pmap[ix][iy] = uf
+
+ return pmap, minx, miny
+
+
+def calc_attractive_potential(x, y, gx, gy):
+ return 0.5 * KP * np.hypot(x - gx, y - gy)
+
+
+def calc_repulsive_potential(x, y, ox, oy, rr):
+ # search nearest obstacle
+ minid = -1
+ dmin = float("inf")
+ for i, _ in enumerate(ox):
+ d = np.hypot(x - ox[i], y - oy[i])
+ if dmin >= d:
+ dmin = d
+ minid = i
+
+ # calc repulsive potential
+ dq = np.hypot(x - ox[minid], y - oy[minid])
+
+ if dq <= rr:
+ if dq <= 0.1:
+ dq = 0.1
+
+ return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2
+ else:
+ return 0.0
+
+
+def get_motion_model():
+ # dx, dy
+ motion = [[1, 0],
+ [0, 1],
+ [-1, 0],
+ [0, -1],
+ [-1, -1],
+ [-1, 1],
+ [1, -1],
+ [1, 1]]
+
+ return motion
+
+
+def oscillations_detection(previous_ids, ix, iy):
+ previous_ids.append((ix, iy))
+
+ if (len(previous_ids) > OSCILLATIONS_DETECTION_LENGTH):
+ previous_ids.popleft()
+
+ # check if contains any duplicates by copying into a set
+ previous_ids_set = set()
+ for index in previous_ids:
+ if index in previous_ids_set:
+ return True
+ else:
+ previous_ids_set.add(index)
+ return False
+
+
+def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
+
+ # calc potential field
+ pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy)
+
+ # search path
+ d = np.hypot(sx - gx, sy - gy)
+ ix = round((sx - minx) / reso)
+ iy = round((sy - miny) / reso)
+ gix = round((gx - minx) / reso)
+ giy = round((gy - miny) / reso)
+
+ if show_animation:
+ draw_heatmap(pmap)
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect('key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
+ plt.plot(ix, iy, "*k")
+ plt.plot(gix, giy, "*m")
+
+ rx, ry = [sx], [sy]
+ motion = get_motion_model()
+ previous_ids = deque()
+
+ while d >= reso:
+ minp = float("inf")
+ minix, miniy = -1, -1
+ for i, _ in enumerate(motion):
+ inx = int(ix + motion[i][0])
+ iny = int(iy + motion[i][1])
+ if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0:
+ p = float("inf") # outside area
+ print("outside potential!")
+ else:
+ p = pmap[inx][iny]
+ if minp > p:
+ minp = p
+ minix = inx
+ miniy = iny
+ ix = minix
+ iy = miniy
+ xp = ix * reso + minx
+ yp = iy * reso + miny
+ d = np.hypot(gx - xp, gy - yp)
+ rx.append(xp)
+ ry.append(yp)
+
+ if (oscillations_detection(previous_ids, ix, iy)):
+ print("Oscillation detected at ({},{})!".format(ix, iy))
+ break
+
+ if show_animation:
+ px.append(ix)
+ py.append(iy)
+ plt.plot(ix, iy, ".r")
+ plt.pause(0.01)
+
+ print("Goal!!")
+
+ return rx, ry
+
+
+def draw_heatmap(data):
+ data = np.array(data).T
+ plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)
+
+
+def create_map():
+ # Map erstellen
+ # Standard imports
+ import cv2
+ import numpy as np;
+
+ # Img Objekt
+ #cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+ #success, img = cap.read()
+
+ gray = cv2.imread("/home/ubuntu/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/map/map.jpg",cv2.IMREAD_GRAYSCALE)
+
+ #gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
+ mask = cv2.inRange(gray, np.array([90]), np.array([255]))
+
+ mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+
+ mask_list= np.ndarray.tolist(mask)
+
+ for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['W' if j > 200 else ' ' for j in mask_list[i]]
+
+ #mapOfWorld[200][200] = 'R' #Start Pos
+ #mapOfWorld[300][300] = 'G' #Ziel Pos
+
+ return mapOfWorld
+
+
+def create_obsticles(mapOfWorld):
+ ox = []
+ oy = []
+
+ mapOfWorld = np.array(mapOfWorld)
+
+ for i in range(0,mapOfWorld.shape[0]):
+ for j in range(0,mapOfWorld.shape[1]):
+ if mapOfWorld[i][j] == 'W':
+ ox.append(i)
+ oy.append(j)
+
+ return ox,oy
+
+def main():
+ print("potential_field_planning start")
+
+ mapOfWorld = create_map()
+
+ sx = 50 # start x position [m]
+ sy = 400 # start y positon [m]
+ gx = 600 # goal x position [m]
+ gy = 100 # goal y position [m]
+ grid_size = 10 # potential grid size [m]
+ robot_radius = 10 # robot radius [m]
+
+ ox, oy = create_obsticles(mapOfWorld)
+
+ if show_animation:
+ plt.grid(True)
+ plt.axis("equal")
+
+ # path generation
+ _, _ = potential_field_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_radius)
+
+ if show_animation:
+ plt.show()
+
+ print("X_Pos", px)
+ print("Y_Pos", py)
+
+if __name__ == '__main__':
+ print(__file__ + " start!!")
+ main()
+ print(__file__ + " Done!!")
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/sphero.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/sphero.py
new file mode 100644
index 0000000000000000000000000000000000000000..b5138d1b23a13a1117038f444cc8a0de0dbc0078
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/sphero.py
@@ -0,0 +1,22 @@
+import numpy as np
+def ref_winkel(sphero, soll):
+
+ startpunkt = sphero.positionsauslesen
+
+ sphero.roll(100,0)
+ sphero.wait(2)
+ sphero.roll(0,0)
+ sphero.wait(0.5)
+
+ ref = sphero.positionsauslesen
+
+ sphero.wait(0.5)
+ sphero.roll(100,0)
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+ start_ref = ref-startpunkt
+ start_soll = soll-startpunkt
+ phi = np.arccos(start_ref*start_soll / (np.linalg.norm(start_ref)*np.linalg.norm(start_soll)))
+ return phi
+
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/test.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/test.py
new file mode 100644
index 0000000000000000000000000000000000000000..b5c4cbbe779a35653aad3fe176e1a0b9802319fd
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/test.py
@@ -0,0 +1,23 @@
+#Map erstellen
+# Standard imports
+import cv2
+import numpy as np;
+
+# Img Objekt
+cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+success, img = cap.read()
+
+gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
+mask = cv2.inRange(gray, np.array([90]), np.array([255]))
+
+mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+
+mask_list= np.ndarray.tolist(mask)
+
+for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['W' if j > 200 else ' ' for j in mask_list[i]]
+
+print(mapOfWorld)
+
+
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/treiber.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/treiber.py
new file mode 100644
index 0000000000000000000000000000000000000000..ec3435051de5aa72fc393d224b0fe1a53927b8b0
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/treiber.py
@@ -0,0 +1,638 @@
+from bluepy.btle import Peripheral
+from bluepy import btle
+from sphero_mini_controller._constants import *
+import struct
+import time
+import sys
+
+class SpheroMini():
+ def __init__(self, MACAddr, verbosity = 4, user_delegate = None):
+ '''
+ initialize class instance and then build collect BLE sevices and characteristics.
+ Also sends text string to Anti-DOS characteristic to prevent returning to sleep,
+ and initializes notifications (which is what the sphero uses to send data back to
+ the client).
+ '''
+ self.verbosity = verbosity # 0 = Silent,
+ # 1 = Connection/disconnection only
+ # 2 = Init messages
+ # 3 = Recieved commands
+ # 4 = Acknowledgements
+ self.sequence = 1
+ self.v_batt = None # will be updated with battery voltage when sphero.getBatteryVoltage() is called
+ self.firmware_version = [] # will be updated with firware version when sphero.returnMainApplicationVersion() is called
+
+ if self.verbosity > 0:
+ print("[INFO] Connecting to ", MACAddr)
+ self.p = Peripheral(MACAddr, "random") #connect
+
+ if self.verbosity > 1:
+ print("[INIT] Initializing")
+
+ # Subscribe to notifications
+ self.sphero_delegate = MyDelegate(self, user_delegate) # Pass a reference to this instance when initializing
+ self.p.setDelegate(self.sphero_delegate)
+
+ if self.verbosity > 1:
+ print("[INIT] Read all characteristics and descriptors")
+ # Get characteristics and descriptors
+ self.API_V2_characteristic = self.p.getCharacteristics(uuid="00010002-574f-4f20-5370-6865726f2121")[0]
+ self.AntiDOS_characteristic = self.p.getCharacteristics(uuid="00020005-574f-4f20-5370-6865726f2121")[0]
+ self.DFU_characteristic = self.p.getCharacteristics(uuid="00020002-574f-4f20-5370-6865726f2121")[0]
+ self.DFU2_characteristic = self.p.getCharacteristics(uuid="00020004-574f-4f20-5370-6865726f2121")[0]
+ self.API_descriptor = self.API_V2_characteristic.getDescriptors(forUUID=0x2902)[0]
+ self.DFU_descriptor = self.DFU_characteristic.getDescriptors(forUUID=0x2902)[0]
+
+ # The rest of this sequence was observed during bluetooth sniffing:
+ # Unlock code: prevent the sphero mini from going to sleep again after 10 seconds
+ if self.verbosity > 1:
+ print("[INIT] Writing AntiDOS characteristic unlock code")
+ self.AntiDOS_characteristic.write("usetheforce...band".encode(), withResponse=True)
+
+ # Enable DFU notifications:
+ if self.verbosity > 1:
+ print("[INIT] Configuring DFU descriptor")
+ self.DFU_descriptor.write(struct.pack('<bb', 0x01, 0x00), withResponse = True)
+
+ # No idea what this is for. Possibly a device ID of sorts? Read request returns '00 00 09 00 0c 00 02 02':
+ if self.verbosity > 1:
+ print("[INIT] Reading DFU2 characteristic")
+ _ = self.DFU2_characteristic.read()
+
+ # Enable API notifications:
+ if self.verbosity > 1:
+ print("[INIT] Configuring API dectriptor")
+ self.API_descriptor.write(struct.pack('<bb', 0x01, 0x00), withResponse = True)
+
+ self.wake()
+
+ # Finished initializing:
+ if self.verbosity > 1:
+ print("[INIT] Initialization complete\n")
+
+ def disconnect(self):
+ if self.verbosity > 0:
+ print("[INFO] Disconnecting")
+
+ self.p.disconnect()
+
+ def wake(self):
+ '''
+ Bring device out of sleep mode (can only be done if device was in sleep, not deep sleep).
+ If in deep sleep, the device should be connected to USB power to wake.
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Waking".format(self.sequence))
+
+ self._send(characteristic=self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=powerCommandIDs["wake"],
+ payload=[]) # empty payload
+
+ self.getAcknowledgement("Wake")
+
+ def sleep(self, deepSleep=False):
+ '''
+ Put device to sleep or deep sleep (deep sleep needs USB power connected to wake up)
+ '''
+ if deepSleep:
+ sleepCommID=powerCommandIDs["deepSleep"]
+ if self.verbosity > 0:
+ print("[INFO] Going into deep sleep. Connect USB power to wake.")
+ else:
+ sleepCommID=powerCommandIDs["sleep"]
+ self._send(characteristic=self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=sleepCommID,
+ payload=[]) #empty payload
+
+ def setLEDColor(self, red = None, green = None, blue = None):
+ '''
+ Set device LED color based on RGB vales (each can range between 0 and 0xFF)
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Setting main LED colour to [{}, {}, {}]".format(self.sequence, red, green, blue))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['userIO'], # 0x1a
+ commID = userIOCommandIDs["allLEDs"], # 0x0e
+ payload = [0x00, 0x0e, red, green, blue])
+
+ self.getAcknowledgement("LED/backlight")
+
+ def setBackLEDIntensity(self, brightness=None):
+ '''
+ Set device LED backlight intensity based on 0-255 values
+
+ NOTE: this is not the same as aiming - it only turns on the LED
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Setting backlight intensity to {}".format(self.sequence, brightness))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['userIO'],
+ commID = userIOCommandIDs["allLEDs"],
+ payload = [0x00, 0x01, brightness])
+
+ self.getAcknowledgement("LED/backlight")
+
+ def roll(self, speed=None, heading=None):
+ '''
+ Start to move the Sphero at a given direction and speed.
+ heading: integer from 0 - 360 (degrees)
+ speed: Integer from 0 - 255
+
+ Note: the zero heading should be set at startup with the resetHeading method. Otherwise, it may
+ seem that the sphero doesn't honor the heading argument
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Rolling with speed {} and heading {}".format(self.sequence, speed, heading))
+
+ if abs(speed) > 255:
+ print("WARNING: roll speed parameter outside of allowed range (-255 to +255)")
+
+ if speed < 0:
+ speed = -1*speed+256 # speed values > 256 in the send packet make the spero go in reverse
+
+ speedH = (speed & 0xFF00) >> 8
+ speedL = speed & 0xFF
+ headingH = (heading & 0xFF00) >> 8
+ headingL = heading & 0xFF
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['driving'],
+ commID = drivingCommands["driveWithHeading"],
+ payload = [speedL, headingH, headingL, speedH])
+
+ self.getAcknowledgement("Roll")
+
+ def resetHeading(self):
+ '''
+ Reset the heading zero angle to the current heading (useful during aiming)
+ Note: in order to manually rotate the sphero, you need to call stabilization(False).
+ Once the heading has been set, call stabilization(True).
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Resetting heading".format(self.sequence))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['driving'],
+ commID = drivingCommands["resetHeading"],
+ payload = []) #empty payload
+
+ self.getAcknowledgement("Heading")
+
+ def returnMainApplicationVersion(self):
+ '''
+ Sends command to return application data in a notification
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Requesting firmware version".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID = deviceID['systemInfo'],
+ commID = SystemInfoCommands['mainApplicationVersion'],
+ payload = []) # empty
+
+ self.getAcknowledgement("Firmware")
+
+ def getBatteryVoltage(self):
+ '''
+ Sends command to return battery voltage data in a notification.
+ Data printed to console screen by the handleNotifications() method in the MyDelegate class.
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Requesting battery voltage".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=powerCommandIDs['batteryVoltage'],
+ payload=[]) # empty
+
+ self.getAcknowledgement("Battery")
+
+ def stabilization(self, stab = True):
+ '''
+ Sends command to turn on/off the motor stabilization system (required when manually turning/aiming the sphero)
+ '''
+ if stab == True:
+ if self.verbosity > 2:
+ print("[SEND {}] Enabling stabilization".format(self.sequence))
+ val = 1
+ else:
+ if self.verbosity > 2:
+ print("[SEND {}] Disabling stabilization".format(self.sequence))
+ val = 0
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['driving'],
+ commID=drivingCommands['stabilization'],
+ payload=[val])
+
+ self.getAcknowledgement("Stabilization")
+
+ def wait(self, delay):
+ '''
+ This is a non-blocking delay command. It is similar to time.sleep(), except it allows asynchronous
+ notification handling to still be performed.
+ '''
+ start = time.time()
+ while(1):
+ self.p.waitForNotifications(0.001)
+ if time.time() - start > delay:
+ break
+
+ def _send(self, characteristic=None, devID=None, commID=None, payload=[]):
+ '''
+ A generic "send" method, which will be used by other methods to send a command ID, payload and
+ appropriate checksum to a specified device ID. Mainly useful because payloads are optional,
+ and can be of varying length, to convert packets to binary, and calculate and send the
+ checksum. For internal use only.
+
+ Packet structure has the following format (in order):
+
+ - Start byte: always 0x8D
+ - Flags byte: indicate response required, etc
+ - Virtual device ID: see _constants.py
+ - Command ID: see _constants.py
+ - Sequence number: Seems to be arbitrary. I suspect it is used to match commands to response packets (in which the number is echoed).
+ - Payload: Could be varying number of bytes (incl. none), depending on the command
+ - Checksum: See below for calculation
+ - End byte: always 0xD8
+
+ '''
+ sendBytes = [sendPacketConstants["StartOfPacket"],
+ sum([flags["resetsInactivityTimeout"], flags["requestsResponse"]]),
+ devID,
+ commID,
+ self.sequence] + payload # concatenate payload list
+
+ self.sequence += 1 # Increment sequence number, ensures we can identify response packets are for this command
+ if self.sequence > 255:
+ self.sequence = 0
+
+ # Compute and append checksum and add EOP byte:
+ # From Sphero docs: "The [checksum is the] modulo 256 sum of all the bytes
+ # from the device ID through the end of the data payload,
+ # bit inverted (1's complement)"
+ # For the sphero mini, the flag bits must be included too:
+ checksum = 0
+ for num in sendBytes[1:]:
+ checksum = (checksum + num) & 0xFF # bitwise "and to get modulo 256 sum of appropriate bytes
+ checksum = 0xff - checksum # bitwise 'not' to invert checksum bits
+ sendBytes += [checksum, sendPacketConstants["EndOfPacket"]] # concatenate
+
+ # Convert numbers to bytes
+ output = b"".join([x.to_bytes(1, byteorder='big') for x in sendBytes])
+
+ #send to specified characteristic:
+ characteristic.write(output, withResponse = True)
+
+ def getAcknowledgement(self, ack):
+ #wait up to 10 secs for correct acknowledgement to come in, including sequence number!
+ start = time.time()
+ while(1):
+ self.p.waitForNotifications(1)
+ if self.sphero_delegate.notification_seq == self.sequence-1: # use one less than sequence, because _send function increments it for next send.
+ if self.verbosity > 3:
+ print("[RESP {}] {}".format(self.sequence-1, self.sphero_delegate.notification_ack))
+ self.sphero_delegate.clear_notification()
+ break
+ elif self.sphero_delegate.notification_seq >= 0:
+ print("Unexpected ACK. Expected: {}/{}, received: {}/{}".format(
+ ack, self.sequence, self.sphero_delegate.notification_ack.split()[0],
+ self.sphero_delegate.notification_seq)
+ )
+ if time.time() > start + 10:
+ print("Timeout waiting for acknowledgement: {}/{}".format(ack, self.sequence))
+ break
+
+# =======================================================================
+# The following functions are experimental:
+# =======================================================================
+
+ def configureCollisionDetection(self,
+ xThreshold = 50,
+ yThreshold = 50,
+ xSpeed = 50,
+ ySpeed = 50,
+ deadTime = 50, # in 10 millisecond increments
+ method = 0x01, # Must be 0x01
+ callback = None):
+ '''
+ Appears to function the same as other Sphero models, however speed settings seem to have no effect.
+ NOTE: Setting to zero seems to cause bluetooth errors with the Sphero Mini/bluepy library - set to
+ 255 to make it effectively disabled.
+
+ deadTime disables future collisions for a short period of time to avoid repeat triggering by the same
+ event. Set in 10ms increments. So if deadTime = 50, that means the delay will be 500ms, or half a second.
+
+ From Sphero docs:
+
+ xThreshold/yThreshold: An 8-bit settable threshold for the X (left/right) and Y (front/back) axes
+ of Sphero.
+
+ xSpeed/ySpeed: An 8-bit settable speed value for the X and Y axes. This setting is ranged by the
+ speed, then added to xThreshold, yThreshold to generate the final threshold value.
+ '''
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring collision detection".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['configureCollision'],
+ payload=[method, xThreshold, xSpeed, yThreshold, ySpeed, deadTime])
+
+ self.collision_detection_callback = callback
+
+ self.getAcknowledgement("Collision")
+
+ def configureSensorStream(self): # Use default values
+ '''
+ Send command to configure sensor stream using default values as found during bluetooth
+ sniffing of the Sphero Edu app.
+
+ Must be called after calling configureSensorMask()
+ '''
+ bitfield1 = 0b00000000 # Unknown function - needs experimenting
+ bitfield2 = 0b00000000 # Unknown function - needs experimenting
+ bitfield3 = 0b00000000 # Unknown function - needs experimenting
+ bitfield4 = 0b00000000 # Unknown function - needs experimenting
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring sensor stream".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['configureSensorStream'],
+ payload=[bitfield1, bitfield1, bitfield1, bitfield1])
+
+ self.getAcknowledgement("Sensor")
+
+ def configureSensorMask(self,
+ sample_rate_divisor = 0x25, # Must be > 0
+ packet_count = 0,
+ IMU_pitch = False,
+ IMU_roll = False,
+ IMU_yaw = False,
+ IMU_acc_x = False,
+ IMU_acc_y = False,
+ IMU_acc_z = False,
+ IMU_gyro_x = False,
+ IMU_gyro_y = False,
+ IMU_gyro_z = False):
+
+ '''
+ Send command to configure sensor mask using default values as found during bluetooth
+ sniffing of the Sphero Edu app. From experimentation, it seems that these are he functions of each:
+
+ Sampling_rate_divisor. Slow data EG: Set to 0x32 to the divide data rate by 50. Setting below 25 (0x19) causes
+ bluetooth errors
+
+ Packet_count: Select the number of packets to transmit before ending the stream. Set to zero to stream infinitely
+
+ All IMU bool parameters: Toggle transmission of that value on or off (e.g. set IMU_acc_x = True to include the
+ X-axis accelerometer readings in the sensor stream)
+ '''
+
+ # Construct bitfields based on function parameters:
+ IMU_bitfield1 = (IMU_pitch<<2) + (IMU_roll<<1) + IMU_yaw
+ IMU_bitfield2 = ((IMU_acc_y<<7) + (IMU_acc_z<<6) + (IMU_acc_x<<5) + \
+ (IMU_gyro_y<<4) + (IMU_gyro_x<<2) + (IMU_gyro_z<<2))
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring sensor mask".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensorMask'],
+ payload=[0x00, # Unknown param - altering it seems to slow data rate. Possibly averages multiple readings?
+ sample_rate_divisor,
+ packet_count, # Packet count: select the number of packets to stop streaming after (zero = infinite)
+ 0b00, # Unknown param: seems to be another accelerometer bitfield? Z-acc, Y-acc
+ IMU_bitfield1,
+ IMU_bitfield2,
+ 0b00]) # reserved, Position?, Position?, velocity?, velocity?, Y-gyro, timer, reserved
+
+ self.getAcknowledgement("Mask")
+
+ '''
+ Since the sensor values arrive as unlabelled lists in the order that they appear in the bitfields above, we need
+ to create a list of sensors that have been configured.Once we have this list, then in the default_delegate class,
+ we can get sensor values as attributes of the sphero_mini class.
+ e.g. print(sphero.IMU_yaw) # displays the current yaw angle
+ '''
+
+ # Initialize dictionary with sensor names as keys and their bool values (set by the user) as values:
+ availableSensors = {"IMU_pitch" : IMU_pitch,
+ "IMU_roll" : IMU_roll,
+ "IMU_yaw" : IMU_yaw,
+ "IMU_acc_y" : IMU_acc_y,
+ "IMU_acc_z" : IMU_acc_z,
+ "IMU_acc_x" : IMU_acc_x,
+ "IMU_gyro_y" : IMU_gyro_y,
+ "IMU_gyro_x" : IMU_gyro_x,
+ "IMU_gyro_z" : IMU_gyro_z}
+
+ # Create list of of only sensors that have been "activated" (set as true in the method arguments):
+ self.configured_sensors = [name for name in availableSensors if availableSensors[name] == True]
+
+ def sensor1(self): # Use default values
+ '''
+ Unknown function. Observed in bluetooth sniffing.
+ '''
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensor1'],
+ payload=[0x01])
+
+ self.getAcknowledgement("Sensor1")
+
+ def sensor2(self): # Use default values
+ '''
+ Unknown function. Observed in bluetooth sniffing.
+ '''
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensor2'],
+ payload=[0x00])
+
+ self.getAcknowledgement("Sensor2")
+
+# =======================================================================
+
+class MyDelegate(btle.DefaultDelegate):
+
+ '''
+ This class handles notifications (both responses and asynchronous notifications).
+
+ Usage of this class is described in the Bluepy documentation
+
+ '''
+
+ def __init__(self, sphero_class, user_delegate):
+ self.sphero_class = sphero_class # for saving sensor values as attributes of sphero class instance
+ self.user_delegate = user_delegate # to directly notify users of callbacks
+ btle.DefaultDelegate.__init__(self)
+ self.clear_notification()
+ self.notificationPacket = []
+
+ def clear_notification(self):
+ self.notification_ack = "DEFAULT ACK"
+ self.notification_seq = -1
+
+ def bits_to_num(self, bits):
+ '''
+ This helper function decodes bytes from sensor packets into single precision floats. Encoding follows the
+ the IEEE-754 standard.
+ '''
+ num = int(bits, 2).to_bytes(len(bits) // 8, byteorder='little')
+ num = struct.unpack('f', num)[0]
+ return num
+
+ def handleNotification(self, cHandle, data):
+ '''
+ This method acts as an interrupt service routine. When a notification comes in, this
+ method is invoked, with the variable 'cHandle' being the handle of the characteristic that
+ sent the notification, and 'data' being the payload (sent one byte at a time, so the packet
+ needs to be reconstructed)
+
+ The method keeps appending bytes to the payload packet byte list until end-of-packet byte is
+ encountered. Note that this is an issue, because 0xD8 could be sent as part of the payload of,
+ say, the battery voltage notification. In future, a more sophisticated method will be required.
+ '''
+ # Allow the user to intercept and process data first..
+ if self.user_delegate != None:
+ if self.user_delegate.handleNotification(cHandle, data):
+ return
+
+ #print("Received notification with packet ", str(data))
+
+ for data_byte in data: # parse each byte separately (sometimes they arrive simultaneously)
+
+ self.notificationPacket.append(data_byte) # Add new byte to packet list
+
+ # If end of packet (need to find a better way to segment the packets):
+ if data_byte == sendPacketConstants['EndOfPacket']:
+ # Once full the packet has arrived, parse it:
+ # Packet structure is similar to the outgoing send packets (see docstring in sphero_mini._send())
+
+ # Attempt to unpack. Might fail if packet is too badly corrupted
+ try:
+ start, flags_bits, devid, commcode, seq, *notification_payload, chsum, end = self.notificationPacket
+ except ValueError:
+ print("Warning: notification packet unparseable ", self.notificationPacket)
+ self.notificationPacket = [] # Discard this packet
+ return # exit
+
+ # Compute and append checksum and add EOP byte:
+ # From Sphero docs: "The [checksum is the] modulo 256 sum of all the bytes
+ # from the device ID through the end of the data payload,
+ # bit inverted (1's complement)"
+ # For the sphero mini, the flag bits must be included too:
+ checksum_bytes = [flags_bits, devid, commcode, seq] + notification_payload
+ checksum = 0 # init
+ for num in checksum_bytes:
+ checksum = (checksum + num) & 0xFF # bitwise "and to get modulo 256 sum of appropriate bytes
+ checksum = 0xff - checksum # bitwise 'not' to invert checksum bits
+ if checksum != chsum: # check computed checksum against that recieved in the packet
+ print("Warning: notification packet checksum failed - ", str(self.notificationPacket))
+ self.notificationPacket = [] # Discard this packet
+ return # exit
+
+ # Check if response packet:
+ if flags_bits & flags['isResponse']: # it is a response
+
+ # Use device ID and command code to determine which command is being acknowledged:
+ if devid == deviceID['powerInfo'] and commcode == powerCommandIDs['wake']:
+ self.notification_ack = "Wake acknowledged" # Acknowledgement after wake command
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands['driveWithHeading']:
+ self.notification_ack = "Roll command acknowledged"
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands['stabilization']:
+ self.notification_ack = "Stabilization command acknowledged"
+
+ elif devid == deviceID['userIO'] and commcode == userIOCommandIDs['allLEDs']:
+ self.notification_ack = "LED/backlight color command acknowledged"
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands["resetHeading"]:
+ self.notification_ack = "Heading reset command acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["configureCollision"]:
+ self.notification_ack = "Collision detection configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["configureSensorStream"]:
+ self.notification_ack = "Sensor stream configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensorMask"]:
+ self.notification_ack = "Mask configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensor1"]:
+ self.notification_ack = "Sensor1 acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensor2"]:
+ self.notification_ack = "Sensor2 acknowledged"
+
+ elif devid == deviceID['powerInfo'] and commcode == powerCommandIDs['batteryVoltage']:
+ V_batt = notification_payload[2] + notification_payload[1]*256 + notification_payload[0]*65536
+ V_batt /= 100 # Notification gives V_batt in 10mV increments. Divide by 100 to get to volts.
+ self.notification_ack = "Battery voltage:" + str(V_batt) + "v"
+ self.sphero_class.v_batt = V_batt
+
+ elif devid == deviceID['systemInfo'] and commcode == SystemInfoCommands['mainApplicationVersion']:
+ version = '.'.join(str(x) for x in notification_payload)
+ self.notification_ack = "Firmware version: " + version
+ self.sphero_class.firmware_version = notification_payload
+
+ else:
+ self.notification_ack = "Unknown acknowledgement" #print(self.notificationPacket)
+ print(self.notificationPacket, "===================> Unknown ack packet")
+
+ self.notification_seq = seq
+
+ else: # Not a response packet - therefore, asynchronous notification (e.g. collision detection, etc):
+
+ # Collision detection:
+ if devid == deviceID['sensor'] and commcode == sensorCommands['collisionDetectedAsync']:
+ # The first four bytes are data that is still un-parsed. the remaining unsaved bytes are always zeros
+ _, _, _, _, _, _, axis, _, Y_mag, _, X_mag, *_ = notification_payload
+ if axis == 1:
+ dir = "Left/right"
+ else:
+ dir = 'Forward/back'
+ print("Collision detected:")
+ print("\tAxis: ", dir)
+ print("\tX_mag: ", X_mag)
+ print("\tY_mag: ", Y_mag)
+
+ if self.sphero_class.collision_detection_callback is not None:
+ self.notificationPacket = [] # need to clear packet, in case new notification comes in during callback
+ self.sphero_class.collision_detection_callback()
+
+ # Sensor response:
+ elif devid == deviceID['sensor'] and commcode == sensorCommands['sensorResponse']:
+ # Convert to binary, pad bytes with leading zeros:
+ val = ''
+ for byte in notification_payload:
+ val += format(int(bin(byte)[2:], 2), '#010b')[2:]
+
+ # Break into 32-bit chunks
+ nums = []
+ while(len(val) > 0):
+ num, val = val[:32], val[32:] # Slice off first 16 bits
+ nums.append(num)
+
+ # convert from raw bits to float:
+ nums = [self.bits_to_num(num) for num in nums]
+
+ # Set sensor values as class attributes:
+ for name, value in zip(self.sphero_class.configured_sensors, nums):
+ print("Setting sensor at .", name, str(value))
+ setattr(self.sphero_class, name, value)
+
+ # Unrecognized packet structure:
+ else:
+ self.notification_ack = "Unknown asynchronous notification" #print(self.notificationPacket)
+ print(str(self.notificationPacket) + " ===================> Unknown async packet")
+
+ self.notificationPacket = [] # Start new payload after this byte
\ No newline at end of file
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/wavefront.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/wavefront.py
new file mode 100644
index 0000000000000000000000000000000000000000..1da387793ce36c131ca7ecc83355afdd5b70da7d
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/wavefront.py
@@ -0,0 +1,154 @@
+#!/usr/bin/env python
+import time
+import sys
+
+def newSearch(mapOfWorld, goal, start):
+ heap = []
+ newheap = []
+ x, y = goal
+ lastwave = 3
+ # Start out by marking nodes around G with a 3
+ moves = [(x + 1, y), (x - 1, y), (x, y - 1), (x, y + 1)]
+
+ for move in moves:
+ if(mapOfWorld.positions[move] == ' '):
+ mapOfWorld.positions[move] = 3
+ heap.append(move)
+ for currentwave in range(4, 10000):
+ lastwave = lastwave + 1
+ while(heap != []):
+ position = heap.pop()
+ (x, y) = position
+ moves = [(x + 1, y), (x - 1, y), (x, y + 1), (x, y - 1)]
+ #x, y = position
+ for move in moves:
+ if(mapOfWorld.positions[move] != 'W'):
+ if(mapOfWorld.positions[move] == ' ' and mapOfWorld.positions[position] == currentwave - 1):
+ mapOfWorld.positions[move] = currentwave
+ newheap.append(move)
+ if(move == start):
+ return mapOfWorld, lastwave
+
+ #time.sleep(0.25)
+ #mapOfWorld.display()
+ #print heap
+ if(newheap == []):
+ print("Goal is unreachable")
+ return 1
+ heap = newheap
+ newheap = []
+
+def printf(format, *args):
+ sys.stdout.write(format % args)
+
+class Map(object):
+
+ def __init__(self, xdim, ydim, positions):
+ self.xdim = xdim
+ self.ydim = ydim
+ self.positions = positions
+ def display(self):
+ printf(" ")
+ for i in range(self.ydim):
+ printf("%3s", str(i))
+ print
+ for x in range(self.xdim):
+ printf("%2s", str(x))
+ for y in range(self.ydim):
+ printf("%3s", str(self.positions[(x, y)]))
+ print
+ # Navigate though the number-populated maze
+ def nav(self, start, current):
+ self.pos = start
+ finished = False
+
+ while(finished == False): # Run this code until we're at the goal
+ x, y = self.pos
+ self.positions[self.pos] = 'R' # Set the start on the map (this USUALLY keeps start the same)
+ # SOUTH NORTH WEST EAST
+ # v v v v
+ moves = [(x + 1, y), (x - 1, y), (x, y - 1), (x, y + 1)] # Establish our directions
+ moveDirections = ["South", "North", "West", "East"] # Create a corresponding list of the cardinal directions
+ """ We don't want least to be 0, because then nothing would be less than it.
+ However, in order to make our code more robust, we set it to one of the values,
+ so that we're comparing least to an actual value instead of an arbitrary number (like 10).
+ """
+ # Do the actual comparing, and give us the least index so we know which move was the least
+ for w in range(len(moves)):
+ move = moves[w]
+
+ # If the position has the current wave - 1 in it, move there.
+ if(self.positions[move] == current - 1):
+ self.least = self.positions[move]
+ leastIndex = w
+ # Or, if the position is the goal, stop the loop
+ elif(self.positions[move] == 'G'):
+ finished = True
+ leastIndex = w
+ # Decrement the current number so we can look for the next number
+ current = current - 1
+ self.positions[self.pos] = ' '
+ print( "Moved " + moveDirections[leastIndex])
+ self.pos = moves[leastIndex] # This will be converted to "move robot in x direction"
+
+ #time.sleep(0.25)
+ #self.display()
+ # Change the goal position (or wherever we stop) to an "!" to show that we've arrived.
+ self.positions[self.pos] = '!'
+ self.display()
+# Find the goal, given the map
+def findGoal(mapOfWorld):
+ positions = mapOfWorld.positions
+ for x in range(mapOfWorld.xdim):
+ for y in range(mapOfWorld.ydim):
+ if(mapOfWorld.positions[(x, y)] == 'G'):
+ return (x, y)
+# Find the start, given the map
+def findStart(mapOfWorld):
+ positions = mapOfWorld.positions
+ for x in range(mapOfWorld.xdim):
+ for y in range(mapOfWorld.ydim):
+ if(mapOfWorld.positions[(x, y)] == 'R'):
+
+ return (x, y)
+
+def convertMap(mapOfWorld):
+ positions = {}
+ xdim = len(mapOfWorld)
+ ydim = len(mapOfWorld[1])
+ for y in range(ydim):
+ for x in range(xdim):
+ positions[(x, y)] = mapOfWorld[x][y]
+
+ return Map(xdim, ydim, positions)
+
+# Map erstellen
+# Standard imports
+import cv2
+import numpy as np;
+
+# Img Objekt
+cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+success, img = cap.read()
+
+gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
+mask = cv2.inRange(gray, np.array([90]), np.array([255]))
+
+mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+
+mask_list= np.ndarray.tolist(mask)
+
+for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['W' if j > 200 else ' ' for j in mask_list[i]]
+
+mapOfWorld[200][200] = 'R' #Start Pos
+mapOfWorld[300][300] = 'G' #Ziel Pos
+
+print(mapOfWorld[200][200])
+print(mapOfWorld[300][300])
+
+mapOfLand = convertMap(mapOfWorld)
+mapOfLand.display()
+mapOfLand, lastwave = newSearch(mapOfLand, findGoal(mapOfLand), findStart(mapOfLand))
+mapOfLand.nav(findStart(mapOfLand), lastwave)
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/wavefront_coverage_path_planner.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/wavefront_coverage_path_planner.py
new file mode 100644
index 0000000000000000000000000000000000000000..85861402d61f643d8a0639c708a494cca6383170
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/wavefront_coverage_path_planner.py
@@ -0,0 +1,218 @@
+"""
+Distance/Path Transform Wavefront Coverage Path Planner
+
+author: Todd Tang
+paper: Planning paths of complete coverage of an unstructured environment
+ by a mobile robot - Zelinsky et.al.
+link: http://pinkwink.kr/attachment/cfile3.uf@1354654A4E8945BD13FE77.pdf
+"""
+
+import os
+import sys
+
+import matplotlib.pyplot as plt
+import numpy as np
+from scipy import ndimage
+
+do_animation = True
+
+
+def transform(
+ grid_map, src, distance_type='chessboard',
+ transform_type='path', alpha=0.01
+):
+ """transform
+
+ calculating transform of transform_type from src
+ in given distance_type
+
+ :param grid_map: 2d binary map
+ :param src: distance transform source
+ :param distance_type: type of distance used
+ :param transform_type: type of transform used
+ :param alpha: weight of Obstacle Transform used when using path_transform
+ """
+
+ n_rows, n_cols = grid_map.shape
+
+ if n_rows == 0 or n_cols == 0:
+ sys.exit('Empty grid_map.')
+
+ inc_order = [[0, 1], [1, 1], [1, 0], [1, -1],
+ [0, -1], [-1, -1], [-1, 0], [-1, 1]]
+ if distance_type == 'chessboard':
+ cost = [1, 1, 1, 1, 1, 1, 1, 1]
+ elif distance_type == 'eculidean':
+ cost = [1, np.sqrt(2), 1, np.sqrt(2), 1, np.sqrt(2), 1, np.sqrt(2)]
+ else:
+ sys.exit('Unsupported distance type.')
+
+ transform_matrix = float('inf') * np.ones_like(grid_map, dtype=float)
+ transform_matrix[src[0], src[1]] = 0
+ if transform_type == 'distance':
+ eT = np.zeros_like(grid_map)
+ elif transform_type == 'path':
+ eT = ndimage.distance_transform_cdt(1 - grid_map, distance_type)
+ else:
+ sys.exit('Unsupported transform type.')
+
+ # set obstacle transform_matrix value to infinity
+ for i in range(n_rows):
+ for j in range(n_cols):
+ if grid_map[i][j] == 1.0:
+ transform_matrix[i][j] = float('inf')
+ is_visited = np.zeros_like(transform_matrix, dtype=bool)
+ is_visited[src[0], src[1]] = True
+ traversal_queue = [src]
+ calculated = [(src[0] - 1) * n_cols + src[1]]
+
+ def is_valid_neighbor(g_i, g_j):
+ return 0 <= g_i < n_rows and 0 <= g_j < n_cols \
+ and not grid_map[g_i][g_j]
+
+ while traversal_queue:
+ i, j = traversal_queue.pop(0)
+ for k, inc in enumerate(inc_order):
+ ni = i + inc[0]
+ nj = j + inc[1]
+ if is_valid_neighbor(ni, nj):
+ is_visited[i][j] = True
+
+ # update transform_matrix
+ transform_matrix[i][j] = min(
+ transform_matrix[i][j],
+ transform_matrix[ni][nj] + cost[k] + alpha * eT[ni][nj])
+
+ if not is_visited[ni][nj] \
+ and ((ni - 1) * n_cols + nj) not in calculated:
+ traversal_queue.append((ni, nj))
+ calculated.append((ni - 1) * n_cols + nj)
+
+ return transform_matrix
+
+
+def get_search_order_increment(start, goal):
+ if start[0] >= goal[0] and start[1] >= goal[1]:
+ order = [[1, 0], [0, 1], [-1, 0], [0, -1],
+ [1, 1], [1, -1], [-1, 1], [-1, -1]]
+ elif start[0] <= goal[0] and start[1] >= goal[1]:
+ order = [[-1, 0], [0, 1], [1, 0], [0, -1],
+ [-1, 1], [-1, -1], [1, 1], [1, -1]]
+ elif start[0] >= goal[0] and start[1] <= goal[1]:
+ order = [[1, 0], [0, -1], [-1, 0], [0, 1],
+ [1, -1], [-1, -1], [1, 1], [-1, 1]]
+ elif start[0] <= goal[0] and start[1] <= goal[1]:
+ order = [[-1, 0], [0, -1], [0, 1], [1, 0],
+ [-1, -1], [-1, 1], [1, -1], [1, 1]]
+ else:
+ sys.exit('get_search_order_increment: cannot determine \
+ start=>goal increment order')
+ return order
+
+
+def wavefront(transform_matrix, start, goal):
+ """wavefront
+
+ performing wavefront coverage path planning
+
+ :param transform_matrix: the transform matrix
+ :param start: start point of planning
+ :param goal: goal point of planning
+ """
+
+ path = []
+ n_rows, n_cols = transform_matrix.shape
+
+ def is_valid_neighbor(g_i, g_j):
+ is_i_valid_bounded = 0 <= g_i < n_rows
+ is_j_valid_bounded = 0 <= g_j < n_cols
+ if is_i_valid_bounded and is_j_valid_bounded:
+ return not is_visited[g_i][g_j] and \
+ transform_matrix[g_i][g_j] != float('inf')
+ return False
+
+ inc_order = get_search_order_increment(start, goal)
+
+ current_node = start
+ is_visited = np.zeros_like(transform_matrix, dtype=bool)
+
+ while current_node != goal:
+ i, j = current_node
+ path.append((i, j))
+ is_visited[i][j] = True
+
+ max_T = float('-inf')
+ i_max = (-1, -1)
+ i_last = 0
+ for i_last in range(len(path)):
+ current_node = path[-1 - i_last] # get latest node in path
+ for ci, cj in inc_order:
+ ni, nj = current_node[0] + ci, current_node[1] + cj
+ if is_valid_neighbor(ni, nj) and \
+ transform_matrix[ni][nj] > max_T:
+ i_max = (ni, nj)
+ max_T = transform_matrix[ni][nj]
+
+ if i_max != (-1, -1):
+ break
+
+ if i_max == (-1, -1):
+ break
+ else:
+ current_node = i_max
+ if i_last != 0:
+ print('backtracing to', current_node)
+ path.append(goal)
+
+ return path
+
+
+def visualize_path(grid_map, start, goal, path): # pragma: no cover
+ oy, ox = start
+ gy, gx = goal
+ px, py = np.transpose(np.flipud(np.fliplr(path)))
+
+ if not do_animation:
+ plt.imshow(grid_map, cmap='Greys')
+ plt.plot(ox, oy, "-xy")
+ plt.plot(px, py, "-r")
+ plt.plot(gx, gy, "-pg")
+ plt.show()
+ else:
+ for ipx, ipy in zip(px, py):
+ plt.cla()
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect(
+ 'key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
+ plt.imshow(grid_map, cmap='Greys')
+ plt.plot(ox, oy, "-xb")
+ plt.plot(px, py, "-r")
+ plt.plot(gx, gy, "-pg")
+ plt.plot(ipx, ipy, "or")
+ plt.axis("equal")
+ plt.grid(True)
+ plt.pause(0.1)
+
+
+def main():
+ dir_path = os.path.dirname(os.path.realpath(__file__))
+ img = plt.imread(os.path.join(dir_path, 'map', 'test_2.png'))
+ img = 1 - img # revert pixel values
+
+ start = (43, 0)
+ goal = (0, 0)
+
+ # distance transform wavefront
+ DT = transform(img, goal, transform_type='distance')
+ DT_path = wavefront(DT, start, goal)
+ visualize_path(img, start, goal, DT_path)
+
+ # path transform wavefront
+ PT = transform(img, goal, transform_type='path', alpha=0.01)
+ PT_path = wavefront(PT, start, goal)
+ visualize_path(img, start, goal, PT_path)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/wavefrontgpt2.py b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/wavefrontgpt2.py
new file mode 100644
index 0000000000000000000000000000000000000000..ef76c9f457ef6872c3b242d9b6815864c9ed7a5c
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/wavefrontgpt2.py
@@ -0,0 +1,76 @@
+from queue import Queue
+
+def wavefront_planner(map_array, start_pos, goal_pos):
+ rows = len(map_array)
+ cols = len(map_array[0])
+
+ # Überprüfe, ob Start- und Zielposition innerhalb der Karte liegen
+ if (start_pos[0] < 0 or start_pos[0] >= rows or start_pos[1] < 0 or start_pos[1] >= cols or
+ goal_pos[0] < 0 or goal_pos[0] >= rows or goal_pos[1] < 0 or goal_pos[1] >= cols):
+ raise ValueError("Start or goal position is out of bounds.")
+
+ # Erzeuge eine Kopie der Karte für den Wavefront-Algorithmus
+ wavefront_map = [[-1] * cols for _ in range(rows)]
+
+ # Definiere die Bewegungsrichtungen (4-Wege-Bewegung: oben, unten, links, rechts)
+ directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]
+
+ # Erzeuge eine Warteschlange für die Wellenfrontausbreitung
+ queue = Queue()
+ queue.put(goal_pos)
+
+ # Führe die Wellenfrontausbreitung durch
+ wavefront_map[goal_pos[0]][goal_pos[1]] = 0
+ while not queue.empty():
+ current_pos = queue.get()
+
+ # Überprüfe die Nachbarzellen
+ for direction in directions:
+ new_pos = (current_pos[0] + direction[0], current_pos[1] + direction[1])
+
+ # Überprüfe, ob die Nachbarzelle gültig und noch nicht besucht ist
+ if (0 <= new_pos[0] < rows and 0 <= new_pos[1] < cols and
+ wavefront_map[new_pos[0]][new_pos[1]] == -1 and map_array[new_pos[0]][new_pos[1]] == 0):
+ wavefront_map[new_pos[0]][new_pos[1]] = wavefront_map[current_pos[0]][current_pos[1]] + 1
+ queue.put(new_pos)
+
+ # Überprüfe, ob der Startpunkt erreichbar ist
+ if wavefront_map[start_pos[0]][start_pos[1]] == -1:
+ raise ValueError("Start position is unreachable.")
+
+ # Verfolge den Pfad basierend auf der Wellenfront
+ path = [start_pos]
+ current_pos = start_pos
+ while current_pos != goal_pos:
+ next_pos = None
+ min_distance = float('inf')
+
+ for direction in directions:
+ neighbor_pos = (current_pos[0] + direction[0], current_pos[1] + direction[1])
+
+ if (0 <= neighbor_pos[0] < rows and 0 <= neighbor_pos[1] < cols and
+ wavefront_map[neighbor_pos[0]][neighbor_pos[1]] < min_distance):
+ next_pos = neighbor_pos
+ min_distance = wavefront_map[neighbor_pos[0]][neighbor_pos[1]]
+
+ if next_pos is None:
+ raise ValueError("No path found.")
+
+ path.append(next_pos)
+ current_pos = next_pos
+
+ return path
+
+# Beispielverwendung
+map_array = [
+ [0, 0, 0, 0],
+ [0, 1, 1, 0],
+ [0, 0, 0, 0],
+ [0, 1, 1, 0],
+]
+
+start_pos = (0, 0)
+goal_pos = (1, 3)
+
+path = wavefront_planner(map_array, start_pos, goal_pos)
+print("Path:", path)
diff --git a/ros2_ws/build/sphero_mini_controller/colcon_build.rc b/ros2_ws/build/sphero_mini_controller/colcon_build.rc
new file mode 100644
index 0000000000000000000000000000000000000000..573541ac9702dd3969c9bc859d2b91ec1f7e6e56
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/colcon_build.rc
@@ -0,0 +1 @@
+0
diff --git a/ros2_ws/build/sphero_mini_controller/colcon_command_prefix_setup_py.sh b/ros2_ws/build/sphero_mini_controller/colcon_command_prefix_setup_py.sh
new file mode 100644
index 0000000000000000000000000000000000000000..f9867d51322a8ef47d4951080db6e3cfd048835e
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/colcon_command_prefix_setup_py.sh
@@ -0,0 +1 @@
+# generated from colcon_core/shell/template/command_prefix.sh.em
diff --git a/ros2_ws/build/sphero_mini_controller/colcon_command_prefix_setup_py.sh.env b/ros2_ws/build/sphero_mini_controller/colcon_command_prefix_setup_py.sh.env
new file mode 100644
index 0000000000000000000000000000000000000000..0e1cc2ad36a65414149f48cf06801963696c85bf
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/colcon_command_prefix_setup_py.sh.env
@@ -0,0 +1,61 @@
+AMENT_PREFIX_PATH=/opt/ros/humble
+COLCON=1
+COLORTERM=truecolor
+DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/999/bus
+DESKTOP_SESSION=ubuntu
+DISPLAY=:0
+GDMSESSION=ubuntu
+GNOME_DESKTOP_SESSION_ID=this-is-deprecated
+GNOME_SHELL_SESSION_MODE=ubuntu
+GNOME_TERMINAL_SCREEN=/org/gnome/Terminal/screen/211b2ac6_0a43_44d5_b3bf_a0e22ff3787d
+GNOME_TERMINAL_SERVICE=:1.128
+GPG_AGENT_INFO=/run/user/999/gnupg/S.gpg-agent:0:1
+GTK_MODULES=gail:atk-bridge
+HOME=/home/ubuntu
+IM_CONFIG_PHASE=1
+LANG=de_DE.UTF-8
+LANGUAGE=de_DE:en
+LC_ADDRESS=de_DE.UTF-8
+LC_IDENTIFICATION=de_DE.UTF-8
+LC_MEASUREMENT=de_DE.UTF-8
+LC_MONETARY=de_DE.UTF-8
+LC_NAME=de_DE.UTF-8
+LC_NUMERIC=de_DE.UTF-8
+LC_PAPER=de_DE.UTF-8
+LC_TELEPHONE=de_DE.UTF-8
+LC_TIME=de_DE.UTF-8
+LD_LIBRARY_PATH=/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib
+LOGNAME=ubuntu
+OLDPWD=/home/ubuntu
+PAPERSIZE=a4
+PATH=/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:/snap/bin
+PWD=/home/ubuntu/ros2_ws/build/sphero_mini_controller
+PYTHONPATH=/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages
+QT_ACCESSIBILITY=1
+QT_IM_MODULE=ibus
+ROS_DISTRO=humble
+ROS_LOCALHOST_ONLY=0
+ROS_PYTHON_VERSION=3
+ROS_VERSION=2
+SESSION_MANAGER=local/ubuntu:@/tmp/.ICE-unix/2661,unix/ubuntu:/tmp/.ICE-unix/2661
+SHELL=/bin/bash
+SHLVL=1
+SSH_AGENT_LAUNCHER=gnome-keyring
+SSH_AUTH_SOCK=/run/user/999/keyring/ssh
+SYSTEMD_EXEC_PID=2684
+TERM=xterm-256color
+USER=ubuntu
+USERNAME=ubuntu
+VTE_VERSION=6800
+WINDOWPATH=2
+XAUTHORITY=/run/user/999/gdm/Xauthority
+XDG_CONFIG_DIRS=/etc/xdg/xdg-ubuntu:/etc/xdg
+XDG_CURRENT_DESKTOP=ubuntu:GNOME
+XDG_DATA_DIRS=/usr/share/ubuntu:/usr/share/gnome:/usr/local/share/:/usr/share/:/var/lib/snapd/desktop
+XDG_MENU_PREFIX=gnome-
+XDG_RUNTIME_DIR=/run/user/999
+XDG_SESSION_CLASS=user
+XDG_SESSION_DESKTOP=ubuntu
+XDG_SESSION_TYPE=x11
+XMODIFIERS=@im=ibus
+_=/usr/bin/colcon
diff --git a/ros2_ws/build/sphero_mini_controller/install.log b/ros2_ws/build/sphero_mini_controller/install.log
new file mode 100644
index 0000000000000000000000000000000000000000..0881f1a7a57e5ec89b61b89766677395d6078cc4
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/install.log
@@ -0,0 +1,22 @@
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node.py
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__init__.py
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/WavefrontPlanner.py
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/treiber.py
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/_constants.py
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/blobErkennung.py
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node.cpython-310.pyc
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/__init__.cpython-310.pyc
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/WavefrontPlanner.cpython-310.pyc
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/treiber.cpython-310.pyc
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/_constants.cpython-310.pyc
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/blobErkennung.cpython-310.pyc
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/ament_index/resource_index/packages/sphero_mini_controller
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.xml
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/entry_points.txt
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/top_level.txt
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/zip-safe
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/PKG-INFO
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/SOURCES.txt
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/requires.txt
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/dependency_links.txt
+/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/sphero_mini_controller/sphero_mini
diff --git a/ros2_ws/build/sphero_mini_controller/prefix_override/__pycache__/sitecustomize.cpython-310.pyc b/ros2_ws/build/sphero_mini_controller/prefix_override/__pycache__/sitecustomize.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..2bd0054d0a2debc06129b0ef052f83ce7d0b4c8f
Binary files /dev/null and b/ros2_ws/build/sphero_mini_controller/prefix_override/__pycache__/sitecustomize.cpython-310.pyc differ
diff --git a/ros2_ws/build/sphero_mini_controller/prefix_override/sitecustomize.py b/ros2_ws/build/sphero_mini_controller/prefix_override/sitecustomize.py
new file mode 100644
index 0000000000000000000000000000000000000000..231b97a501846f86cbc275a9fe99486605ce8148
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/prefix_override/sitecustomize.py
@@ -0,0 +1,3 @@
+import sys
+sys.real_prefix = sys.prefix
+sys.prefix = sys.exec_prefix = '/home/ubuntu/ros2_ws/install/sphero_mini_controller'
diff --git a/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/PKG-INFO b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/PKG-INFO
new file mode 100644
index 0000000000000000000000000000000000000000..1772b2b4b87ed715b1c9529109e2a64ffa956ef2
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/PKG-INFO
@@ -0,0 +1,12 @@
+Metadata-Version: 2.1
+Name: sphero-mini-controller
+Version: 0.0.0
+Summary: TODO: Package description
+Home-page: UNKNOWN
+Maintainer: ubuntu
+Maintainer-email: ubuntu@todo.todo
+License: TODO: License declaration
+Platform: UNKNOWN
+
+UNKNOWN
+
diff --git a/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt
new file mode 100644
index 0000000000000000000000000000000000000000..450bb514f818ddd322e445deee994a8cbb2136e8
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt
@@ -0,0 +1,20 @@
+package.xml
+setup.cfg
+setup.py
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/PKG-INFO
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/dependency_links.txt
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/entry_points.txt
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/requires.txt
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/top_level.txt
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/zip-safe
+resource/sphero_mini_controller
+sphero_mini_controller/WavefrontPlanner.py
+sphero_mini_controller/__init__.py
+sphero_mini_controller/_constants.py
+sphero_mini_controller/blobErkennung.py
+sphero_mini_controller/my_first_node.py
+sphero_mini_controller/treiber.py
+test/test_copyright.py
+test/test_flake8.py
+test/test_pep257.py
\ No newline at end of file
diff --git a/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/dependency_links.txt b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/dependency_links.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/entry_points.txt b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/entry_points.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6a40ae61982b9df335a55fd38c98ba80a816cb93
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/entry_points.txt
@@ -0,0 +1,3 @@
+[console_scripts]
+sphero_mini = sphero_mini_controller.my_first_node:main
+
diff --git a/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/requires.txt b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/requires.txt
new file mode 100644
index 0000000000000000000000000000000000000000..49fe098d9e6bccd89482b34510da60ab28556880
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/requires.txt
@@ -0,0 +1 @@
+setuptools
diff --git a/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/top_level.txt b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/top_level.txt
new file mode 100644
index 0000000000000000000000000000000000000000..60845dab3412fa47e2fb49f683a3db10ce25c01e
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/top_level.txt
@@ -0,0 +1 @@
+sphero_mini_controller
diff --git a/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/zip-safe b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/zip-safe
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/ros2_ws/build/sphero_mini_controller/sphero_mini_controller.egg-info/zip-safe
@@ -0,0 +1 @@
+
diff --git a/ros2_ws/install/.colcon_install_layout b/ros2_ws/install/.colcon_install_layout
new file mode 100644
index 0000000000000000000000000000000000000000..3aad5336af1f22b8088508218dceeda3d7bc8cc2
--- /dev/null
+++ b/ros2_ws/install/.colcon_install_layout
@@ -0,0 +1 @@
+isolated
diff --git a/ros2_ws/install/COLCON_IGNORE b/ros2_ws/install/COLCON_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2_ws/install/_local_setup_util_ps1.py b/ros2_ws/install/_local_setup_util_ps1.py
new file mode 100644
index 0000000000000000000000000000000000000000..98348eebcfb4cc97ed554c9864b4a1811af7fb49
--- /dev/null
+++ b/ros2_ws/install/_local_setup_util_ps1.py
@@ -0,0 +1,404 @@
+# Copyright 2016-2019 Dirk Thomas
+# Licensed under the Apache License, Version 2.0
+
+import argparse
+from collections import OrderedDict
+import os
+from pathlib import Path
+import sys
+
+
+FORMAT_STR_COMMENT_LINE = '# {comment}'
+FORMAT_STR_SET_ENV_VAR = 'Set-Item -Path "Env:{name}" -Value "{value}"'
+FORMAT_STR_USE_ENV_VAR = '$env:{name}'
+FORMAT_STR_INVOKE_SCRIPT = '_colcon_prefix_powershell_source_script "{script_path}"'
+FORMAT_STR_REMOVE_LEADING_SEPARATOR = ''
+FORMAT_STR_REMOVE_TRAILING_SEPARATOR = ''
+
+DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists'
+DSV_TYPE_SET = 'set'
+DSV_TYPE_SET_IF_UNSET = 'set-if-unset'
+DSV_TYPE_SOURCE = 'source'
+
+
+def main(argv=sys.argv[1:]): # noqa: D103
+ parser = argparse.ArgumentParser(
+ description='Output shell commands for the packages in topological '
+ 'order')
+ parser.add_argument(
+ 'primary_extension',
+ help='The file extension of the primary shell')
+ parser.add_argument(
+ 'additional_extension', nargs='?',
+ help='The additional file extension to be considered')
+ parser.add_argument(
+ '--merged-install', action='store_true',
+ help='All install prefixes are merged into a single location')
+ args = parser.parse_args(argv)
+
+ packages = get_packages(Path(__file__).parent, args.merged_install)
+
+ ordered_packages = order_packages(packages)
+ for pkg_name in ordered_packages:
+ if _include_comments():
+ print(
+ FORMAT_STR_COMMENT_LINE.format_map(
+ {'comment': 'Package: ' + pkg_name}))
+ prefix = os.path.abspath(os.path.dirname(__file__))
+ if not args.merged_install:
+ prefix = os.path.join(prefix, pkg_name)
+ for line in get_commands(
+ pkg_name, prefix, args.primary_extension,
+ args.additional_extension
+ ):
+ print(line)
+
+ for line in _remove_ending_separators():
+ print(line)
+
+
+def get_packages(prefix_path, merged_install):
+ """
+ Find packages based on colcon-specific files created during installation.
+
+ :param Path prefix_path: The install prefix path of all packages
+ :param bool merged_install: The flag if the packages are all installed
+ directly in the prefix or if each package is installed in a subdirectory
+ named after the package
+ :returns: A mapping from the package name to the set of runtime
+ dependencies
+ :rtype: dict
+ """
+ packages = {}
+ # since importing colcon_core isn't feasible here the following constant
+ # must match colcon_core.location.get_relative_package_index_path()
+ subdirectory = 'share/colcon-core/packages'
+ if merged_install:
+ # return if workspace is empty
+ if not (prefix_path / subdirectory).is_dir():
+ return packages
+ # find all files in the subdirectory
+ for p in (prefix_path / subdirectory).iterdir():
+ if not p.is_file():
+ continue
+ if p.name.startswith('.'):
+ continue
+ add_package_runtime_dependencies(p, packages)
+ else:
+ # for each subdirectory look for the package specific file
+ for p in prefix_path.iterdir():
+ if not p.is_dir():
+ continue
+ if p.name.startswith('.'):
+ continue
+ p = p / subdirectory / p.name
+ if p.is_file():
+ add_package_runtime_dependencies(p, packages)
+
+ # remove unknown dependencies
+ pkg_names = set(packages.keys())
+ for k in packages.keys():
+ packages[k] = {d for d in packages[k] if d in pkg_names}
+
+ return packages
+
+
+def add_package_runtime_dependencies(path, packages):
+ """
+ Check the path and if it exists extract the packages runtime dependencies.
+
+ :param Path path: The resource file containing the runtime dependencies
+ :param dict packages: A mapping from package names to the sets of runtime
+ dependencies to add to
+ """
+ content = path.read_text()
+ dependencies = set(content.split(os.pathsep) if content else [])
+ packages[path.name] = dependencies
+
+
+def order_packages(packages):
+ """
+ Order packages topologically.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies
+ :returns: The package names
+ :rtype: list
+ """
+ # select packages with no dependencies in alphabetical order
+ to_be_ordered = list(packages.keys())
+ ordered = []
+ while to_be_ordered:
+ pkg_names_without_deps = [
+ name for name in to_be_ordered if not packages[name]]
+ if not pkg_names_without_deps:
+ reduce_cycle_set(packages)
+ raise RuntimeError(
+ 'Circular dependency between: ' + ', '.join(sorted(packages)))
+ pkg_names_without_deps.sort()
+ pkg_name = pkg_names_without_deps[0]
+ to_be_ordered.remove(pkg_name)
+ ordered.append(pkg_name)
+ # remove item from dependency lists
+ for k in list(packages.keys()):
+ if pkg_name in packages[k]:
+ packages[k].remove(pkg_name)
+ return ordered
+
+
+def reduce_cycle_set(packages):
+ """
+ Reduce the set of packages to the ones part of the circular dependency.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies which is modified in place
+ """
+ last_depended = None
+ while len(packages) > 0:
+ # get all remaining dependencies
+ depended = set()
+ for pkg_name, dependencies in packages.items():
+ depended = depended.union(dependencies)
+ # remove all packages which are not dependent on
+ for name in list(packages.keys()):
+ if name not in depended:
+ del packages[name]
+ if last_depended:
+ # if remaining packages haven't changed return them
+ if last_depended == depended:
+ return packages.keys()
+ # otherwise reduce again
+ last_depended = depended
+
+
+def _include_comments():
+ # skipping comment lines when COLCON_TRACE is not set speeds up the
+ # processing especially on Windows
+ return bool(os.environ.get('COLCON_TRACE'))
+
+
+def get_commands(pkg_name, prefix, primary_extension, additional_extension):
+ commands = []
+ package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv')
+ if os.path.exists(package_dsv_path):
+ commands += process_dsv_file(
+ package_dsv_path, prefix, primary_extension, additional_extension)
+ return commands
+
+
+def process_dsv_file(
+ dsv_path, prefix, primary_extension=None, additional_extension=None
+):
+ commands = []
+ if _include_comments():
+ commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
+ with open(dsv_path, 'r') as h:
+ content = h.read()
+ lines = content.splitlines()
+
+ basenames = OrderedDict()
+ for i, line in enumerate(lines):
+ # skip over empty or whitespace-only lines
+ if not line.strip():
+ continue
+ try:
+ type_, remainder = line.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "Line %d in '%s' doesn't contain a semicolon separating the "
+ 'type from the arguments' % (i + 1, dsv_path))
+ if type_ != DSV_TYPE_SOURCE:
+ # handle non-source lines
+ try:
+ commands += handle_dsv_types_except_source(
+ type_, remainder, prefix)
+ except RuntimeError as e:
+ raise RuntimeError(
+ "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e
+ else:
+ # group remaining source lines by basename
+ path_without_ext, ext = os.path.splitext(remainder)
+ if path_without_ext not in basenames:
+ basenames[path_without_ext] = set()
+ assert ext.startswith('.')
+ ext = ext[1:]
+ if ext in (primary_extension, additional_extension):
+ basenames[path_without_ext].add(ext)
+
+ # add the dsv extension to each basename if the file exists
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if os.path.exists(basename + '.dsv'):
+ extensions.add('dsv')
+
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if 'dsv' in extensions:
+ # process dsv files recursively
+ commands += process_dsv_file(
+ basename + '.dsv', prefix, primary_extension=primary_extension,
+ additional_extension=additional_extension)
+ elif primary_extension in extensions and len(extensions) == 1:
+ # source primary-only files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + primary_extension})]
+ elif additional_extension in extensions:
+ # source non-primary files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + additional_extension})]
+
+ return commands
+
+
+def handle_dsv_types_except_source(type_, remainder, prefix):
+ commands = []
+ if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET):
+ try:
+ env_name, value = remainder.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the value')
+ try_prefixed_value = os.path.join(prefix, value) if value else prefix
+ if os.path.exists(try_prefixed_value):
+ value = try_prefixed_value
+ if type_ == DSV_TYPE_SET:
+ commands += _set(env_name, value)
+ elif type_ == DSV_TYPE_SET_IF_UNSET:
+ commands += _set_if_unset(env_name, value)
+ else:
+ assert False
+ elif type_ in (
+ DSV_TYPE_APPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS
+ ):
+ try:
+ env_name_and_values = remainder.split(';')
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the values')
+ env_name = env_name_and_values[0]
+ values = env_name_and_values[1:]
+ for value in values:
+ if not value:
+ value = prefix
+ elif not os.path.isabs(value):
+ value = os.path.join(prefix, value)
+ if (
+ type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and
+ not os.path.exists(value)
+ ):
+ comment = f'skip extending {env_name} with not existing ' \
+ f'path: {value}'
+ if _include_comments():
+ commands.append(
+ FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
+ elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
+ commands += _append_unique_value(env_name, value)
+ else:
+ commands += _prepend_unique_value(env_name, value)
+ else:
+ raise RuntimeError(
+ 'contains an unknown environment hook type: ' + type_)
+ return commands
+
+
+env_state = {}
+
+
+def _append_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # append even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional leading separator
+ extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': extend + value})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+def _prepend_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # prepend even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional trailing separator
+ extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value + extend})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+# generate commands for removing prepended underscores
+def _remove_ending_separators():
+ # do nothing if the shell extension does not implement the logic
+ if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
+ return []
+
+ global env_state
+ commands = []
+ for name in env_state:
+ # skip variables that already had values before this script started prepending
+ if name in os.environ:
+ continue
+ commands += [
+ FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}),
+ FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})]
+ return commands
+
+
+def _set(name, value):
+ global env_state
+ env_state[name] = value
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ return [line]
+
+
+def _set_if_unset(name, value):
+ global env_state
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ if env_state.get(name, os.environ.get(name)):
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+if __name__ == '__main__': # pragma: no cover
+ try:
+ rc = main()
+ except RuntimeError as e:
+ print(str(e), file=sys.stderr)
+ rc = 1
+ sys.exit(rc)
diff --git a/ros2_ws/install/_local_setup_util_sh.py b/ros2_ws/install/_local_setup_util_sh.py
new file mode 100644
index 0000000000000000000000000000000000000000..35c017b2558a0b0a148d67778c1ae302f023361d
--- /dev/null
+++ b/ros2_ws/install/_local_setup_util_sh.py
@@ -0,0 +1,404 @@
+# Copyright 2016-2019 Dirk Thomas
+# Licensed under the Apache License, Version 2.0
+
+import argparse
+from collections import OrderedDict
+import os
+from pathlib import Path
+import sys
+
+
+FORMAT_STR_COMMENT_LINE = '# {comment}'
+FORMAT_STR_SET_ENV_VAR = 'export {name}="{value}"'
+FORMAT_STR_USE_ENV_VAR = '${name}'
+FORMAT_STR_INVOKE_SCRIPT = 'COLCON_CURRENT_PREFIX="{prefix}" _colcon_prefix_sh_source_script "{script_path}"'
+FORMAT_STR_REMOVE_LEADING_SEPARATOR = 'if [ "$(echo -n ${name} | head -c 1)" = ":" ]; then export {name}=${{{name}#?}} ; fi'
+FORMAT_STR_REMOVE_TRAILING_SEPARATOR = 'if [ "$(echo -n ${name} | tail -c 1)" = ":" ]; then export {name}=${{{name}%?}} ; fi'
+
+DSV_TYPE_APPEND_NON_DUPLICATE = 'append-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE = 'prepend-non-duplicate'
+DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS = 'prepend-non-duplicate-if-exists'
+DSV_TYPE_SET = 'set'
+DSV_TYPE_SET_IF_UNSET = 'set-if-unset'
+DSV_TYPE_SOURCE = 'source'
+
+
+def main(argv=sys.argv[1:]): # noqa: D103
+ parser = argparse.ArgumentParser(
+ description='Output shell commands for the packages in topological '
+ 'order')
+ parser.add_argument(
+ 'primary_extension',
+ help='The file extension of the primary shell')
+ parser.add_argument(
+ 'additional_extension', nargs='?',
+ help='The additional file extension to be considered')
+ parser.add_argument(
+ '--merged-install', action='store_true',
+ help='All install prefixes are merged into a single location')
+ args = parser.parse_args(argv)
+
+ packages = get_packages(Path(__file__).parent, args.merged_install)
+
+ ordered_packages = order_packages(packages)
+ for pkg_name in ordered_packages:
+ if _include_comments():
+ print(
+ FORMAT_STR_COMMENT_LINE.format_map(
+ {'comment': 'Package: ' + pkg_name}))
+ prefix = os.path.abspath(os.path.dirname(__file__))
+ if not args.merged_install:
+ prefix = os.path.join(prefix, pkg_name)
+ for line in get_commands(
+ pkg_name, prefix, args.primary_extension,
+ args.additional_extension
+ ):
+ print(line)
+
+ for line in _remove_ending_separators():
+ print(line)
+
+
+def get_packages(prefix_path, merged_install):
+ """
+ Find packages based on colcon-specific files created during installation.
+
+ :param Path prefix_path: The install prefix path of all packages
+ :param bool merged_install: The flag if the packages are all installed
+ directly in the prefix or if each package is installed in a subdirectory
+ named after the package
+ :returns: A mapping from the package name to the set of runtime
+ dependencies
+ :rtype: dict
+ """
+ packages = {}
+ # since importing colcon_core isn't feasible here the following constant
+ # must match colcon_core.location.get_relative_package_index_path()
+ subdirectory = 'share/colcon-core/packages'
+ if merged_install:
+ # return if workspace is empty
+ if not (prefix_path / subdirectory).is_dir():
+ return packages
+ # find all files in the subdirectory
+ for p in (prefix_path / subdirectory).iterdir():
+ if not p.is_file():
+ continue
+ if p.name.startswith('.'):
+ continue
+ add_package_runtime_dependencies(p, packages)
+ else:
+ # for each subdirectory look for the package specific file
+ for p in prefix_path.iterdir():
+ if not p.is_dir():
+ continue
+ if p.name.startswith('.'):
+ continue
+ p = p / subdirectory / p.name
+ if p.is_file():
+ add_package_runtime_dependencies(p, packages)
+
+ # remove unknown dependencies
+ pkg_names = set(packages.keys())
+ for k in packages.keys():
+ packages[k] = {d for d in packages[k] if d in pkg_names}
+
+ return packages
+
+
+def add_package_runtime_dependencies(path, packages):
+ """
+ Check the path and if it exists extract the packages runtime dependencies.
+
+ :param Path path: The resource file containing the runtime dependencies
+ :param dict packages: A mapping from package names to the sets of runtime
+ dependencies to add to
+ """
+ content = path.read_text()
+ dependencies = set(content.split(os.pathsep) if content else [])
+ packages[path.name] = dependencies
+
+
+def order_packages(packages):
+ """
+ Order packages topologically.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies
+ :returns: The package names
+ :rtype: list
+ """
+ # select packages with no dependencies in alphabetical order
+ to_be_ordered = list(packages.keys())
+ ordered = []
+ while to_be_ordered:
+ pkg_names_without_deps = [
+ name for name in to_be_ordered if not packages[name]]
+ if not pkg_names_without_deps:
+ reduce_cycle_set(packages)
+ raise RuntimeError(
+ 'Circular dependency between: ' + ', '.join(sorted(packages)))
+ pkg_names_without_deps.sort()
+ pkg_name = pkg_names_without_deps[0]
+ to_be_ordered.remove(pkg_name)
+ ordered.append(pkg_name)
+ # remove item from dependency lists
+ for k in list(packages.keys()):
+ if pkg_name in packages[k]:
+ packages[k].remove(pkg_name)
+ return ordered
+
+
+def reduce_cycle_set(packages):
+ """
+ Reduce the set of packages to the ones part of the circular dependency.
+
+ :param dict packages: A mapping from package name to the set of runtime
+ dependencies which is modified in place
+ """
+ last_depended = None
+ while len(packages) > 0:
+ # get all remaining dependencies
+ depended = set()
+ for pkg_name, dependencies in packages.items():
+ depended = depended.union(dependencies)
+ # remove all packages which are not dependent on
+ for name in list(packages.keys()):
+ if name not in depended:
+ del packages[name]
+ if last_depended:
+ # if remaining packages haven't changed return them
+ if last_depended == depended:
+ return packages.keys()
+ # otherwise reduce again
+ last_depended = depended
+
+
+def _include_comments():
+ # skipping comment lines when COLCON_TRACE is not set speeds up the
+ # processing especially on Windows
+ return bool(os.environ.get('COLCON_TRACE'))
+
+
+def get_commands(pkg_name, prefix, primary_extension, additional_extension):
+ commands = []
+ package_dsv_path = os.path.join(prefix, 'share', pkg_name, 'package.dsv')
+ if os.path.exists(package_dsv_path):
+ commands += process_dsv_file(
+ package_dsv_path, prefix, primary_extension, additional_extension)
+ return commands
+
+
+def process_dsv_file(
+ dsv_path, prefix, primary_extension=None, additional_extension=None
+):
+ commands = []
+ if _include_comments():
+ commands.append(FORMAT_STR_COMMENT_LINE.format_map({'comment': dsv_path}))
+ with open(dsv_path, 'r') as h:
+ content = h.read()
+ lines = content.splitlines()
+
+ basenames = OrderedDict()
+ for i, line in enumerate(lines):
+ # skip over empty or whitespace-only lines
+ if not line.strip():
+ continue
+ try:
+ type_, remainder = line.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "Line %d in '%s' doesn't contain a semicolon separating the "
+ 'type from the arguments' % (i + 1, dsv_path))
+ if type_ != DSV_TYPE_SOURCE:
+ # handle non-source lines
+ try:
+ commands += handle_dsv_types_except_source(
+ type_, remainder, prefix)
+ except RuntimeError as e:
+ raise RuntimeError(
+ "Line %d in '%s' %s" % (i + 1, dsv_path, e)) from e
+ else:
+ # group remaining source lines by basename
+ path_without_ext, ext = os.path.splitext(remainder)
+ if path_without_ext not in basenames:
+ basenames[path_without_ext] = set()
+ assert ext.startswith('.')
+ ext = ext[1:]
+ if ext in (primary_extension, additional_extension):
+ basenames[path_without_ext].add(ext)
+
+ # add the dsv extension to each basename if the file exists
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if os.path.exists(basename + '.dsv'):
+ extensions.add('dsv')
+
+ for basename, extensions in basenames.items():
+ if not os.path.isabs(basename):
+ basename = os.path.join(prefix, basename)
+ if 'dsv' in extensions:
+ # process dsv files recursively
+ commands += process_dsv_file(
+ basename + '.dsv', prefix, primary_extension=primary_extension,
+ additional_extension=additional_extension)
+ elif primary_extension in extensions and len(extensions) == 1:
+ # source primary-only files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + primary_extension})]
+ elif additional_extension in extensions:
+ # source non-primary files
+ commands += [
+ FORMAT_STR_INVOKE_SCRIPT.format_map({
+ 'prefix': prefix,
+ 'script_path': basename + '.' + additional_extension})]
+
+ return commands
+
+
+def handle_dsv_types_except_source(type_, remainder, prefix):
+ commands = []
+ if type_ in (DSV_TYPE_SET, DSV_TYPE_SET_IF_UNSET):
+ try:
+ env_name, value = remainder.split(';', 1)
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the value')
+ try_prefixed_value = os.path.join(prefix, value) if value else prefix
+ if os.path.exists(try_prefixed_value):
+ value = try_prefixed_value
+ if type_ == DSV_TYPE_SET:
+ commands += _set(env_name, value)
+ elif type_ == DSV_TYPE_SET_IF_UNSET:
+ commands += _set_if_unset(env_name, value)
+ else:
+ assert False
+ elif type_ in (
+ DSV_TYPE_APPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE,
+ DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS
+ ):
+ try:
+ env_name_and_values = remainder.split(';')
+ except ValueError:
+ raise RuntimeError(
+ "doesn't contain a semicolon separating the environment name "
+ 'from the values')
+ env_name = env_name_and_values[0]
+ values = env_name_and_values[1:]
+ for value in values:
+ if not value:
+ value = prefix
+ elif not os.path.isabs(value):
+ value = os.path.join(prefix, value)
+ if (
+ type_ == DSV_TYPE_PREPEND_NON_DUPLICATE_IF_EXISTS and
+ not os.path.exists(value)
+ ):
+ comment = f'skip extending {env_name} with not existing ' \
+ f'path: {value}'
+ if _include_comments():
+ commands.append(
+ FORMAT_STR_COMMENT_LINE.format_map({'comment': comment}))
+ elif type_ == DSV_TYPE_APPEND_NON_DUPLICATE:
+ commands += _append_unique_value(env_name, value)
+ else:
+ commands += _prepend_unique_value(env_name, value)
+ else:
+ raise RuntimeError(
+ 'contains an unknown environment hook type: ' + type_)
+ return commands
+
+
+env_state = {}
+
+
+def _append_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # append even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional leading separator
+ extend = FORMAT_STR_USE_ENV_VAR.format_map({'name': name}) + os.pathsep
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': extend + value})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+def _prepend_unique_value(name, value):
+ global env_state
+ if name not in env_state:
+ if os.environ.get(name):
+ env_state[name] = set(os.environ[name].split(os.pathsep))
+ else:
+ env_state[name] = set()
+ # prepend even if the variable has not been set yet, in case a shell script sets the
+ # same variable without the knowledge of this Python script.
+ # later _remove_ending_separators() will cleanup any unintentional trailing separator
+ extend = os.pathsep + FORMAT_STR_USE_ENV_VAR.format_map({'name': name})
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value + extend})
+ if value not in env_state[name]:
+ env_state[name].add(value)
+ else:
+ if not _include_comments():
+ return []
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+# generate commands for removing prepended underscores
+def _remove_ending_separators():
+ # do nothing if the shell extension does not implement the logic
+ if FORMAT_STR_REMOVE_TRAILING_SEPARATOR is None:
+ return []
+
+ global env_state
+ commands = []
+ for name in env_state:
+ # skip variables that already had values before this script started prepending
+ if name in os.environ:
+ continue
+ commands += [
+ FORMAT_STR_REMOVE_LEADING_SEPARATOR.format_map({'name': name}),
+ FORMAT_STR_REMOVE_TRAILING_SEPARATOR.format_map({'name': name})]
+ return commands
+
+
+def _set(name, value):
+ global env_state
+ env_state[name] = value
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ return [line]
+
+
+def _set_if_unset(name, value):
+ global env_state
+ line = FORMAT_STR_SET_ENV_VAR.format_map(
+ {'name': name, 'value': value})
+ if env_state.get(name, os.environ.get(name)):
+ line = FORMAT_STR_COMMENT_LINE.format_map({'comment': line})
+ return [line]
+
+
+if __name__ == '__main__': # pragma: no cover
+ try:
+ rc = main()
+ except RuntimeError as e:
+ print(str(e), file=sys.stderr)
+ rc = 1
+ sys.exit(rc)
diff --git a/ros2_ws/install/local_setup.bash b/ros2_ws/install/local_setup.bash
new file mode 100644
index 0000000000000000000000000000000000000000..efd5f8c9e24546b7d9b90d2e7928ea126de164e3
--- /dev/null
+++ b/ros2_ws/install/local_setup.bash
@@ -0,0 +1,107 @@
+# generated from colcon_bash/shell/template/prefix.bash.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# a bash script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)"
+else
+ _colcon_prefix_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prefix_bash_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prefix_bash_prepend_unique_value_IFS="$IFS"
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ # restore the field separator
+ IFS="$_colcon_prefix_bash_prepend_unique_value_IFS"
+ unset _colcon_prefix_bash_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# add this prefix to the COLCON_PREFIX_PATH
+_colcon_prefix_bash_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX"
+unset _colcon_prefix_bash_prepend_unique_value
+
+# check environment variable for custom Python executable
+if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
+ if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
+ echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ return 1
+ fi
+ _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
+else
+ # try the Python executable known at configure time
+ _colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if [ ! -f "$_colcon_python_executable" ]; then
+ if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
+ echo "error: unable to find python3 executable"
+ return 1
+ fi
+ _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
+ fi
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo ". \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# get all commands in topological order
+_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_bash_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh bash)"
+unset _colcon_python_executable
+if [ -n "$COLCON_TRACE" ]; then
+ echo "Execute generated script:"
+ echo "<<<"
+ echo "${_colcon_ordered_commands}"
+ echo ">>>"
+fi
+eval "${_colcon_ordered_commands}"
+unset _colcon_ordered_commands
+
+unset _colcon_prefix_sh_source_script
+
+unset _colcon_prefix_bash_COLCON_CURRENT_PREFIX
diff --git a/ros2_ws/install/local_setup.ps1 b/ros2_ws/install/local_setup.ps1
new file mode 100644
index 0000000000000000000000000000000000000000..6f68c8dede9ed4ecb63a4eb6ac2a7450bd18ec3b
--- /dev/null
+++ b/ros2_ws/install/local_setup.ps1
@@ -0,0 +1,55 @@
+# generated from colcon_powershell/shell/template/prefix.ps1.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# check environment variable for custom Python executable
+if ($env:COLCON_PYTHON_EXECUTABLE) {
+ if (!(Test-Path "$env:COLCON_PYTHON_EXECUTABLE" -PathType Leaf)) {
+ echo "error: COLCON_PYTHON_EXECUTABLE '$env:COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ exit 1
+ }
+ $_colcon_python_executable="$env:COLCON_PYTHON_EXECUTABLE"
+} else {
+ # use the Python executable known at configure time
+ $_colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if (!(Test-Path "$_colcon_python_executable" -PathType Leaf)) {
+ if (!(Get-Command "python3" -ErrorAction SilentlyContinue)) {
+ echo "error: unable to find python3 executable"
+ exit 1
+ }
+ $_colcon_python_executable="python3"
+ }
+}
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+function _colcon_prefix_powershell_source_script {
+ param (
+ $_colcon_prefix_powershell_source_script_param
+ )
+ # source script with conditional trace output
+ if (Test-Path $_colcon_prefix_powershell_source_script_param) {
+ if ($env:COLCON_TRACE) {
+ echo ". '$_colcon_prefix_powershell_source_script_param'"
+ }
+ . "$_colcon_prefix_powershell_source_script_param"
+ } else {
+ Write-Error "not found: '$_colcon_prefix_powershell_source_script_param'"
+ }
+}
+
+# get all commands in topological order
+$_colcon_ordered_commands = & "$_colcon_python_executable" "$(Split-Path $PSCommandPath -Parent)/_local_setup_util_ps1.py" ps1
+
+# execute all commands in topological order
+if ($env:COLCON_TRACE) {
+ echo "Execute generated script:"
+ echo "<<<"
+ $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Write-Output
+ echo ">>>"
+}
+if ($_colcon_ordered_commands) {
+ $_colcon_ordered_commands.Split([Environment]::NewLine, [StringSplitOptions]::RemoveEmptyEntries) | Invoke-Expression
+}
diff --git a/ros2_ws/install/local_setup.sh b/ros2_ws/install/local_setup.sh
new file mode 100644
index 0000000000000000000000000000000000000000..ef8aee8333db86abe0e07e0b1e44ac839636a33f
--- /dev/null
+++ b/ros2_ws/install/local_setup.sh
@@ -0,0 +1,137 @@
+# generated from colcon_core/shell/template/prefix.sh.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_prefix_sh_COLCON_CURRENT_PREFIX="/home/ubuntu/ros2_ws/install"
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ if [ ! -d "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX" ]; then
+ echo "The build time path \"$_colcon_prefix_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+ unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX
+ return 1
+ fi
+else
+ _colcon_prefix_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prefix_sh_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prefix_sh_prepend_unique_value_IFS="$IFS"
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ _contained_value=""
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ _contained_value=1
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ if [ -z "$_contained_value" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ if [ "$_all_values" = "$_value" ]; then
+ echo "export $_listname=$_value"
+ else
+ echo "export $_listname=$_value:\$$_listname"
+ fi
+ fi
+ fi
+ unset _contained_value
+ # restore the field separator
+ IFS="$_colcon_prefix_sh_prepend_unique_value_IFS"
+ unset _colcon_prefix_sh_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# add this prefix to the COLCON_PREFIX_PATH
+_colcon_prefix_sh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX"
+unset _colcon_prefix_sh_prepend_unique_value
+
+# check environment variable for custom Python executable
+if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
+ if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
+ echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ return 1
+ fi
+ _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
+else
+ # try the Python executable known at configure time
+ _colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if [ ! -f "$_colcon_python_executable" ]; then
+ if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
+ echo "error: unable to find python3 executable"
+ return 1
+ fi
+ _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
+ fi
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# get all commands in topological order
+_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_sh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh)"
+unset _colcon_python_executable
+if [ -n "$COLCON_TRACE" ]; then
+ echo "_colcon_prefix_sh_source_script() {
+ if [ -f \"\$1\" ]; then
+ if [ -n \"\$COLCON_TRACE\" ]; then
+ echo \"# . \\\"\$1\\\"\"
+ fi
+ . \"\$1\"
+ else
+ echo \"not found: \\\"\$1\\\"\" 1>&2
+ fi
+ }"
+ echo "# Execute generated script:"
+ echo "# <<<"
+ echo "${_colcon_ordered_commands}"
+ echo "# >>>"
+ echo "unset _colcon_prefix_sh_source_script"
+fi
+eval "${_colcon_ordered_commands}"
+unset _colcon_ordered_commands
+
+unset _colcon_prefix_sh_source_script
+
+unset _colcon_prefix_sh_COLCON_CURRENT_PREFIX
diff --git a/ros2_ws/install/local_setup.zsh b/ros2_ws/install/local_setup.zsh
new file mode 100644
index 0000000000000000000000000000000000000000..f7a8d904f2019736b6114cec0f6250da480c415c
--- /dev/null
+++ b/ros2_ws/install/local_setup.zsh
@@ -0,0 +1,120 @@
+# generated from colcon_zsh/shell/template/prefix.zsh.em
+
+# This script extends the environment with all packages contained in this
+# prefix path.
+
+# a zsh script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)"
+else
+ _colcon_prefix_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to convert array-like strings into arrays
+# to workaround SH_WORD_SPLIT not being set
+_colcon_prefix_zsh_convert_to_array() {
+ local _listname=$1
+ local _dollar="$"
+ local _split="{="
+ local _to_array="(\"$_dollar$_split$_listname}\")"
+ eval $_listname=$_to_array
+}
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prefix_zsh_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prefix_zsh_prepend_unique_value_IFS="$IFS"
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ # workaround SH_WORD_SPLIT not being set
+ _colcon_prefix_zsh_convert_to_array _values
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ # restore the field separator
+ IFS="$_colcon_prefix_zsh_prepend_unique_value_IFS"
+ unset _colcon_prefix_zsh_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# add this prefix to the COLCON_PREFIX_PATH
+_colcon_prefix_zsh_prepend_unique_value COLCON_PREFIX_PATH "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX"
+unset _colcon_prefix_zsh_prepend_unique_value
+unset _colcon_prefix_zsh_convert_to_array
+
+# check environment variable for custom Python executable
+if [ -n "$COLCON_PYTHON_EXECUTABLE" ]; then
+ if [ ! -f "$COLCON_PYTHON_EXECUTABLE" ]; then
+ echo "error: COLCON_PYTHON_EXECUTABLE '$COLCON_PYTHON_EXECUTABLE' doesn't exist"
+ return 1
+ fi
+ _colcon_python_executable="$COLCON_PYTHON_EXECUTABLE"
+else
+ # try the Python executable known at configure time
+ _colcon_python_executable="/usr/bin/python3"
+ # if it doesn't exist try a fall back
+ if [ ! -f "$_colcon_python_executable" ]; then
+ if ! /usr/bin/env python3 --version > /dev/null 2> /dev/null; then
+ echo "error: unable to find python3 executable"
+ return 1
+ fi
+ _colcon_python_executable=`/usr/bin/env python3 -c "import sys; print(sys.executable)"`
+ fi
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo ". \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# get all commands in topological order
+_colcon_ordered_commands="$($_colcon_python_executable "$_colcon_prefix_zsh_COLCON_CURRENT_PREFIX/_local_setup_util_sh.py" sh zsh)"
+unset _colcon_python_executable
+if [ -n "$COLCON_TRACE" ]; then
+ echo "Execute generated script:"
+ echo "<<<"
+ echo "${_colcon_ordered_commands}"
+ echo ">>>"
+fi
+eval "${_colcon_ordered_commands}"
+unset _colcon_ordered_commands
+
+unset _colcon_prefix_sh_source_script
+
+unset _colcon_prefix_zsh_COLCON_CURRENT_PREFIX
diff --git a/ros2_ws/install/setup.bash b/ros2_ws/install/setup.bash
new file mode 100644
index 0000000000000000000000000000000000000000..4c55244159be959e730ef507f8e45f57c6ee7bb0
--- /dev/null
+++ b/ros2_ws/install/setup.bash
@@ -0,0 +1,31 @@
+# generated from colcon_bash/shell/template/prefix_chain.bash.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_chain_bash_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo ". \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source chained prefixes
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="/opt/ros/humble"
+_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
+
+# source this prefix
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null && pwd)"
+_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
+
+unset COLCON_CURRENT_PREFIX
+unset _colcon_prefix_chain_bash_source_script
diff --git a/ros2_ws/install/setup.ps1 b/ros2_ws/install/setup.ps1
new file mode 100644
index 0000000000000000000000000000000000000000..558e9b9e627400bd65c3fc2e0751bb19ec5efa69
--- /dev/null
+++ b/ros2_ws/install/setup.ps1
@@ -0,0 +1,29 @@
+# generated from colcon_powershell/shell/template/prefix_chain.ps1.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+function _colcon_prefix_chain_powershell_source_script {
+ param (
+ $_colcon_prefix_chain_powershell_source_script_param
+ )
+ # source script with conditional trace output
+ if (Test-Path $_colcon_prefix_chain_powershell_source_script_param) {
+ if ($env:COLCON_TRACE) {
+ echo ". '$_colcon_prefix_chain_powershell_source_script_param'"
+ }
+ . "$_colcon_prefix_chain_powershell_source_script_param"
+ } else {
+ Write-Error "not found: '$_colcon_prefix_chain_powershell_source_script_param'"
+ }
+}
+
+# source chained prefixes
+_colcon_prefix_chain_powershell_source_script "/opt/ros/humble\local_setup.ps1"
+
+# source this prefix
+$env:COLCON_CURRENT_PREFIX=(Split-Path $PSCommandPath -Parent)
+_colcon_prefix_chain_powershell_source_script "$env:COLCON_CURRENT_PREFIX\local_setup.ps1"
diff --git a/ros2_ws/install/setup.sh b/ros2_ws/install/setup.sh
new file mode 100644
index 0000000000000000000000000000000000000000..5b2b7fe9c8f7b012cb21a74f58a8b069e712053c
--- /dev/null
+++ b/ros2_ws/install/setup.sh
@@ -0,0 +1,45 @@
+# generated from colcon_core/shell/template/prefix_chain.sh.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX=/home/ubuntu/ros2_ws/install
+if [ ! -z "$COLCON_CURRENT_PREFIX" ]; then
+ _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+elif [ ! -d "$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX" ]; then
+ echo "The build time path \"$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+ unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX
+ return 1
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_chain_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source chained prefixes
+# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
+COLCON_CURRENT_PREFIX="/opt/ros/humble"
+_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
+
+
+# source this prefix
+# setting COLCON_CURRENT_PREFIX avoids relying on the build time prefix of the sourced script
+COLCON_CURRENT_PREFIX="$_colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX"
+_colcon_prefix_chain_sh_source_script "$COLCON_CURRENT_PREFIX/local_setup.sh"
+
+unset _colcon_prefix_chain_sh_COLCON_CURRENT_PREFIX
+unset _colcon_prefix_chain_sh_source_script
+unset COLCON_CURRENT_PREFIX
diff --git a/ros2_ws/install/setup.zsh b/ros2_ws/install/setup.zsh
new file mode 100644
index 0000000000000000000000000000000000000000..990d17190a973865cdb60bcea8875b768db28150
--- /dev/null
+++ b/ros2_ws/install/setup.zsh
@@ -0,0 +1,31 @@
+# generated from colcon_zsh/shell/template/prefix_chain.zsh.em
+
+# This script extends the environment with the environment of other prefix
+# paths which were sourced when this file was generated as well as all packages
+# contained in this prefix path.
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+_colcon_prefix_chain_zsh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo ". \"$1\""
+ fi
+ . "$1"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source chained prefixes
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="/opt/ros/humble"
+_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
+
+# source this prefix
+# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
+COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)"
+_colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh"
+
+unset COLCON_CURRENT_PREFIX
+unset _colcon_prefix_chain_zsh_source_script
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/PKG-INFO b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/PKG-INFO
new file mode 100644
index 0000000000000000000000000000000000000000..1772b2b4b87ed715b1c9529109e2a64ffa956ef2
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/PKG-INFO
@@ -0,0 +1,12 @@
+Metadata-Version: 2.1
+Name: sphero-mini-controller
+Version: 0.0.0
+Summary: TODO: Package description
+Home-page: UNKNOWN
+Maintainer: ubuntu
+Maintainer-email: ubuntu@todo.todo
+License: TODO: License declaration
+Platform: UNKNOWN
+
+UNKNOWN
+
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/SOURCES.txt b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/SOURCES.txt
new file mode 100644
index 0000000000000000000000000000000000000000..450bb514f818ddd322e445deee994a8cbb2136e8
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/SOURCES.txt
@@ -0,0 +1,20 @@
+package.xml
+setup.cfg
+setup.py
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/PKG-INFO
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/dependency_links.txt
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/entry_points.txt
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/requires.txt
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/top_level.txt
+../../build/sphero_mini_controller/sphero_mini_controller.egg-info/zip-safe
+resource/sphero_mini_controller
+sphero_mini_controller/WavefrontPlanner.py
+sphero_mini_controller/__init__.py
+sphero_mini_controller/_constants.py
+sphero_mini_controller/blobErkennung.py
+sphero_mini_controller/my_first_node.py
+sphero_mini_controller/treiber.py
+test/test_copyright.py
+test/test_flake8.py
+test/test_pep257.py
\ No newline at end of file
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/dependency_links.txt b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/dependency_links.txt
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/dependency_links.txt
@@ -0,0 +1 @@
+
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/entry_points.txt b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/entry_points.txt
new file mode 100644
index 0000000000000000000000000000000000000000..6a40ae61982b9df335a55fd38c98ba80a816cb93
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/entry_points.txt
@@ -0,0 +1,3 @@
+[console_scripts]
+sphero_mini = sphero_mini_controller.my_first_node:main
+
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/requires.txt b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/requires.txt
new file mode 100644
index 0000000000000000000000000000000000000000..49fe098d9e6bccd89482b34510da60ab28556880
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/requires.txt
@@ -0,0 +1 @@
+setuptools
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/top_level.txt b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/top_level.txt
new file mode 100644
index 0000000000000000000000000000000000000000..60845dab3412fa47e2fb49f683a3db10ce25c01e
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/top_level.txt
@@ -0,0 +1 @@
+sphero_mini_controller
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/zip-safe b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/zip-safe
new file mode 100644
index 0000000000000000000000000000000000000000..8b137891791fe96927ad78e64b0aad7bded08bdc
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info/zip-safe
@@ -0,0 +1 @@
+
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/GameWavefront.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/GameWavefront.py
new file mode 100644
index 0000000000000000000000000000000000000000..aae934fa4259ad8bf8e9b6554fc34074aadac6ed
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/GameWavefront.py
@@ -0,0 +1,375 @@
+# Wavefront Planning
+# Sajad Saeedi, Andrew Davison 2017
+# Implementation is based on the following reference
+#
+# H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki and S. Thrun,
+# Principles of Robot Motion: Theory, Algorithms, and Implementations,
+# MIT Press, Boston, 2005.
+# http://www.cs.cmu.edu/afs/cs/Web/People/motionplanning/
+#
+# From Chapter 4.5 - Wave-Front Planner
+# This planner determines a path via gradient descent on the grid starting from the start.
+# Essentially, the planner determines the path one pixel at a time.
+# The wave-front planner essentially forms a potential function on the grid which has one local minimum and thus is resolution complete.
+# The planner also determines the shortest path, but at the cost of coming dangerously close to obstacles.
+# The major drawback of this method is that the planner has to search the entire space for a path
+
+import pygame, os, math, time, random
+import numpy as np
+try:
+ import pygame
+ from pygame import surfarray
+ from pygame.locals import *
+except ImportError:
+ raise ImportError('Error Importing Pygame/surfarray')
+
+pygame.init()
+
+SCALE = 1
+
+# set the width and height of the screen
+WIDTH = 1500/SCALE
+HEIGHT = 1000/SCALE
+
+# The region we will fill with obstacles
+PLAYFIELDCORNERS = (-3.0, -3.0, 3.0, 3.0)
+
+# Barrier locations
+barriers = []
+# barrier contents are (bx, by, visibilitymask)
+for i in range(100):
+ (bx, by) = (random.uniform(PLAYFIELDCORNERS[0], PLAYFIELDCORNERS[2]), random.uniform(PLAYFIELDCORNERS[1], PLAYFIELDCORNERS[3]))
+ barrier = [bx, by, 0]
+ barriers.append(barrier)
+
+BARRIERRADIUS = 0.1
+
+
+ROBOTRADIUS = 0.15
+W = 2 * ROBOTRADIUS
+SAFEDIST = 0.2
+BARRIERINFILATE = ROBOTRADIUS
+
+MAXVELOCITY = 0.5 #ms^(-1) max speed of each wheel
+MAXACCELERATION = 0.5 #ms^(-2) max rate we can change speed of each wheel
+
+target = (PLAYFIELDCORNERS[2] + 1.0, 0)
+
+k = 160/SCALE # pixels per metre for graphics
+u0 = WIDTH / 2
+v0 = HEIGHT / 2
+
+x = PLAYFIELDCORNERS[0] - 0.5
+y = 0.0
+theta = 0.0
+
+vL = 0.00
+vR = 0.00
+
+size = [int(WIDTH), int(HEIGHT)]
+screen = pygame.display.set_mode(size)
+black = (20,20,40)
+
+# This makes the normal mouse pointer invisible in graphics window
+pygame.mouse.set_visible(0)
+
+# time delta
+dt = 0.1
+
+
+def surfdemo_show(array_img, name):
+ "displays a surface, waits for user to continue"
+ screen = pygame.display.set_mode(array_img.shape[:2], 0, 32)
+ surfarray.blit_array(screen, array_img)
+ pygame.display.flip()
+ pygame.display.set_caption(name)
+ while 1:
+ e = pygame.event.wait()
+ if e.type == MOUSEBUTTONDOWN: break
+ if e.type == KEYDOWN: break
+
+
+def predictPosition(vL, vR, x, y, theta, dt):
+
+ # Position update in time dt
+
+ # Special cases
+ # Straight line motion
+ if (vL == vR):
+ xnew = x + vL * dt * math.cos(theta)
+ ynew = y + vL * dt * math.sin(theta)
+ thetanew = theta
+ # Pure rotation motion
+ elif (vL == -vR):
+ xnew = x
+ ynew = y
+ thetanew = theta + ((vR - vL) * dt / W)
+ else:
+ # Rotation and arc angle of general circular motion
+ R = W / 2.0 * (vR + vL) / (vR - vL)
+ deltatheta = (vR - vL) * dt / W
+ xnew = x + R * (math.sin(deltatheta + theta) - math.sin(theta))
+ ynew = y - R * (math.cos(deltatheta + theta) - math.cos(theta))
+ thetanew = theta + deltatheta
+
+ return (xnew, ynew, thetanew)
+
+
+def drawBarriers(barriers):
+ for barrier in barriers:
+ if(barrier[2] == 0):
+ pygame.draw.circle(screen, (0,20,80), (int(u0 + k * barrier[0]), int(v0 - k * barrier[1])), int(k * BARRIERRADIUS), 0)
+ else:
+ pygame.draw.circle(screen, (0,120,255), (int(u0 + k * barrier[0]), int(v0 - k * barrier[1])), int(k * BARRIERRADIUS), 0)
+ return
+
+def dialtebarrieres(imgin, barriers):
+ imgout = imgin
+ for barrier in barriers:
+ if(barrier[2] == 0):
+ center_u = int(u0 + k * barrier[0])
+ center_v = int(v0 - k * barrier[1])
+ RAD = BARRIERRADIUS+BARRIERINFILATE
+ radius = int(k * (RAD))
+ radius2 = int(k * (BARRIERRADIUS))
+ points = [(x,y) for x in range(center_u-radius, center_u+radius) for y in range(center_v-radius, center_v+radius)]
+ for pt in points:
+ vu = center_u - pt[0]
+ vv = center_v - pt[1]
+ distance = math.sqrt(vv*vv + vu*vu)
+ if (distance < radius and distance > radius2 and pt[0] < WIDTH-1 and pt[1] < HEIGHT-1):
+ imgout[pt[0],pt[1],0] = 0
+ imgout[pt[0],pt[1],1] = 40
+ imgout[pt[0],pt[1],2] = 80
+ return imgout
+
+def MakeWaveFront(imgarray, start_uv, target_uv):
+ print ("building wavefront, please wait ... ")
+ heap = []
+ newheap = []
+
+ u = target_uv[0]
+ v = target_uv[1]
+
+ imgarray[:,:,0] = 0 # use channel 0 for planning
+ imgarray[u, v, 0] = 2
+
+ lastwave = 3 # start by setting nodes around target to 3
+ moves = [(u + 1, v), (u - 1, v), (u, v - 1), (u, v + 1)]
+ for move in moves:
+ imgarray[move[0], move[1], 0] = 3
+ heap.append(move)
+
+ path = False
+ max_search = int(np.sqrt(WIDTH*WIDTH + HEIGHT*HEIGHT))
+ for currentwave in range(4, max_search):
+ lastwave = lastwave + 1
+ while(heap != []):
+ position = heap.pop()
+ (u, v) = position
+ moves = [(u + 1, v), (u - 1, v), (u, v + 1), (u, v - 1)]
+ for move in moves:
+ if(imgarray[move[0], move[1], 2] != 80):
+ if(imgarray[move[0], move[1], 0] == 0 and imgarray[position[0], position[1], 0] == currentwave - 1 and move[0] < WIDTH-1 and move[1] < HEIGHT-1 and move[0]>2 and move[1]>2):
+ imgarray[move[0], move[1], 0] = currentwave
+ newheap.append(move)
+ if(move == start_uv):
+ path = True
+ break
+ if(path == True):
+ break
+ if(path == True):
+ break
+ heap = newheap
+ newheap = []
+ return imgarray, currentwave
+
+
+def FindPath(imgarray, start_uv, currentwave):
+ trajectory = []
+ nextpt = start_uv
+ path = []
+ for backwave in range(currentwave-1,2,-1):
+ path.append(nextpt)
+ (u,v) = nextpt
+ values = []
+ val = []
+ moves = [(u + 1, v), (u - 1, v), (u, v + 1), (u, v - 1), (u + 1, v + 1), (u - 1, v - 1), (u + 1, v - 1), (u - 1, v - 1)]
+ for move in moves:
+ val.append(imgarray[move[0], move[1], 0])
+ if(imgarray[move[0], move[1], 0] == backwave):
+ values.append(imgarray[move[0], move[1], 0])
+ minimum = min(values)
+ indices = [i for i, v in enumerate(val) if v == minimum]
+ nextid = random.choice(indices)
+ nextpt = moves[nextid]
+ return path
+
+def GetControl(x,y,theta, waypoint):
+ wpu = waypoint[0]
+ wpv = waypoint[1]
+
+ vt = 0.2
+ u = u0 + k * x
+ v = v0 - k * y
+ vector = (wpu - u, -wpv + v)
+ vectorangle = math.atan2(vector[1], vector[0])
+ psi = vectorangle - theta
+
+ if(abs(psi) > (2*3.14/180)):
+ if(psi > 0):
+ vR = vt/2
+ vL = -vt/2
+ else:
+ vR = -vt/2
+ vL = vt/2
+ else:
+ wlx = x - (W/2.0) * math.sin(theta)
+ wly = y + (W/2.0) * math.cos(theta)
+ ulx = u0 + k * wlx
+ vlx = v0 - k * wly
+ dl = math.sqrt((ulx-wpu)*(ulx-wpu) + (vlx-wpv)*(vlx-wpv))
+
+ wrx = x + (W/2.0) * math.sin(theta)
+ wry = y - (W/2.0) * math.cos(theta)
+ urx = u0 + k * wrx
+ vrx = v0 - k * wry
+ dr = math.sqrt((urx-wpu)*(urx-wpu) + (vrx-wpv)*(vrx-wpv))
+
+ vR = 1*(2*vt)/(1+(dl/dr))
+ vL = 1*(2*vt - vR)
+
+ return vL, vR
+
+def AnimateRobot(x,y,theta):
+ # Draw robot
+ u = u0 + k * x
+ v = v0 - k * y
+ pygame.draw.circle(screen, (255,255,255), (int(u), int(v)), int(k * ROBOTRADIUS), 3)
+ # Draw wheels
+ # left wheel centre
+ wlx = x - (W/2.0) * math.sin(theta)
+ wly = y + (W/2.0) * math.cos(theta)
+ ulx = u0 + k * wlx
+ vlx = v0 - k * wly
+ WHEELBLOB = 0.04
+ pygame.draw.circle(screen, (0,0,255), (int(ulx), int(vlx)), int(k * WHEELBLOB))
+ # right wheel centre
+ wrx = x + (W/2.0) * math.sin(theta)
+ wry = y - (W/2.0) * math.cos(theta)
+ urx = u0 + k * wrx
+ vrx = v0 - k * wry
+ pygame.draw.circle(screen, (0,0,255), (int(urx), int(vrx)), int(k * WHEELBLOB))
+
+ time.sleep(dt / 5)
+ pygame.display.flip()
+ #time.sleep(0.2)
+
+def AnimateNavigation(barriers, waypint, path):
+ screen.fill(black)
+ drawBarriers(barriers)
+
+ for pt in path:
+ screen.set_at(pt, (255,255,255))
+
+ pygame.draw.circle(screen, (255,100,0), target_uv, int(k * ROBOTRADIUS), 0)
+ pygame.draw.circle(screen, (255,100,0), (int(u0 + k * target[0]), int(v0 - k * target[1])), int(k * ROBOTRADIUS), 0)
+ pygame.draw.circle(screen, (255,0,255), (int(waypint[0]), int(waypint[1])), int(5))
+
+def AnimatePath(imgarray, path, start_uv):
+ for pt in path:
+ imgarray[pt[0], pt[1], 0] = 0
+ imgarray[pt[0], pt[1], 1] = 255
+ imgarray[pt[0], pt[1], 2] = 255
+
+ #imgarray[:,:,0] /= imgarray[:,:,0].max()/255.0
+ scalefactor = imgarray[:,:,0].max()/255.0
+ imgarray[:,:,0] = imgarray[:,:,0]/scalefactor
+ imgarray[:,:,0].astype(int)
+ imgarray[start_uv[0], start_uv[1], 0] = 0
+ imgarray[start_uv[0], start_uv[1], 1] = 255
+ imgarray[start_uv[0], start_uv[1], 2] = 255
+
+ surfdemo_show(imgarray, 'Wavefront Path Planning')
+
+
+def GetWaypoint(x,y,theta, path, waypointIndex, waypointSeperation, target):
+ reset = False
+ waypoint = path[waypointIndex]
+ u = u0 + k * x
+ v = v0 - k * y
+ distance_to_wp = math.sqrt((waypoint[0]-u)**2+(waypoint[1]-v)**2) # todo compare distance in metrics
+ if(distance_to_wp < 3):
+ waypointIndex = waypointSeperation + waypointIndex
+ if(waypointIndex > len(path)):
+ waypointIndex = len(path) - 1
+
+ return waypoint, reset, waypointIndex
+
+def IsAtTarget(x,y,target):
+ disttotarget = math.sqrt((x - target[0])**2 + (y - target[1])**2)
+ if (disttotarget < 0.04):
+ return True
+ else:
+ return False
+
+
+while(1):
+ start_uv = (int(u0 + k * x), int(v0 - k * y))
+ target_uv = (int(u0 + k * target[0]), int(v0 - k * target[1]))
+ print ("start is: ", start_uv)
+ print ("goal is: ", target_uv)
+
+ # prepare map of the world for plannign
+ screen.fill(black)
+ drawBarriers(barriers)
+ pygame.draw.circle(screen, (255,100,0), target_uv, int(k * ROBOTRADIUS), 0)
+ pygame.draw.circle(screen, (255,255,0), start_uv, int(k * ROBOTRADIUS), 0)
+ imgscreen8 = pygame.surfarray.array3d(screen)
+ imgscreen16 = np.array(imgscreen8, dtype=np.uint16)
+ drawBarriers(barriers)
+ imgarray = dialtebarrieres(imgscreen16, barriers)
+ pygame.display.flip()
+ pygame.display.set_caption('Wavefront Path Planning')
+
+ # build wavefront, given the map, start point, and target point
+ imgarray, currentwave = MakeWaveFront(imgarray, start_uv, target_uv)
+ print ("press a key to start navaigation ... ")
+
+ # find the path using the wavefront
+ path = FindPath(imgarray, start_uv, currentwave)
+
+ # normlaize and show the path on the map
+ AnimatePath(imgarray, path, start_uv)
+
+ # set start point
+ x = PLAYFIELDCORNERS[0] - 0.5
+ y = 0
+ theta = 0
+ waypointSeperation = 40;
+ waypointIndex = waypointSeperation;
+
+ # loop to navigate the path from start to target
+ while(1):
+ # get a waypoint to follow the path
+ (waypoint, reset, waypointIndex)= GetWaypoint(x,y,theta, path, waypointIndex, waypointSeperation, target)
+
+ # reset the simulation, if robot is at target
+ if (IsAtTarget(x,y,target)):
+ target = (target[0], random.uniform(PLAYFIELDCORNERS[1], PLAYFIELDCORNERS[3]))
+ x = PLAYFIELDCORNERS[0] - 0.5
+ y = 0.0
+ theta = 0.0
+ break
+
+
+ # calculate control signals to get to the next waypoint
+ (vL, vR) = GetControl(x,y,theta, waypoint)
+
+ # Actually now move and update position based on chosen vL and vR
+ (x, y, theta) = predictPosition(vL, vR, x, y, theta, dt)
+
+ # animate
+ AnimateNavigation(barriers, waypoint, path)
+ AnimateRobot(x,y,theta)
+
\ No newline at end of file
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/WavefrontChatGpt.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/WavefrontChatGpt.py
new file mode 100644
index 0000000000000000000000000000000000000000..a358dfa0282ce93f94817eca2941d5fa2e991539
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/WavefrontChatGpt.py
@@ -0,0 +1,53 @@
+import numpy as np
+import cv2
+import matplotlib.pyplot as plt
+import time
+
+class WaveFrontPlanner:
+ __wall = 999
+ __goal = 1
+ __nothing = '000'
+ __path = 'PATH'
+
+ def __init__(self, mask_list):
+ if not isinstance(mask_list, np.ndarray):
+ raise ValueError("Invalid input type. 'mask_list' must be a numpy array.")
+ self.__mapOfWorld = np.array([['999' if j > 200 else '000' for j in row] for row in mask_list])
+
+ def minSurroundingNodeValue(self, x, y):
+ values = [self.__mapOfWorld[x + 1][y], self.__mapOfWorld[x - 1][y],
+ self.__mapOfWorld[x][y + 1], self.__mapOfWorld[x][y - 1]]
+ min_value = min(value for value in values if value != self.__nothing)
+ min_index = values.index(min_value) + 1
+ return min_value, min_index
+
+ def waveFrontAlgorithm(self):
+ goal_coordinates = np.where(self.__mapOfWorld == self.__goal)
+ goal_x, goal_y = goal_coordinates[0][0], goal_coordinates[1][0]
+ queue = [(goal_x, goal_y)]
+
+ while queue:
+ x, y = queue.pop(0)
+
+ if self.__mapOfWorld[x][y] == self.__nothing:
+ min_value, min_index = self.minSurroundingNodeValue(x, y)
+ self.__mapOfWorld[x][y] = min_value + 1
+ queue.append((x + 1, y)) # Move down
+ queue.append((x - 1, y)) # Move up
+ queue.append((x, y + 1)) # Move right
+ queue.append((x, y - 1)) # Move left
+
+ self.__mapOfWorld[goal_x][goal_y] = self.__goal
+
+ def visualizeMap(self):
+ plt.imshow(cv2.flip(self.__mapOfWorld, 0), cmap='hot', interpolation='nearest')
+ plt.colorbar()
+ plt.show()
+
+# Beispielverwendung:
+mask_list = np.zeros((200, 200))
+mask_list[50:150, 50:150] = 1
+
+planner = WaveFrontPlanner(mask_list)
+planner.waveFrontAlgorithm()
+planner.visualizeMap()
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/WavefrontPlanner.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/WavefrontPlanner.py
new file mode 100644
index 0000000000000000000000000000000000000000..13cebc92bac4ee4b8096a05c1e3007b8f8741a69
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/WavefrontPlanner.py
@@ -0,0 +1,397 @@
+############################################################################
+# WAVEFRONT ALGORITHM
+# Adapted to Python Code By Darin Velarde
+# Fri Jan 29 13:56:53 PST 2010
+# from C code from John Palmisano
+# (www.societyofrobots.com)
+############################################################################
+import cv2
+import numpy as np
+import matplotlib.pyplot as plt
+import time
+
+class waveFrontPlanner:
+ ############################################################################
+ # WAVEFRONT ALGORITHM
+ # Adapted to Python Code By Darin Velarde
+ # Fri Jan 29 13:56:53 PST 2010
+ # from C code from John Palmisano
+ # (www.societyofrobots.com)
+ ############################################################################
+
+ def __init__(self, mapOfWorld, slow=False):
+ self.__slow = slow
+ self.__mapOfWorld = mapOfWorld
+ if str(type(mapOfWorld)).find("numpy") != -1:
+ #If its a numpy array
+ self.__height, self.__width = self.__mapOfWorld.shape
+ else:
+ self.__height, self.__width = len(self.__mapOfWorld), len(self.__mapOfWorld[0])
+
+ self.__nothing = 000
+ self.__wall = 999
+ self.__goal = 1
+ self.__path = "PATH"
+
+ self.__finalPath = []
+
+ #Robot value
+ self.__robot = 254
+ #Robot default Location
+ self.__robot_x = 0
+ self.__robot_y = 0
+
+ #default goal location
+ self.__goal_x = 8
+ self.__goal_y = 9
+
+ #temp variables
+ self.__temp_A = 0
+ self.__temp_B = 0
+ self.__counter = 0
+ self.__steps = 0 #determine how processor intensive the algorithm was
+
+ #when searching for a node with a lower value
+ self.__minimum_node = 250
+ self.__min_node_location = 250
+ self.__new_state = 1
+ self.__old_state = 1
+ self.__reset_min = 250 #above this number is a special (wall or robot)
+ ###########################################################################
+
+ def setRobotPosition(self, x, y):
+ """
+ Sets the robot's current position
+
+ """
+
+ self.__robot_x = x
+ self.__robot_y = y
+ ###########################################################################
+
+ def setGoalPosition(self, x, y):
+ """
+ Sets the goal position.
+
+ """
+
+ self.__goal_x = x
+ self.__goal_y = y
+ ###########################################################################
+
+ def robotPosition(self):
+ return (self.__robot_x, self.__robot_y)
+ ###########################################################################
+
+ def goalPosition(self):
+ return (self.__goal_x, self.__goal_y)
+ ###########################################################################
+
+ def run(self, prnt=False):
+ """
+ The entry point for the robot algorithm to use wavefront propagation.
+
+ """
+
+ path = []
+ while self.__mapOfWorld[self.__robot_x][self.__robot_y] != self.__goal:
+ if self.__steps > 10000:
+ print ("Cannot find a path.")
+ return
+ #find new location to go to
+ self.__new_state = self.propagateWavefront()
+ #update location of robot
+ if self.__new_state == 1:
+ self.__robot_x -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" % (self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 2:
+ self.__robot_y += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 3:
+ self.__robot_x += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 4:
+ self.__robot_y -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ self.__old_state = self.__new_state
+ msg = "Found the goal in %i steps:\n" % self.__steps
+ msg += "mapOfWorld size= %i %i\n\n" % (self.__height, self.__width)
+ if prnt:
+ print(msg)
+ self.printMap()
+ return path
+ ###########################################################################
+
+ def propagateWavefront(self, prnt=False):
+ self.unpropagate()
+ #Old robot location was deleted, store new robot location in mapOfWorld
+ self.__mapOfWorld[self.__robot_x][self.__robot_y] = self.__robot
+ self.__path = self.__robot
+ #start location to begin scan at goal location
+ self.__mapOfWorld[self.__goal_x][self.__goal_y] = self.__goal
+ counter = 0
+ while counter < 200: #allows for recycling until robot is found
+ x = 0
+ y = 0
+ #time.sleep(0.00001)
+ #while the mapOfWorld hasnt been fully scanned
+ while x < self.__height and y < self.__width:
+ #if this location is a wall or the goal, just ignore it
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal:
+ #a full trail to the robot has been located, finished!
+ minLoc = self.minSurroundingNodeValue(x, y)
+ if minLoc < self.__reset_min and \
+ self.__mapOfWorld[x][y] == self.__robot:
+ if prnt:
+ print("Finished Wavefront:\n")
+ self.printMap()
+ # Tell the robot to move after this return.
+ return self.__min_node_location
+ #record a value in to this node
+ elif self.__minimum_node != self.__reset_min:
+ #if this isnt here, 'nothing' will go in the location
+ self.__mapOfWorld[x][y] = self.__minimum_node + 1
+ #go to next node and/or row
+ y += 1
+ if y == self.__width and x != self.__height:
+ x += 1
+ y = 0
+ #print self.__robot_x, self.__robot_y
+ if prnt:
+ print("Sweep #: %i\n" % (counter + 1))
+ self.printMap()
+ self.__steps += 1
+ counter += 1
+ return 0
+ ###########################################################################
+
+ def unpropagate(self):
+ """
+ clears old path to determine new path
+ stay within boundary
+
+ """
+
+ for x in range(0, self.__height):
+ for y in range(0, self.__width):
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal and \
+ self.__mapOfWorld[x][y] != self.__path:
+ #if this location is a wall or goal, just ignore it
+ self.__mapOfWorld[x][y] = self.__nothing #clear that space
+ ###########################################################################
+
+ def minSurroundingNodeValue(self, x, y):
+ """
+ this method looks at a node and returns the lowest value around that
+ node.
+
+ """
+
+ #reset minimum
+ self.__minimum_node = self.__reset_min
+ #down
+ if x < self.__height -1:
+ if self.__mapOfWorld[x + 1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x + 1][y] != self.__nothing:
+ #find the lowest number node, and exclude empty nodes (0's)
+ self.__minimum_node = self.__mapOfWorld[x + 1][y]
+ self.__min_node_location = 3
+ #up
+ if x > 0:
+ if self.__mapOfWorld[x-1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x-1][y] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x-1][y]
+ self.__min_node_location = 1
+ #right
+ if y < self.__width -1:
+ if self.__mapOfWorld[x][y + 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y + 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y + 1]
+ self.__min_node_location = 2
+ #left
+ if y > 0:
+ if self.__mapOfWorld[x][y - 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y - 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y-1]
+ self.__min_node_location = 4
+ return self.__minimum_node
+ ###########################################################################
+
+ def printMap(self):
+ """
+ Prints out the map of this instance of the class.
+
+ """
+
+ msg = ''
+ for temp_B in range(0, self.__height):
+ for temp_A in range(0, self.__width):
+ if self.__mapOfWorld[temp_B][temp_A] == self.__wall:
+ msg += "%04s" % "[#]"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__robot:
+ msg += "%04s" % "-"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__goal:
+ msg += "%04s" % "G"
+ else:
+ msg += "%04s" % str(self.__mapOfWorld[temp_B][temp_A])
+ msg += "\n\n"
+ msg += "\n\n"
+ print(msg)
+ #
+ if self.__slow == True:
+ time.sleep(0.05)
+############################################################################
+
+class mapCreate:
+ ############################################################################
+
+ def create_map(self, scale,img):
+
+ # Map erstellen
+
+ # Img Objekt
+ #cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+ #success, img = cap.read()
+
+ #Karte von Bild
+ #succ, img = cv2.imread(2)
+ print('Original Dimensions : ',img.shape)
+
+ scale_percent = 100-scale # percent of original size
+ width = int(img.shape[1] * scale_percent / 100)
+ height = int(img.shape[0] * scale_percent / 100)
+ dim = (width, height)
+
+ # resize image
+ resized = cv2.resize(img, dim, interpolation = cv2.INTER_AREA)
+ print('Resized Dimensions : ',resized.shape)
+
+ gray = cv2.cvtColor(resized, cv2.COLOR_RGB2GRAY)
+ mask = cv2.inRange(gray, np.array([90]), np.array([255]))
+
+ mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+ mask_list= np.ndarray.tolist(mask)
+
+ #Markiere alle leeren Zellen mit 000 und alle Zellen mit Hinderniss mit 999
+ for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['999' if j > 200 else '000' for j in mask_list[i]]
+
+ mapOfWorld = np.array(mapOfWorld, dtype = int)
+ mapOfWorldSized = mapOfWorld.copy()
+
+ e = 3
+ #Hindernisse Größer machen
+ for i in range(0,mapOfWorld.shape[0]):
+ for j in range(0,mapOfWorld.shape[1]):
+ for r in range(0,e):
+ try:
+ if (mapOfWorldSized[i][j] == 999):
+ mapOfWorld[i][j+e] = 999
+ mapOfWorld[i+e][j] = 999
+ mapOfWorld[i-e][j] = 999
+ mapOfWorld[i][j-e] = 999
+ mapOfWorld[i+e][j+e] = 999
+ mapOfWorld[i+e][j-e] = 999
+ mapOfWorld[i-e][j+e] = 999
+ mapOfWorld[i-e][j-e] = 999
+ except Exception:
+ continue
+
+ return mapOfWorld
+ ############################################################################
+
+ def scale_koord(self, sx, sy, gx, gy,scale):
+ scale_percent = 100-scale # percent of original size
+
+ sx = int(sx*scale_percent/100)
+ sy = int(sy*scale_percent/100)
+ gx = int(gx*scale_percent/100)
+ gy = int(gy*scale_percent/100)
+
+ return sx,sy,gx,gy
+ ############################################################################
+
+ def rescale_koord(self, path,scale):
+ scale_percent = 100-scale # percent of original size100
+
+ path = np.array(path)
+
+ for i in range(len(path)):
+ path[i] = path[i]*100/scale_percent
+
+ return path
+ ############################################################################
+
+ def calc_trans(self, x, y, height):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+###############################################################################
+
+if __name__ == "__main__":
+ """
+ X is vertical, Y is horizontal
+
+ """
+
+ Map = mapCreate() #Map Objekt erzeugen
+
+ #Verkleinerungsfaktor in %
+ scale = 85
+
+ img = cv2.imread("D:/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/karte.jpg", cv2.COLOR_RGB2GRAY)
+ #cap = cv2.VideoCapture(2)
+ mapWorld = Map.create_map(scale, img)
+ height, width = img.shape[:2]
+
+ #Angaben in Weltkoordinaten
+ sy = 300 #X-Koordinate im Weltkoordinaten System
+ sx = 200 #Y-Koordinate im Weltkoordinaten System
+
+ gy = 700 #X-Koordinate im Weltkordinaten System
+ gx = 240 #Y-Koordinate im Weltkoordinaten System
+
+ sx,sy,gx,gy = Map.scale_koord(sx,sy,gx,gy,scale) #Kordinaten Tauschen X,Y
+
+ start = time.time()
+ planner = waveFrontPlanner(mapWorld)
+ planner.setGoalPosition(gx,gy)
+ planner.setRobotPosition(sx,sy)
+ path = planner.run(False)
+ end = time.time()
+ print("Took %f seconds to run wavefront simulation" % (end - start))
+
+ path = Map.rescale_koord(path, scale)
+ #print(path)
+#%% Plot
+ #Programm Koordinaten
+ imgTrans = cv2.transpose(img) # X und Y-Achse im Bild tauschen
+ imgPlot, ax1 = plt.subplots()
+
+ ax1.set_title('Programm Koordinaten')
+ ax1.imshow(imgTrans)
+ ax1.set_xlabel('[px]')
+ ax1.set_ylabel('[px]')
+ ax1.scatter(path[:,0], path[:,1], color='r')
+
+ #Bild Koordinaten
+ imgPlot2, ax2 = plt.subplots()
+ ax2.set_title('Bild Koordinaten')
+ ax2.set_xlabel('[px]')
+ ax2.set_ylabel('[px]')
+ ax2.imshow(img)
+ ax2.scatter(path[:,1], path[:,0], color='r')
+
+#%% Sphero Pfad
+ pathWeltX, pathWeltY = Map.calc_trans(path[:,1], path[:,0], height)
\ No newline at end of file
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__init__.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/GameWavefront.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/GameWavefront.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..6ece39f995948bca62bb749c1b4a8dab486ff1e6
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/GameWavefront.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/WavefrontChatGpt.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/WavefrontChatGpt.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..fa1580cd086f8ded4733863dfce1d64fad32f881
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/WavefrontChatGpt.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/WavefrontPlanner.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/WavefrontPlanner.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..69ecb8c18f678b45bcd6677f7b077115e6a96516
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/WavefrontPlanner.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/__init__.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..ec9298825be9e809c9848ee863fea5c84f958e7f
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/__init__.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/_constants.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/_constants.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..7445d8092b0df51dc16b0e6b2f448a1c3754da6b
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/_constants.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/a_star.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/a_star.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..08bd874229860af63cd5c26676a69f05c506eb88
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/a_star.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/blobErkennung.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/blobErkennung.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..9a67c7471a3ce7a50008d13c5d01bf261017add2
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/blobErkennung.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/core.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/core.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..0f0d8bf82de3f38133e759720cfe87e1591b9d40
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/core.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/dynamicPathPlanning.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/dynamicPathPlanning.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..783f7aad7b95b157db735ab8cb5f819656e8d116
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/dynamicPathPlanning.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/map.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/map.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..620dd53b562bd7e966028c25d4cf45bea0a723f2
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/map.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/maperstellen.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/maperstellen.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..d7213af60b65c5c312c56bbf5e7092116999e2e1
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/maperstellen.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node copy.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node copy.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..ff02a86b8c9265a9299da6733a1150a03c43a61e
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node copy.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..248bb73e7affc7258d67ea37f5b04afb3cfde55b
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_alt.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_alt.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..06a366f1ce4b99ca1ddaf00017abd0a8c6868050
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_alt.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_komisch.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_komisch.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..c1e46cdc304f86d99a14af9101ff3d91dc2a4b89
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_komisch.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_refwinkel.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_refwinkel.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..e43bbb000175664ea4762646b70442df7a02e31f
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_refwinkel.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_testfahrt.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_testfahrt.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..76054ad895508d64e1faf9ec5c48338bd50f956e
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/my_first_node_testfahrt.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/pfadplanung.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/pfadplanung.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..9c01561f4863ce4dd3f66016afa225f0416bf5b5
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/pfadplanung.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/potential_field_planning (1).cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/potential_field_planning (1).cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..7f6164b8357f6cfb2c910cd87d914c0b16dcb959
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/potential_field_planning (1).cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/potential_field_planning.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/potential_field_planning.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..89654d357ee99d891520683d490f3272debfb4a9
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/potential_field_planning.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/simple_moves.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/simple_moves.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..650d674336f70da01d14cf5aebb304a70229227c
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/simple_moves.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/sphero.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/sphero.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..c18e5dffcef2b5c7adb3e92614aaaa2715e23296
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/sphero.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/sphero_main.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/sphero_main.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..d95a51ee66ceb429dcd3290ef44c9288bc375fdb
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/sphero_main.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/test.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/test.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..1652f1531f8f1fa2134712bba5ab5118856c0730
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/test.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/treiber.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/treiber.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..371445741325a38df010cc651d7b73d397ff1efc
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/treiber.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/wavefront.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/wavefront.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..751df02c9c93c0c2ace4b207f86ae0998a1571c7
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/wavefront.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/wavefront_coverage_path_planner.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/wavefront_coverage_path_planner.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..bb64c53ebc91edff78d10e432020a92c12d0dd20
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/wavefront_coverage_path_planner.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/wavefrontgpt2.cpython-310.pyc b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/wavefrontgpt2.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..693e46d875943c27484df42e4d6294ef7faa6e5f
Binary files /dev/null and b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/__pycache__/wavefrontgpt2.cpython-310.pyc differ
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/_constants.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/_constants.py
new file mode 100644
index 0000000000000000000000000000000000000000..fa8a0e875eed9f484945c1eeaf2fda3fd98503a4
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/_constants.py
@@ -0,0 +1,72 @@
+'''
+Known Peripheral UUIDs, obtained by querying using the Bluepy module:
+=====================================================================
+Anti DOS Characteristic <00020005-574f-4f20-5370-6865726f2121>
+Battery Level Characteristic <Battery Level>
+Peripheral Preferred Connection Parameters Characteristic <Peripheral Preferred Connection Parameters>
+API V2 Characteristic <00010002-574f-4f20-5370-6865726f2121>
+DFU Control Characteristic <00020002-574f-4f20-5370-6865726f2121>
+Name Characteristic <Device Name>
+Appearance Characteristic <Appearance>
+DFU Info Characteristic <00020004-574f-4f20-5370-6865726f2121>
+Service Changed Characteristic <Service Changed>
+Unknown1 Characteristic <00020003-574f-4f20-5370-6865726f2121>
+Unknown2 Characteristic <00010003-574f-4f20-5370-6865726f2121>
+
+The rest of the values saved in the dictionaries below, were borrowed from
+@igbopie's javacript library, which is available at https://github.com/igbopie/spherov2.js
+
+'''
+
+deviceID = {"apiProcessor": 0x10, # 16
+ "systemInfo": 0x11, # 17
+ "powerInfo": 0x13, # 19
+ "driving": 0x16, # 22
+ "animatronics": 0x17, # 23
+ "sensor": 0x18, # 24
+ "something": 0x19, # 25
+ "userIO": 0x1a, # 26
+ "somethingAPI": 0x1f} # 31
+
+SystemInfoCommands = {"mainApplicationVersion": 0x00, # 00
+ "bootloaderVersion": 0x01, # 01
+ "something": 0x06, # 06
+ "something2": 0x13, # 19
+ "something6": 0x12, # 18
+ "something7": 0x28} # 40
+
+sendPacketConstants = {"StartOfPacket": 0x8d, # 141
+ "EndOfPacket": 0xd8} # 216
+
+userIOCommandIDs = {"allLEDs": 0x0e} # 14
+
+flags= {"isResponse": 0x01, # 0x01
+ "requestsResponse": 0x02, # 0x02
+ "requestsOnlyErrorResponse": 0x04, # 0x04
+ "resetsInactivityTimeout": 0x08} # 0x08
+
+powerCommandIDs={"deepSleep": 0x00, # 0
+ "sleep": 0x01, # 01
+ "batteryVoltage": 0x03, # 03
+ "wake": 0x0D, # 13
+ "something": 0x05, # 05
+ "something2": 0x10, # 16
+ "something3": 0x04, # 04
+ "something4": 0x1E} # 30
+
+drivingCommands={"rawMotor": 0x01, # 1
+ "resetHeading": 0x06, # 6
+ "driveAsSphero": 0x04, # 4
+ "driveAsRc": 0x02, # 2
+ "driveWithHeading": 0x07, # 7
+ "stabilization": 0x0C} # 12
+
+sensorCommands={'sensorMask': 0x00, # 00
+ 'sensorResponse': 0x02, # 02
+ 'configureCollision': 0x11, # 17
+ 'collisionDetectedAsync': 0x12, # 18
+ 'resetLocator': 0x13, # 19
+ 'enableCollisionAsync': 0x14, # 20
+ 'sensor1': 0x0F, # 15
+ 'sensor2': 0x17, # 23
+ 'configureSensorStream': 0x0C} # 12
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/a_star.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/a_star.py
new file mode 100644
index 0000000000000000000000000000000000000000..6d203504327dd356614b08d9d8f169f63aa2ad55
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/a_star.py
@@ -0,0 +1,282 @@
+"""
+
+A* grid planning
+
+author: Atsushi Sakai(@Atsushi_twi)
+ Nikos Kanargias (nkana@tee.gr)
+
+See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
+
+"""
+
+import math
+
+import matplotlib.pyplot as plt
+
+show_animation = True
+
+
+class AStarPlanner:
+
+ def __init__(self, ox, oy, resolution, rr):
+ """
+ Initialize grid map for a star planning
+
+ ox: x position list of Obstacles [m]
+ oy: y position list of Obstacles [m]
+ resolution: grid resolution [m]
+ rr: robot radius[m]
+ """
+
+ self.resolution = resolution
+ self.rr = rr
+ self.min_x, self.min_y = 0, 0
+ self.max_x, self.max_y = 0, 0
+ self.obstacle_map = None
+ self.x_width, self.y_width = 0, 0
+ self.motion = self.get_motion_model()
+ self.calc_obstacle_map(ox, oy)
+
+ class Node:
+ def __init__(self, x, y, cost, parent_index):
+ self.x = x # index of grid
+ self.y = y # index of grid
+ self.cost = cost
+ self.parent_index = parent_index
+
+ def __str__(self):
+ return str(self.x) + "," + str(self.y) + "," + str(
+ self.cost) + "," + str(self.parent_index)
+
+ def planning(self, sx, sy, gx, gy):
+ """
+ A star path search
+
+ input:
+ s_x: start x position [m]
+ s_y: start y position [m]
+ gx: goal x position [m]
+ gy: goal y position [m]
+
+ output:
+ rx: x position list of the final path
+ ry: y position list of the final path
+ """
+
+ start_node = self.Node(self.calc_xy_index(sx, self.min_x),
+ self.calc_xy_index(sy, self.min_y), 0.0, -1)
+ goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
+ self.calc_xy_index(gy, self.min_y), 0.0, -1)
+
+ open_set, closed_set = dict(), dict()
+ open_set[self.calc_grid_index(start_node)] = start_node
+
+ while True:
+ if len(open_set) == 0:
+ print("Open set is empty..")
+ break
+
+ c_id = min(
+ open_set,
+ key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node,
+ open_set[
+ o]))
+ current = open_set[c_id]
+
+ # show graph
+ if show_animation: # pragma: no cover
+ plt.plot(self.calc_grid_position(current.x, self.min_x),
+ self.calc_grid_position(current.y, self.min_y), "xc")
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect('key_release_event',
+ lambda event: [exit(
+ 0) if event.key == 'escape' else None])
+ if len(closed_set.keys()) % 10 == 0:
+ plt.pause(0.001)
+
+ if current.x == goal_node.x and current.y == goal_node.y:
+ print("Find goal")
+ goal_node.parent_index = current.parent_index
+ goal_node.cost = current.cost
+ break
+
+ # Remove the item from the open set
+ del open_set[c_id]
+
+ # Add it to the closed set
+ closed_set[c_id] = current
+
+ # expand_grid search grid based on motion model
+ for i, _ in enumerate(self.motion):
+ node = self.Node(current.x + self.motion[i][0],
+ current.y + self.motion[i][1],
+ current.cost + self.motion[i][2], c_id)
+ n_id = self.calc_grid_index(node)
+
+ # If the node is not safe, do nothing
+ if not self.verify_node(node):
+ continue
+
+ if n_id in closed_set:
+ continue
+
+ if n_id not in open_set:
+ open_set[n_id] = node # discovered a new node
+ else:
+ if open_set[n_id].cost > node.cost:
+ # This path is the best until now. record it
+ open_set[n_id] = node
+
+ rx, ry = self.calc_final_path(goal_node, closed_set)
+
+ return rx, ry
+
+ def calc_final_path(self, goal_node, closed_set):
+ # generate final course
+ rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [
+ self.calc_grid_position(goal_node.y, self.min_y)]
+ parent_index = goal_node.parent_index
+ while parent_index != -1:
+ n = closed_set[parent_index]
+ rx.append(self.calc_grid_position(n.x, self.min_x))
+ ry.append(self.calc_grid_position(n.y, self.min_y))
+ parent_index = n.parent_index
+
+ return rx, ry
+
+ @staticmethod
+ def calc_heuristic(n1, n2):
+ w = 1.0 # weight of heuristic
+ d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
+ return d
+
+ def calc_grid_position(self, index, min_position):
+ """
+ calc grid position
+
+ :param index:
+ :param min_position:
+ :return:
+ """
+ pos = index * self.resolution + min_position
+ return pos
+
+ def calc_xy_index(self, position, min_pos):
+ return round((position - min_pos) / self.resolution)
+
+ def calc_grid_index(self, node):
+ return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)
+
+ def verify_node(self, node):
+ px = self.calc_grid_position(node.x, self.min_x)
+ py = self.calc_grid_position(node.y, self.min_y)
+
+ if px < self.min_x:
+ return False
+ elif py < self.min_y:
+ return False
+ elif px >= self.max_x:
+ return False
+ elif py >= self.max_y:
+ return False
+
+ # collision check
+ if self.obstacle_map[node.x][node.y]:
+ return False
+
+ return True
+
+ def calc_obstacle_map(self, ox, oy):
+
+ self.min_x = round(min(ox))
+ self.min_y = round(min(oy))
+ self.max_x = round(max(ox))
+ self.max_y = round(max(oy))
+ print("min_x:", self.min_x)
+ print("min_y:", self.min_y)
+ print("max_x:", self.max_x)
+ print("max_y:", self.max_y)
+
+ self.x_width = round((self.max_x - self.min_x) / self.resolution)
+ self.y_width = round((self.max_y - self.min_y) / self.resolution)
+ print("x_width:", self.x_width)
+ print("y_width:", self.y_width)
+
+ # obstacle map generation
+ self.obstacle_map = [[False for _ in range(self.y_width)]
+ for _ in range(self.x_width)]
+ for ix in range(self.x_width):
+ x = self.calc_grid_position(ix, self.min_x)
+ for iy in range(self.y_width):
+ y = self.calc_grid_position(iy, self.min_y)
+ for iox, ioy in zip(ox, oy):
+ d = math.hypot(iox - x, ioy - y)
+ if d <= self.rr:
+ self.obstacle_map[ix][iy] = True
+ break
+
+ @staticmethod
+ def get_motion_model():
+ # dx, dy, cost
+ motion = [[1, 0, 1],
+ [0, 1, 1],
+ [-1, 0, 1],
+ [0, -1, 1],
+ [-1, -1, math.sqrt(2)],
+ [-1, 1, math.sqrt(2)],
+ [1, -1, math.sqrt(2)],
+ [1, 1, math.sqrt(2)]]
+
+ return motion
+
+
+def main():
+ print(__file__ + " start!!")
+
+ # start and goal position
+ sx = 10.0 # [m]
+ sy = 10.0 # [m]
+ gx = 50.0 # [m]
+ gy = 50.0 # [m]
+ grid_size = 2.0 # [m]
+ robot_radius = 1.0 # [m]
+
+ # set obstacle positions
+ ox, oy = [], []
+ for i in range(-10, 60):
+ ox.append(i)
+ oy.append(-10.0)
+ for i in range(-10, 60):
+ ox.append(60.0)
+ oy.append(i)
+ for i in range(-10, 61):
+ ox.append(i)
+ oy.append(60.0)
+ for i in range(-10, 61):
+ ox.append(-10.0)
+ oy.append(i)
+ for i in range(-10, 40):
+ ox.append(20.0)
+ oy.append(i)
+ for i in range(0, 40):
+ ox.append(40.0)
+ oy.append(60.0 - i)
+
+ if show_animation: # pragma: no cover
+ plt.plot(ox, oy, ".k")
+ plt.plot(sx, sy, "og")
+ plt.plot(gx, gy, "xb")
+ plt.grid(True)
+ plt.axis("equal")
+
+ a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
+ rx, ry = a_star.planning(sx, sy, gx, gy)
+
+ if show_animation: # pragma: no cover
+ plt.plot(rx, ry, "-r")
+ plt.pause(0.001)
+ plt.show()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/blobErkennung.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/blobErkennung.py
new file mode 100644
index 0000000000000000000000000000000000000000..20036f56c3bb1f237bb1ec84a06c18afa70b5308
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/blobErkennung.py
@@ -0,0 +1,80 @@
+#!/usr/bin/env python3
+
+# Standard imports
+import cv2
+import numpy as np;
+
+# Img Objekt
+#cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+cap = cv2.VideoCapture(4)
+
+# Set up the detector with default parameters.
+detector = cv2.SimpleBlobDetector()
+
+# Setup SimpleBlobDetector parameters.
+params = cv2.SimpleBlobDetector_Params()
+
+# Change thresholds
+#params.minThreshold = 5
+#params.maxThreshold = 500
+
+#FIlter by Color
+params.filterByColor = True
+params.blobColor = 255
+
+# Filter by Area.
+params.filterByArea = True
+params.minArea = 20
+params.maxArea = 200
+
+# Filter by Circularity
+#params.filterByCircularity = True
+#params.minCircularity = 0.5
+#params.maxCircularity = 1
+
+# Filter by Convexity
+#params.filterByConvexity = True
+#params.minConvexity = 0.7
+#params.maxConvexity = 1
+
+# Filter by Inertia
+params.filterByInertia = True
+params.minInertiaRatio = 0.5
+
+# Create a detector with the parameters
+detector = cv2.SimpleBlobDetector_create(params)
+
+success, img = cap.read()
+height, width = img.shape[:2]
+
+def calc_trans(x,y):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+
+while True:
+
+ success, img = cap.read()
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+
+ # Detect blobs.
+ keypoints = detector.detect(gray)
+
+ #print(keypoints)
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+ #s = keyPoint.sizekeyPoints
+
+ # Draw detected blobs as red circles.
+ # cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures
+ # the size of the circle corresponds to the size of blob
+
+ im_with_keypoints = cv2.drawKeypoints(gray, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
+
+ # Show blobs
+ cv2.imshow("Keypoints", im_with_keypoints)
+
+ if cv2.waitKey(10) & 0xFF == ord('q'):
+ break
\ No newline at end of file
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/core.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/core.py
new file mode 100644
index 0000000000000000000000000000000000000000..e8fecdcfd3cef71e846e88909c52fd5eb52800da
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/core.py
@@ -0,0 +1,638 @@
+from bluepy.btle import Peripheral
+from bluepy import btle
+from _constants import *
+import struct
+import time
+import sys
+
+class SpheroMini():
+ def __init__(self, MACAddr, verbosity = 4, user_delegate = None):
+ '''
+ initialize class instance and then build collect BLE sevices and characteristics.
+ Also sends text string to Anti-DOS characteristic to prevent returning to sleep,
+ and initializes notifications (which is what the sphero uses to send data back to
+ the client).
+ '''
+ self.verbosity = verbosity # 0 = Silent,
+ # 1 = Connection/disconnection only
+ # 2 = Init messages
+ # 3 = Recieved commands
+ # 4 = Acknowledgements
+ self.sequence = 1
+ self.v_batt = None # will be updated with battery voltage when sphero.getBatteryVoltage() is called
+ self.firmware_version = [] # will be updated with firware version when sphero.returnMainApplicationVersion() is called
+
+ if self.verbosity > 0:
+ print("[INFO] Connecting to %s", MACAddr)
+ self.p = Peripheral(MACAddr, "random") #connect
+
+ if self.verbosity > 1:
+ print("[INIT] Initializing")
+
+ # Subscribe to notifications
+ self.sphero_delegate = MyDelegate(self, user_delegate) # Pass a reference to this instance when initializing
+ self.p.setDelegate(self.sphero_delegate)
+
+ if self.verbosity > 1:
+ print("[INIT] Read all characteristics and descriptors")
+ # Get characteristics and descriptors
+ self.API_V2_characteristic = self.p.getCharacteristics(uuid="00010002-574f-4f20-5370-6865726f2121")[0]
+ self.AntiDOS_characteristic = self.p.getCharacteristics(uuid="00020005-574f-4f20-5370-6865726f2121")[0]
+ self.DFU_characteristic = self.p.getCharacteristics(uuid="00020002-574f-4f20-5370-6865726f2121")[0]
+ self.DFU2_characteristic = self.p.getCharacteristics(uuid="00020004-574f-4f20-5370-6865726f2121")[0]
+ self.API_descriptor = self.API_V2_characteristic.getDescriptors(forUUID=0x2902)[0]
+ self.DFU_descriptor = self.DFU_characteristic.getDescriptors(forUUID=0x2902)[0]
+
+ # The rest of this sequence was observed during bluetooth sniffing:
+ # Unlock code: prevent the sphero mini from going to sleep again after 10 seconds
+ if self.verbosity > 1:
+ print("[INIT] Writing AntiDOS characteristic unlock code")
+ self.AntiDOS_characteristic.write("usetheforce...band".encode(), withResponse=True)
+
+ # Enable DFU notifications:
+ if self.verbosity > 1:
+ print("[INIT] Configuring DFU descriptor")
+ self.DFU_descriptor.write(struct.pack('<bb', 0x01, 0x00), withResponse = True)
+
+ # No idea what this is for. Possibly a device ID of sorts? Read request returns '00 00 09 00 0c 00 02 02':
+ if self.verbosity > 1:
+ print("[INIT] Reading DFU2 characteristic")
+ _ = self.DFU2_characteristic.read()
+
+ # Enable API notifications:
+ if self.verbosity > 1:
+ print("[INIT] Configuring API dectriptor")
+ self.API_descriptor.write(struct.pack('<bb', 0x01, 0x00), withResponse = True)
+
+ self.wake()
+
+ # Finished initializing:
+ if self.verbosity > 1:
+ print("[INIT] Initialization complete\n")
+
+ def disconnect(self):
+ if self.verbosity > 0:
+ print("[INFO] Disconnecting")
+
+ self.p.disconnect()
+
+ def wake(self):
+ '''
+ Bring device out of sleep mode (can only be done if device was in sleep, not deep sleep).
+ If in deep sleep, the device should be connected to USB power to wake.
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Waking".format(self.sequence))
+
+ self._send(characteristic=self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=powerCommandIDs["wake"],
+ payload=[]) # empty payload
+
+ self.getAcknowledgement("Wake")
+
+ def sleep(self, deepSleep=False):
+ '''
+ Put device to sleep or deep sleep (deep sleep needs USB power connected to wake up)
+ '''
+ if deepSleep:
+ sleepCommID=powerCommandIDs["deepSleep"]
+ if self.verbosity > 0:
+ print("[INFO] Going into deep sleep. Connect USB power to wake.")
+ else:
+ sleepCommID=powerCommandIDs["sleep"]
+ self._send(characteristic=self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=sleepCommID,
+ payload=[]) #empty payload
+
+ def setLEDColor(self, red = None, green = None, blue = None):
+ '''
+ Set device LED color based on RGB vales (each can range between 0 and 0xFF)
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Setting main LED colour to [{}, {}, {}]".format(self.sequence, red, green, blue))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['userIO'], # 0x1a
+ commID = userIOCommandIDs["allLEDs"], # 0x0e
+ payload = [0x00, 0x0e, red, green, blue])
+
+ self.getAcknowledgement("LED/backlight")
+
+ def setBackLEDIntensity(self, brightness=None):
+ '''
+ Set device LED backlight intensity based on 0-255 values
+
+ NOTE: this is not the same as aiming - it only turns on the LED
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Setting backlight intensity to {}".format(self.sequence, brightness))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['userIO'],
+ commID = userIOCommandIDs["allLEDs"],
+ payload = [0x00, 0x01, brightness])
+
+ self.getAcknowledgement("LED/backlight")
+
+ def roll(self, speed=None, heading=None):
+ '''
+ Start to move the Sphero at a given direction and speed.
+ heading: integer from 0 - 360 (degrees)
+ speed: Integer from 0 - 255
+
+ Note: the zero heading should be set at startup with the resetHeading method. Otherwise, it may
+ seem that the sphero doesn't honor the heading argument
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Rolling with speed {} and heading {}".format(self.sequence, speed, heading))
+
+ if abs(speed) > 255:
+ print("WARNING: roll speed parameter outside of allowed range (-255 to +255)")
+
+ if speed < 0:
+ speed = -1*speed+256 # speed values > 256 in the send packet make the spero go in reverse
+
+ speedH = (speed & 0xFF00) >> 8
+ speedL = speed & 0xFF
+ headingH = (heading & 0xFF00) >> 8
+ headingL = heading & 0xFF
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['driving'],
+ commID = drivingCommands["driveWithHeading"],
+ payload = [speedL, headingH, headingL, speedH])
+
+ self.getAcknowledgement("Roll")
+
+ def resetHeading(self):
+ '''
+ Reset the heading zero angle to the current heading (useful during aiming)
+ Note: in order to manually rotate the sphero, you need to call stabilization(False).
+ Once the heading has been set, call stabilization(True).
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Resetting heading".format(self.sequence))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['driving'],
+ commID = drivingCommands["resetHeading"],
+ payload = []) #empty payload
+
+ self.getAcknowledgement("Heading")
+
+ def returnMainApplicationVersion(self):
+ '''
+ Sends command to return application data in a notification
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Requesting firmware version".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID = deviceID['systemInfo'],
+ commID = SystemInfoCommands['mainApplicationVersion'],
+ payload = []) # empty
+
+ self.getAcknowledgement("Firmware")
+
+ def getBatteryVoltage(self):
+ '''
+ Sends command to return battery voltage data in a notification.
+ Data printed to console screen by the handleNotifications() method in the MyDelegate class.
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Requesting battery voltage".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=powerCommandIDs['batteryVoltage'],
+ payload=[]) # empty
+
+ self.getAcknowledgement("Battery")
+
+ def stabilization(self, stab = True):
+ '''
+ Sends command to turn on/off the motor stabilization system (required when manually turning/aiming the sphero)
+ '''
+ if stab == True:
+ if self.verbosity > 2:
+ print("[SEND {}] Enabling stabilization".format(self.sequence))
+ val = 1
+ else:
+ if self.verbosity > 2:
+ print("[SEND {}] Disabling stabilization".format(self.sequence))
+ val = 0
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['driving'],
+ commID=drivingCommands['stabilization'],
+ payload=[val])
+
+ self.getAcknowledgement("Stabilization")
+
+ def wait(self, delay):
+ '''
+ This is a non-blocking delay command. It is similar to time.sleep(), except it allows asynchronous
+ notification handling to still be performed.
+ '''
+ start = time.time()
+ while(1):
+ self.p.waitForNotifications(0.001)
+ if time.time() - start > delay:
+ break
+
+ def _send(self, characteristic=None, devID=None, commID=None, payload=[]):
+ '''
+ A generic "send" method, which will be used by other methods to send a command ID, payload and
+ appropriate checksum to a specified device ID. Mainly useful because payloads are optional,
+ and can be of varying length, to convert packets to binary, and calculate and send the
+ checksum. For internal use only.
+
+ Packet structure has the following format (in order):
+
+ - Start byte: always 0x8D
+ - Flags byte: indicate response required, etc
+ - Virtual device ID: see _constants.py
+ - Command ID: see _constants.py
+ - Sequence number: Seems to be arbitrary. I suspect it is used to match commands to response packets (in which the number is echoed).
+ - Payload: Could be varying number of bytes (incl. none), depending on the command
+ - Checksum: See below for calculation
+ - End byte: always 0xD8
+
+ '''
+ sendBytes = [sendPacketConstants["StartOfPacket"],
+ sum([flags["resetsInactivityTimeout"], flags["requestsResponse"]]),
+ devID,
+ commID,
+ self.sequence] + payload # concatenate payload list
+
+ self.sequence += 1 # Increment sequence number, ensures we can identify response packets are for this command
+ if self.sequence > 255:
+ self.sequence = 0
+
+ # Compute and append checksum and add EOP byte:
+ # From Sphero docs: "The [checksum is the] modulo 256 sum of all the bytes
+ # from the device ID through the end of the data payload,
+ # bit inverted (1's complement)"
+ # For the sphero mini, the flag bits must be included too:
+ checksum = 0
+ for num in sendBytes[1:]:
+ checksum = (checksum + num) & 0xFF # bitwise "and to get modulo 256 sum of appropriate bytes
+ checksum = 0xff - checksum # bitwise 'not' to invert checksum bits
+ sendBytes += [checksum, sendPacketConstants["EndOfPacket"]] # concatenate
+
+ # Convert numbers to bytes
+ output = b"".join([x.to_bytes(1, byteorder='big') for x in sendBytes])
+
+ #send to specified characteristic:
+ characteristic.write(output, withResponse = True)
+
+ def getAcknowledgement(self, ack):
+ #wait up to 10 secs for correct acknowledgement to come in, including sequence number!
+ start = time.time()
+ while(1):
+ self.p.waitForNotifications(1)
+ if self.sphero_delegate.notification_seq == self.sequence-1: # use one less than sequence, because _send function increments it for next send.
+ if self.verbosity > 3:
+ print("[RESP {}] {}".format(self.sequence-1, self.sphero_delegate.notification_ack))
+ self.sphero_delegate.clear_notification()
+ break
+ elif self.sphero_delegate.notification_seq >= 0:
+ print("Unexpected ACK. Expected: {}/{}, received: {}/{}".format(
+ ack, self.sequence, self.sphero_delegate.notification_ack.split()[0],
+ self.sphero_delegate.notification_seq)
+ )
+ if time.time() > start + 10:
+ print("Timeout waiting for acknowledgement: {}/{}".format(ack, self.sequence))
+ break
+
+# =======================================================================
+# The following functions are experimental:
+# =======================================================================
+
+ def configureCollisionDetection(self,
+ xThreshold = 50,
+ yThreshold = 50,
+ xSpeed = 50,
+ ySpeed = 50,
+ deadTime = 50, # in 10 millisecond increments
+ method = 0x01, # Must be 0x01
+ callback = None):
+ '''
+ Appears to function the same as other Sphero models, however speed settings seem to have no effect.
+ NOTE: Setting to zero seems to cause bluetooth errors with the Sphero Mini/bluepy library - set to
+ 255 to make it effectively disabled.
+
+ deadTime disables future collisions for a short period of time to avoid repeat triggering by the same
+ event. Set in 10ms increments. So if deadTime = 50, that means the delay will be 500ms, or half a second.
+
+ From Sphero docs:
+
+ xThreshold/yThreshold: An 8-bit settable threshold for the X (left/right) and Y (front/back) axes
+ of Sphero.
+
+ xSpeed/ySpeed: An 8-bit settable speed value for the X and Y axes. This setting is ranged by the
+ speed, then added to xThreshold, yThreshold to generate the final threshold value.
+ '''
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring collision detection".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['configureCollision'],
+ payload=[method, xThreshold, xSpeed, yThreshold, ySpeed, deadTime])
+
+ self.collision_detection_callback = callback
+
+ self.getAcknowledgement("Collision")
+
+ def configureSensorStream(self): # Use default values
+ '''
+ Send command to configure sensor stream using default values as found during bluetooth
+ sniffing of the Sphero Edu app.
+
+ Must be called after calling configureSensorMask()
+ '''
+ bitfield1 = 0b00000000 # Unknown function - needs experimenting
+ bitfield2 = 0b00000000 # Unknown function - needs experimenting
+ bitfield3 = 0b00000000 # Unknown function - needs experimenting
+ bitfield4 = 0b00000000 # Unknown function - needs experimenting
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring sensor stream".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['configureSensorStream'],
+ payload=[bitfield1, bitfield1, bitfield1, bitfield1])
+
+ self.getAcknowledgement("Sensor")
+
+ def configureSensorMask(self,
+ sample_rate_divisor = 0x25, # Must be > 0
+ packet_count = 0,
+ IMU_pitch = False,
+ IMU_roll = False,
+ IMU_yaw = False,
+ IMU_acc_x = False,
+ IMU_acc_y = False,
+ IMU_acc_z = False,
+ IMU_gyro_x = False,
+ IMU_gyro_y = False,
+ IMU_gyro_z = False):
+
+ '''
+ Send command to configure sensor mask using default values as found during bluetooth
+ sniffing of the Sphero Edu app. From experimentation, it seems that these are he functions of each:
+
+ Sampling_rate_divisor. Slow data EG: Set to 0x32 to the divide data rate by 50. Setting below 25 (0x19) causes
+ bluetooth errors
+
+ Packet_count: Select the number of packets to transmit before ending the stream. Set to zero to stream infinitely
+
+ All IMU bool parameters: Toggle transmission of that value on or off (e.g. set IMU_acc_x = True to include the
+ X-axis accelerometer readings in the sensor stream)
+ '''
+
+ # Construct bitfields based on function parameters:
+ IMU_bitfield1 = (IMU_pitch<<2) + (IMU_roll<<1) + IMU_yaw
+ IMU_bitfield2 = ((IMU_acc_y<<7) + (IMU_acc_z<<6) + (IMU_acc_x<<5) + \
+ (IMU_gyro_y<<4) + (IMU_gyro_x<<2) + (IMU_gyro_z<<2))
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring sensor mask".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensorMask'],
+ payload=[0x00, # Unknown param - altering it seems to slow data rate. Possibly averages multiple readings?
+ sample_rate_divisor,
+ packet_count, # Packet count: select the number of packets to stop streaming after (zero = infinite)
+ 0b00, # Unknown param: seems to be another accelerometer bitfield? Z-acc, Y-acc
+ IMU_bitfield1,
+ IMU_bitfield2,
+ 0b00]) # reserved, Position?, Position?, velocity?, velocity?, Y-gyro, timer, reserved
+
+ self.getAcknowledgement("Mask")
+
+ '''
+ Since the sensor values arrive as unlabelled lists in the order that they appear in the bitfields above, we need
+ to create a list of sensors that have been configured.Once we have this list, then in the default_delegate class,
+ we can get sensor values as attributes of the sphero_mini class.
+ e.g. print(sphero.IMU_yaw) # displays the current yaw angle
+ '''
+
+ # Initialize dictionary with sensor names as keys and their bool values (set by the user) as values:
+ availableSensors = {"IMU_pitch" : IMU_pitch,
+ "IMU_roll" : IMU_roll,
+ "IMU_yaw" : IMU_yaw,
+ "IMU_acc_y" : IMU_acc_y,
+ "IMU_acc_z" : IMU_acc_z,
+ "IMU_acc_x" : IMU_acc_x,
+ "IMU_gyro_y" : IMU_gyro_y,
+ "IMU_gyro_x" : IMU_gyro_x,
+ "IMU_gyro_z" : IMU_gyro_z}
+
+ # Create list of of only sensors that have been "activated" (set as true in the method arguments):
+ self.configured_sensors = [name for name in availableSensors if availableSensors[name] == True]
+
+ def sensor1(self): # Use default values
+ '''
+ Unknown function. Observed in bluetooth sniffing.
+ '''
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensor1'],
+ payload=[0x01])
+
+ self.getAcknowledgement("Sensor1")
+
+ def sensor2(self): # Use default values
+ '''
+ Unknown function. Observed in bluetooth sniffing.
+ '''
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensor2'],
+ payload=[0x00])
+
+ self.getAcknowledgement("Sensor2")
+
+# =======================================================================
+
+class MyDelegate(btle.DefaultDelegate):
+
+ '''
+ This class handles notifications (both responses and asynchronous notifications).
+
+ Usage of this class is described in the Bluepy documentation
+
+ '''
+
+ def __init__(self, sphero_class, user_delegate):
+ self.sphero_class = sphero_class # for saving sensor values as attributes of sphero class instance
+ self.user_delegate = user_delegate # to directly notify users of callbacks
+ btle.DefaultDelegate.__init__(self)
+ self.clear_notification()
+ self.notificationPacket = []
+
+ def clear_notification(self):
+ self.notification_ack = "DEFAULT ACK"
+ self.notification_seq = -1
+
+ def bits_to_num(self, bits):
+ '''
+ This helper function decodes bytes from sensor packets into single precision floats. Encoding follows the
+ the IEEE-754 standard.
+ '''
+ num = int(bits, 2).to_bytes(len(bits) // 8, byteorder='little')
+ num = struct.unpack('f', num)[0]
+ return num
+
+ def handleNotification(self, cHandle, data):
+ '''
+ This method acts as an interrupt service routine. When a notification comes in, this
+ method is invoked, with the variable 'cHandle' being the handle of the characteristic that
+ sent the notification, and 'data' being the payload (sent one byte at a time, so the packet
+ needs to be reconstructed)
+
+ The method keeps appending bytes to the payload packet byte list until end-of-packet byte is
+ encountered. Note that this is an issue, because 0xD8 could be sent as part of the payload of,
+ say, the battery voltage notification. In future, a more sophisticated method will be required.
+ '''
+ # Allow the user to intercept and process data first..
+ if self.user_delegate != None:
+ if self.user_delegate.handleNotification(cHandle, data):
+ return
+
+ print("Received notification with packet %s", str(data))
+
+ for data_byte in data: # parse each byte separately (sometimes they arrive simultaneously)
+
+ self.notificationPacket.append(data_byte) # Add new byte to packet list
+
+ # If end of packet (need to find a better way to segment the packets):
+ if data_byte == sendPacketConstants['EndOfPacket']:
+ # Once full the packet has arrived, parse it:
+ # Packet structure is similar to the outgoing send packets (see docstring in sphero_mini._send())
+
+ # Attempt to unpack. Might fail if packet is too badly corrupted
+ try:
+ start, flags_bits, devid, commcode, seq, *notification_payload, chsum, end = self.notificationPacket
+ except ValueError:
+ print("Warning: notification packet unparseable %s", self.notificationPacket)
+ self.notificationPacket = [] # Discard this packet
+ return # exit
+
+ # Compute and append checksum and add EOP byte:
+ # From Sphero docs: "The [checksum is the] modulo 256 sum of all the bytes
+ # from the device ID through the end of the data payload,
+ # bit inverted (1's complement)"
+ # For the sphero mini, the flag bits must be included too:
+ checksum_bytes = [flags_bits, devid, commcode, seq] + notification_payload
+ checksum = 0 # init
+ for num in checksum_bytes:
+ checksum = (checksum + num) & 0xFF # bitwise "and to get modulo 256 sum of appropriate bytes
+ checksum = 0xff - checksum # bitwise 'not' to invert checksum bits
+ if checksum != chsum: # check computed checksum against that recieved in the packet
+ print("Warning: notification packet checksum failed - %s", str(self.notificationPacket))
+ self.notificationPacket = [] # Discard this packet
+ return # exit
+
+ # Check if response packet:
+ if flags_bits & flags['isResponse']: # it is a response
+
+ # Use device ID and command code to determine which command is being acknowledged:
+ if devid == deviceID['powerInfo'] and commcode == powerCommandIDs['wake']:
+ self.notification_ack = "Wake acknowledged" # Acknowledgement after wake command
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands['driveWithHeading']:
+ self.notification_ack = "Roll command acknowledged"
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands['stabilization']:
+ self.notification_ack = "Stabilization command acknowledged"
+
+ elif devid == deviceID['userIO'] and commcode == userIOCommandIDs['allLEDs']:
+ self.notification_ack = "LED/backlight color command acknowledged"
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands["resetHeading"]:
+ self.notification_ack = "Heading reset command acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["configureCollision"]:
+ self.notification_ack = "Collision detection configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["configureSensorStream"]:
+ self.notification_ack = "Sensor stream configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensorMask"]:
+ self.notification_ack = "Mask configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensor1"]:
+ self.notification_ack = "Sensor1 acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensor2"]:
+ self.notification_ack = "Sensor2 acknowledged"
+
+ elif devid == deviceID['powerInfo'] and commcode == powerCommandIDs['batteryVoltage']:
+ V_batt = notification_payload[2] + notification_payload[1]*256 + notification_payload[0]*65536
+ V_batt /= 100 # Notification gives V_batt in 10mV increments. Divide by 100 to get to volts.
+ self.notification_ack = "Battery voltage:" + str(V_batt) + "v"
+ self.sphero_class.v_batt = V_batt
+
+ elif devid == deviceID['systemInfo'] and commcode == SystemInfoCommands['mainApplicationVersion']:
+ version = '.'.join(str(x) for x in notification_payload)
+ self.notification_ack = "Firmware version: " + version
+ self.sphero_class.firmware_version = notification_payload
+
+ else:
+ self.notification_ack = "Unknown acknowledgement" #print(self.notificationPacket)
+ print(self.notificationPacket, "===================> Unknown ack packet")
+
+ self.notification_seq = seq
+
+ else: # Not a response packet - therefore, asynchronous notification (e.g. collision detection, etc):
+
+ # Collision detection:
+ if devid == deviceID['sensor'] and commcode == sensorCommands['collisionDetectedAsync']:
+ # The first four bytes are data that is still un-parsed. the remaining unsaved bytes are always zeros
+ _, _, _, _, _, _, axis, _, Y_mag, _, X_mag, *_ = notification_payload
+ if axis == 1:
+ dir = "Left/right"
+ else:
+ dir = 'Forward/back'
+ print("Collision detected:")
+ print("\tAxis: %s", dir)
+ print("\tX_mag: %s", X_mag)
+ print("\tY_mag: %s", Y_mag)
+
+ if self.sphero_class.collision_detection_callback is not None:
+ self.notificationPacket = [] # need to clear packet, in case new notification comes in during callback
+ self.sphero_class.collision_detection_callback()
+
+ # Sensor response:
+ elif devid == deviceID['sensor'] and commcode == sensorCommands['sensorResponse']:
+ # Convert to binary, pad bytes with leading zeros:
+ val = ''
+ for byte in notification_payload:
+ val += format(int(bin(byte)[2:], 2), '#010b')[2:]
+
+ # Break into 32-bit chunks
+ nums = []
+ while(len(val) > 0):
+ num, val = val[:32], val[32:] # Slice off first 16 bits
+ nums.append(num)
+
+ # convert from raw bits to float:
+ nums = [self.bits_to_num(num) for num in nums]
+
+ # Set sensor values as class attributes:
+ for name, value in zip(self.sphero_class.configured_sensors, nums):
+ print("Setting sensor %s at %s.", name, str(value))
+ setattr(self.sphero_class, name, value)
+
+ # Unrecognized packet structure:
+ else:
+ self.notification_ack = "Unknown asynchronous notification" #print(self.notificationPacket)
+ print(str(self.notificationPacket) + " ===================> Unknown async packet")
+
+ self.notificationPacket = [] # Start new payload after this byte
\ No newline at end of file
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/dynamicPathPlanning.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/dynamicPathPlanning.py
new file mode 100644
index 0000000000000000000000000000000000000000..6607c85a415e48eac5483d8ecc329954fbf76051
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/dynamicPathPlanning.py
@@ -0,0 +1,504 @@
+import numpy as np
+import matplotlib.pyplot as plt
+import robotController as rc
+import mapGeneration as mg
+import random
+from controller import Robot
+
+#======[Scenario Control functions]============================================
+
+#This function causes the robot to wait some time steps before starting it's operation
+#DO NOT CHANGE THIS FUNCTION OR IT'S CALL IN THE CODE
+def waitRandomTimeSteps(turtle, stop=50):
+ initialPose = np.zeros(7)
+ initialPose[0:2] = findRobotPosition(turtle)
+ turtle.setPoseGoal(initialPose)
+ turtle.drive_goalPose()
+ nRand = random.randint(1, stop)
+ for i in range(nRand):
+ turtle.robot.step(turtle.TIME_STEP)
+ turtle.update()
+
+#=========[Example functions for finding objects in the map]===================
+
+def findTargetPosition(turtlebot):
+ mapInd = turtlebot.findInMap_FinalTarget()
+ if(len(mapInd) != 0):
+ pos = turtlebot.getPositionFromMapIndex(mapInd[0])
+ else:
+ pos = []
+ return np.array(pos, dtype=float).tolist()
+
+def findRobotPosition(turtlebot):
+ mapInds = turtlebot.findInMap_Robot()
+ robotPos = np.zeros((len(mapInds), 2))
+ for i in range(len(mapInds)):
+ pos = turtlebot.getPositionFromMapIndex(mapInds[i])
+ robotPos[i, ...] = pos
+ XYInd = np.average(robotPos, axis=0)
+ return np.array(XYInd, dtype=float).tolist()
+
+def findObstaclePositions(turtlebot):
+ mapInds = turtlebot.findInMap_Obstacles()
+ obsPos = np.zeros((len(mapInds), 2))
+ for i in range(len(mapInds)):
+ pos = turtlebot.getPositionFromMapIndex(mapInds[i])
+ obsPos[i, ...] = pos
+ return np.array(obsPos, dtype=float).tolist()
+
+#=========[Framework for own implementations of path tracking and planning]====
+
+def checkForPossibleCollisions(turtlebot, turtlePos, collisionCntr):
+ #Fill this with your collision detection between the planed path and
+ #moving obstacles
+ hitBox = 0
+
+ faktor = 0.1
+
+ turtlePos1 = [turtlePos[0] + faktor, turtlePos[1] + faktor]
+ turtlePos2 = [turtlePos[0] + faktor, turtlePos[1] - faktor]
+ turtlePos3 = [turtlePos[0] - faktor, turtlePos[1] + faktor]
+ turtlePos4 = [turtlePos[0] - faktor, turtlePos[1] - faktor]
+
+ turtlePos5 = [turtlePos[0] + faktor, turtlePos[1] ]
+ turtlePos6 = [turtlePos[0] , turtlePos[1] + faktor]
+ turtlePos7 = [turtlePos[0] - faktor, turtlePos[1] ]
+ turtlePos8 = [turtlePos[0] , turtlePos[1] - faktor]
+
+ turtlePosBool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos))
+ turtlePos1Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos1))
+ turtlePos2Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos2))
+ turtlePos3Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos3))
+ turtlePos4Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos4))
+ turtlePos5Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos5))
+ turtlePos6Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos6))
+ turtlePos7Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos7))
+ turtlePos8Bool = bool(turtlebot.isMapAtPosition_Obstacle(turtlePos8))
+
+ hitBox = turtlePosBool | turtlePos1Bool | turtlePos2Bool | turtlePos3Bool | turtlePos4Bool | turtlePos5Bool | turtlePos6Bool | turtlePos7Bool | turtlePos8Bool
+
+ if(hitBox):
+ collisionCntr += 1
+ return True
+
+ if(~hitBox):
+ return False
+
+def staticPathTracking(turtlebot, path, indCntr):
+ #This delta value determines the precision of the position control during path tracking
+ #A high value leads towards a larger curve radius at corners in the path,
+ # which might result in a smoothing of the path and keeping the velocity high.
+ #A lower value sets a higher precision for the precision control and thus,
+ # the robot might slow down when reaching a path point, but it stays very
+ # close to the original path
+ delta = 0.05
+ if(indCntr == (len(path)-1)):
+ delta = 0.01
+ #Control the turtlebot to drive towards the target position
+ isNearGoal = turtlebot.drive_goalPose(delta=delta)
+ #If the robot is close to the goal select the next goal position
+ if (isNearGoal):
+ indCntr += 1
+ i = min(indCntr, len(path))
+ if i == len(path):
+ indCntr = -1
+ else:
+ #print("Aktuelle Position Pfad: ", path[i])
+ turtlebot.setPoseGoal(path[i])
+ return indCntr
+
+def planInitialPath(turtle):
+
+ initialObstacles = findObstaclePositions(turtle)
+ initialPose = findRobotPosition(turtle)
+ initialFinalTarget = findTargetPosition(turtle)
+
+ plt.scatter(np.array(initialObstacles)[..., 0], np.array(initialObstacles)[..., 1])
+ plt.scatter(np.array(initialPose)[..., 0], np.array(initialPose)[..., 1] )
+ plt.scatter(np.array(initialFinalTarget)[..., 0], np.array(initialFinalTarget)[..., 1])
+ plt.show()
+ plt.pause(0.001)
+
+ from matplotlib import pyplot as ppl
+ from matplotlib import cm
+ import random, sys, math, os.path
+ from matplotlib.pyplot import imread
+
+ #Implementation of RRT
+
+ MAP_IMG = './karte.png' #Black and white image for a map
+ MIN_NUM_VERT = 20 # Minimum number of vertex in the graph
+ MAX_NUM_VERT = 1500 # Maximum number of vertex in the graph
+ STEP_DISTANCE = 20 # Maximum distance between two vertex
+ SEED = None # For random numbers
+
+ def rapidlyExploringRandomTree(ax, img, start, goal, seed=None):
+ hundreds = 100
+ seed = random.seed(seed)
+ #print("Zufallsseed: ", seed)
+ points = []
+ graph = []
+ points.append(start)
+ graph.append((start, []))
+ print('Generating and conecting random points')
+ occupied = True
+ phaseTwo = False
+
+ # Phase two values (points 5 step distances around the goal point)
+ minX = max(goal[0] - 5 * STEP_DISTANCE, 0)
+ maxX = min(goal[0] + 5 * STEP_DISTANCE, len(img[0]) - 1)
+ minY = max(goal[1] - 5 * STEP_DISTANCE, 0)
+ maxY = min(goal[1] + 5 * STEP_DISTANCE, len(img) - 1)
+
+ i = 0
+ while (goal not in points) and (len(points) < MAX_NUM_VERT):
+ if (i % 100) == 0:
+ print(i, 'points randomly generated')
+
+ if (len(points) % hundreds) == 0:
+ print(len(points), 'vertex generated')
+ hundreds = hundreds + 100
+
+ while (occupied):
+ if phaseTwo and (random.random() > 0.8):
+ point = [random.randint(minX, maxX), random.randint(minY, maxY)]
+ else:
+ point = [random.randint(0, len(img[0]) - 1), random.randint(0, len(img) - 1)]
+
+ if (img[point[1]][point[0]][0] * 255 == 255):
+ occupied = False
+
+ occupied = True
+
+ nearest = findNearestPoint(points, point)
+ newPoints = connectPoints(point, nearest, img)
+ addToGraph(ax, graph, newPoints, point)
+ newPoints.pop(0) # The first element is already in the points list
+ points.extend(newPoints)
+ ppl.draw()
+ i = i + 1
+
+ if len(points) >= MIN_NUM_VERT:
+ if not phaseTwo:
+ print('Phase Two')
+ phaseTwo = True
+
+ if phaseTwo:
+ nearest = findNearestPoint(points, goal)
+ newPoints = connectPoints(goal, nearest, img)
+ addToGraph(ax, graph, newPoints, goal)
+ newPoints.pop(0)
+ points.extend(newPoints)
+ ppl.draw()
+
+ if goal in points:
+ print('Goal found, total vertex in graph:', len(points), 'total random points generated:', i)
+
+ path = searchPath(graph, start, [start])
+
+ for i in range(len(path) - 1):
+ ax.plot([path[i][0], path[i + 1][0]], [path[i][1], path[i + 1][1]], color='g', linestyle='-',
+ linewidth=2)
+ ppl.draw()
+
+ print('Showing resulting map')
+ print('Final path:', path)
+ print('The final path is made from:', len(path), 'connected points')
+ else:
+ path = None
+ print('Reached maximum number of vertex and goal was not found')
+ print('Total vertex in graph:', len(points), 'total random points generated:', i)
+ print('Showing resulting map')
+
+ ppl.show()
+ return path
+
+ def searchPath(graph, point, path):
+ for i in graph:
+ if point == i[0]:
+ p = i
+
+ if p[0] == graph[-1][0]:
+ return path
+
+ for link in p[1]:
+ path.append(link)
+ finalPath = searchPath(graph, link, path)
+
+ if finalPath != None:
+ return finalPath
+ else:
+ path.pop()
+
+ def addToGraph(ax, graph, newPoints, point):
+ if len(newPoints) > 1: # If there is anything to add to the graph
+ for p in range(len(newPoints) - 1):
+ nearest = [nearest for nearest in graph if (nearest[0] == [newPoints[p][0], newPoints[p][1]])]
+ nearest[0][1].append(newPoints[p + 1])
+ graph.append((newPoints[p + 1], []))
+
+ if not p == 0:
+ ax.plot(newPoints[p][0], newPoints[p][1], '+k') # First point is already painted
+ ax.plot([newPoints[p][0], newPoints[p + 1][0]], [newPoints[p][1], newPoints[p + 1][1]], color='k',
+ linestyle='-', linewidth=1)
+
+ if point in newPoints:
+ ax.plot(point[0], point[1], '.g') # Last point is green
+ else:
+ ax.plot(newPoints[p + 1][0], newPoints[p + 1][1], '+k') # Last point is not green
+
+ def connectPoints(a, b, img):
+ newPoints = []
+ newPoints.append([b[0], b[1]])
+ step = [(a[0] - b[0]) / float(STEP_DISTANCE), (a[1] - b[1]) / float(STEP_DISTANCE)]
+
+ # Set small steps to check for walls
+ pointsNeeded = int(math.floor(max(math.fabs(step[0]), math.fabs(step[1]))))
+
+ if math.fabs(step[0]) > math.fabs(step[1]):
+ if step[0] >= 0:
+ step = [1, step[1] / math.fabs(step[0])]
+ else:
+ step = [-1, step[1] / math.fabs(step[0])]
+
+ else:
+ if step[1] >= 0:
+ step = [step[0] / math.fabs(step[1]), 1]
+ else:
+ step = [step[0] / math.fabs(step[1]), -1]
+
+ blocked = False
+ for i in range(pointsNeeded + 1): # Creates points between graph and solitary point
+ for j in range(STEP_DISTANCE): # Check if there are walls between points
+ coordX = round(newPoints[i][0] + step[0] * j)
+ coordY = round(newPoints[i][1] + step[1] * j)
+
+ if coordX == a[0] and coordY == a[1]:
+ break
+ if coordY >= len(img) or coordX >= len(img[0]):
+ break
+ if img[int(coordY)][int(coordX)][0] * 255 < 255:
+ blocked = True
+ if blocked:
+ break
+
+ if blocked:
+ break
+ if not (coordX == a[0] and coordY == a[1]):
+ newPoints.append(
+ [newPoints[i][0] + (step[0] * STEP_DISTANCE), newPoints[i][1] + (step[1] * STEP_DISTANCE)])
+
+ if not blocked:
+ newPoints.append([a[0], a[1]])
+ return newPoints
+
+ def findNearestPoint(points, point):
+ best = (sys.maxsize, sys.maxsize, sys.maxsize)
+ for p in points:
+ if p == point:
+ continue
+ dist = math.sqrt((p[0] - point[0]) ** 2 + (p[1] - point[1]) ** 2)
+ if dist < best[2]:
+ best = (p[0], p[1], dist)
+ return (best[0], best[1])
+
+ karte = np.ones([80, 80, 3], dtype=int)
+
+ # Koordinatentransformation
+ initialObstacles = [[int(x[0] * 10), int(x[1] * 10)] for x in initialObstacles]
+ initialPose = [initialPose[0] * 10, initialPose[1] * 10]
+ initialFinalTarget = [initialFinalTarget[0] * 10, initialFinalTarget[1] * 10]
+
+ initialObstacles = [[int(x[0] + 40), int(x[1] - 40)] for x in initialObstacles]
+ initialPose = [initialPose[0] + 40, initialPose[1] - 40]
+ initialFinalTarget = [initialFinalTarget[0] + 40, initialFinalTarget[1] - 40]
+
+ initialObstacles = [[int(x[0]), int(x[1] * (-1))] for x in initialObstacles]
+ initialPose = [int(initialPose[0]), int(initialPose[1] * -1)]
+ initialFinalTarget = [int(initialFinalTarget[0]), int(initialFinalTarget[1] * -1)]
+
+ # Karte Hindernisse einzeichnen
+ rot = np.array(0, dtype=int)
+ gruen = np.array(0, dtype=int)
+ blau = np.array(0, dtype=int)
+
+ for k in initialObstacles:
+ karte[k[1], k[0], 0] = rot;
+ karte[k[1], k[0], 1] = gruen;
+ karte[k[1], k[0], 2] = blau;
+
+ karte[k[1] + 1, k[0] + 0, 0] = rot;
+ karte[k[1] + 1, k[0] + 0, 1] = gruen;
+ karte[k[1] + 1, k[0] + 0, 2] = blau;
+
+ karte[k[1] - 0, k[0] + 1, 0] = rot;
+ karte[k[1] - 0, k[0] + 1, 1] = gruen;
+ karte[k[1] - 0, k[0] + 1, 2] = blau;
+
+ karte[k[1] + 0, k[0] - 1, 0] = rot;
+ karte[k[1] + 0, k[0] - 1, 1] = gruen;
+ karte[k[1] + 0, k[0] - 1, 2] = blau;
+
+ karte[k[1] - 1, k[0] - 0, 0] = rot;
+ karte[k[1] - 1, k[0] - 0, 1] = gruen;
+ karte[k[1] - 1, k[0] - 0, 2] = blau;
+
+ #Plotten als PNG speichern
+ from PIL import Image
+ im = Image.fromarray((karte * 255).astype(np.uint8))
+ im.save('karte.png')
+
+ print('Loading map... with file \'', MAP_IMG, '\'')
+ img = imread(MAP_IMG)
+ fig = ppl.gcf()
+ fig.clf()
+ ax = fig.add_subplot(1, 1, 1)
+ ax.imshow(img, cmap=cm.Greys_r)
+ ax.axis('image')
+ ppl.draw()
+ print('Map is', len(img[0]), 'x', len(img))
+ start = initialPose
+ goal = initialFinalTarget
+
+ path = rapidlyExploringRandomTree(ax, img, start, goal, seed=SEED)
+
+ # Ruecktransformation
+ path1 = [[x[0], x[1] * (-1)] for x in path]
+ path2 = [[(x[0] - 40), (x[1] + 40)] for x in path1]
+ path3 = [[(x[0] / 10), (x[1] / 10)] for x in path2]
+
+ path3 = [[(x[0]), x[1], 0, 0, 0, 0, 0] for x in path3]
+
+ print("Umgerechneter Pfad", path3)
+
+ if len(sys.argv) > 2:
+ print('Only one argument is needed')
+ elif len(sys.argv) > 1:
+ if os.path.isfile(sys.argv[1]):
+ MAP_IMG = sys.argv[1]
+ else:
+ print(sys.argv[1], 'is not a file')
+
+ pathIndCntr = 0
+
+ return path3, pathIndCntr
+
+#=============[Evaluation functions]===========================================
+collisionCntr = 0
+recalcCntr = 0
+def updateEval_RobotCollisions(detector):
+ global collisionCntr
+ collision = np.nan_to_num(detector.getValue(), nan=0)
+ collisionCntr += int(collision)
+ return bool(collision)
+
+#======================================[MAIN Execution]========================
+if __name__ == "__main__":
+ plotting = False
+ nPlot = 80
+ festgefahren = 0
+ #Only increase this value if the simulation is running slowly
+ TIME_STEP = 64
+
+ #%%=============[DO NOT CHANGE THIS PART OF THE SCRIPT]==================
+ loopCntr = 0 #
+ turtle = rc.turtlebotController(TIME_STEP,
+ poseGoal_tcs=np.array([-0.863, -2.0, 0.0, 0.0, 0.0, 1.0, 3.14159])) #
+ detector = turtle.robot.getDevice("collisionDetector") #
+ detector.enable(TIME_STEP) #
+ #
+ #The map will be initialized after the first timestep (Transmission) #
+ turtle.robot.step(turtle.TIME_STEP) #
+ turtle.update() #
+
+ #The map will have the dynamic objects after the second timestep #
+ turtle.robot.step(turtle.TIME_STEP) #
+ turtle.update() #
+ ScenarioFinalTarget = findTargetPosition(turtle) #
+ #
+ if plotting:
+ mg.plotting_GM_plotly(turtle.mapGenerator)
+ #DO NOT CHANGE OR REMOVE THIS FUNCTION CALL AND POSITION #
+ waitRandomTimeSteps(turtle) #
+ #===============[From here you can change the script]====================
+ #%%
+ #here you should insert a function, which plans a path; the functions has to return a path and related index
+ try:
+ path, pathIndCntr = planInitialPath(turtle)
+ except TypeError:
+ print("Erste Pfadplanung nicht erfolgreich, neustart...")
+ try:
+ path, pathIndCntr = planInitialPath(turtle)
+ except TypeError:
+ print("Zweite Pfadplanung nicht erfolgreich, neustart...")
+ try:
+ path, pathIndCntr = planInitialPath(turtle)
+ except TypeError:
+ print("Dritte Pfadplanung nicht erfolgreich, neustart...")
+ exit()
+
+ turtle.setPoseGoal(path[pathIndCntr])
+ startTime = turtle.robot.getTime()
+
+ inhibitCntr = 0
+ while turtle.robot.step(turtle.TIME_STEP) != -1:
+ #%%===[DO NOT CHANGE THIS PART OF THE LOOP]==========================
+ loopCntr += 1 #
+ turtle.update()
+
+ collision = checkForPossibleCollisions(turtle, findRobotPosition(turtle), collisionCntr)
+
+ if ( (plotting) & (loopCntr % nPlot == 0)): #
+ mg.plotting_GM_plotly(turtle.mapGenerator) #
+ #====[From here you can change the loop]=============================
+ #%%
+ #Follow the static path towards the index pathIndCntr in the path
+ inhibitCntr = max(0, inhibitCntr - 1)
+
+ if(collision == False):
+ pathIndCntr = staticPathTracking(turtle, path, pathIndCntr)
+
+ if(collision == True):
+ print("Knallt")
+ turtle.stop()
+ festgefahren += 1
+
+ if(festgefahren == 10):
+ #Berechnen neue Pos
+ vorherigePos = path[pathIndCntr-1]
+ neueRichtung = [vorherigePos[0] / 2, vorherigePos[1] / 2, 0, 0, 0, 0, 0]
+ path.insert(pathIndCntr, neueRichtung)
+
+ #Fahre neue Pos an bis du da bist
+ #Wenn du da bist festgefahren 0 und hoffentlich keine Collsion mehr
+ recalcCntr += 1
+ inhibitCntr = 15
+ festgefahren = 0
+ collision = False
+
+ #This is the exit condition. Changing this is not recommended
+ if(pathIndCntr == -1):
+ break
+ #End while
+ #%%
+ print("Simulation ended")
+ print("Timesteps with collisions: ", collisionCntr)
+ print("Recalculations done: ", recalcCntr)
+ print("Simulationtime: ", turtle.robot.getTime() - startTime)
+ print("")
+ print("Recalculation")
+ print("Timesteps with collisions: ", collisionCntr)
+ print("Recalculations done: ", recalcCntr)
+ print("Simulationtime: ", turtle.robot.getTime() - startTime)
+
+ lastStateObstacles = np.array(findObstaclePositions(turtle))
+ lastStateRobotPose = np.array(findRobotPosition(turtle))
+
+ trackRobot = np.array(turtle.trackRobot)
+ plt.scatter(lastStateObstacles[..., 0], lastStateObstacles[..., 1])
+ plt.scatter(lastStateRobotPose[..., 0], lastStateRobotPose[..., 1])
+
+ plt.plot(trackRobot[..., 0], trackRobot[..., 1])
+ plt.scatter(turtle.poseGoal[0], turtle.poseGoal[1])
+ plt.scatter(turtle.mapGenerator.finalTarget[0], turtle.mapGenerator.finalTarget[1])
+ plt.show()
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/map.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/map.py
new file mode 100644
index 0000000000000000000000000000000000000000..379645de42c7898a6d3e846c21e3c8bf996ac70a
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/map.py
@@ -0,0 +1,24 @@
+#Map erstellen
+
+# Standard imports
+import cv2
+import numpy as np;
+
+# Img Objekt
+cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+img = cap.read()
+#height, width = img.shape[:2]
+
+success, img = cap.read()
+gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
+mask = cv2.inRange(gray, np.array([90]), np.array([255]))
+
+mask[mask == 255] = 87
+
+mask = np.array(mask, dtype=np.uint8).view('S2').squeeze()
+
+mask[mask == b'WW'] = 'W'
+
+print(mask)
+
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/maperstellen.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/maperstellen.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node copy.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node copy.py
new file mode 100644
index 0000000000000000000000000000000000000000..29f2473b20a9c782704e15851518da8ec90ee823
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node copy.py
@@ -0,0 +1,183 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import json
+from sensor_msgs.msg import Imu
+import threading
+from std_msgs.msg import String
+from geometry_msgs.msg import Quaternion, Vector3
+from box import Box
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import datetime
+
+
+class MyNode(Node):
+
+ def __init__(self):
+ super().__init__("sphero_mini")
+ self.publisher_ = self.create_publisher(String,"Imu",5)
+ #self.get_logger().info("Hello from ROS2")
+
+ def connect(self):
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ #print(i)
+
+ #self.publisher_.publish(i)
+
+
+
+def main(args = None):
+
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ rate = node.create_rate(2)
+
+ try:
+ while rclpy.ok():
+
+ sphero.configureSensorMask(
+ IMU_yaw=True,
+ IMU_pitch=True,
+ IMU_roll=True,
+ IMU_acc_y=True,
+ IMU_acc_z=True,
+ IMU_acc_x=True,
+ IMU_gyro_x=True,
+ IMU_gyro_y=True,
+ #IMU_gyro_z=True
+ )
+ sphero.configureSensorStream()
+
+ sensors_values = node.get_sensors_data(sphero)
+ #rclpy.logdebug(sensors_values)
+ node.publish_imu(sensors_values, node)
+ sphero.setLEDColor(red = 0, green = 255, blue = 0) # Turn LEDs green
+
+ # Aiming:
+ sphero.setLEDColor(red = 0, green = 0, blue = 0) # Turn main LED off
+ sphero.stabilization(False) # Turn off stabilization
+ sphero.setBackLEDIntensity(255) # turn back LED on
+ sphero.wait(3) # Non-blocking pau`se
+ sphero.resetHeading() # Reset heading
+ sphero.stabilization(True) # Turn on stabilization
+ sphero.setBackLEDIntensity(0) # Turn back LED off
+
+ # Move around:
+ sphero.setLEDColor(red = 0, green = 0, blue = 255) # Turn main LED blue
+ sphero.roll(100, 0) # roll forwards (heading = 0) at speed = 50
+
+ sphero.wait(3) # Keep rolling for three seconds
+
+ sphero.roll(0, 0) # stop
+ sphero.wait(1) # Allow time to stop
+
+ sphero.setLEDColor(red = 0, green = 255, blue = 0) # Turn main LED green
+ sphero.roll(-100, 0) # Keep facing forwards but roll backwards at speed = 50
+ sphero.wait(3) # Keep rolling for three seconds
+
+ sphero.roll(0, 0) # stop
+ sphero.wait(1) # Allow time to stop
+
+ rate.sleep()
+
+ except KeyboardInterrupt:
+ pass
+
+ rclpy.shutdown()
+ thread.join()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node.py
new file mode 100644
index 0000000000000000000000000000000000000000..8906b23876454b93bf060523bda262e61363cdd2
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node.py
@@ -0,0 +1,766 @@
+#!/usr/bin/env python3
+############################################################################
+#%% Imports
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+import threading
+from geometry_msgs.msg import Quaternion, Vector3
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import cv2
+import time
+import queue
+import matplotlib.pyplot as plt
+############################################################################
+#%% Blob Detector / Globale Variablen
+detector = cv2.SimpleBlobDetector()
+
+# Setup SimpleBlobDetector parameters.
+params = cv2.SimpleBlobDetector_Params()
+
+# Change thresholds
+#params.minThreshold = 5
+#params.maxThreshold = 500
+
+#FIlter by Color
+params.filterByColor = True
+params.blobColor = 255
+
+# Filter by Area.
+params.filterByArea = True
+params.minArea = 10
+params.maxArea = 200
+
+# Filter by Circularity
+#params.filterByCircularity = True
+#params.minCircularity = 0.5
+#params.maxCircularity = 1
+
+# Filter by Convexity
+#params.filterByConvexity = True
+#params.minConvexity = 0.7
+#params.maxConvexity = 1
+
+# Filter by Inertia
+#params.filterByInertia = True
+#params.minInertiaRatio = 0.01
+
+# Create a detector with the parameters
+detector = cv2.SimpleBlobDetector_create(params)
+############################################################################
+#%% Class Video Capture
+# Bufferless VideoCapture
+# Quelle ?
+class VideoCapture:
+ def __init__(self, name):
+ self.cap = cv2.VideoCapture(name)
+ self.q = queue.Queue()
+ t = threading.Thread(target=self._reader)
+ t.daemon = True
+ t.start()
+ # read frames as soon as they are available, keeping only most recent one
+ def _reader(self):
+ while True:
+ ret, frame = self.cap.read()
+ #cv2.imshow("Cap", frame)
+
+ if not ret:
+ break
+ if not self.q.empty():
+ try:
+ self.q.get_nowait() # discard previous (unprocessed) frame
+ except queue.Empty:
+ pass
+ self.q.put(frame)
+
+ def read(self):
+ return self.q.get()
+############################################################################
+#%% Class Sphero Node
+class MyNode(Node):
+ def __init__(self):
+ super().__init__("sphero_mini")
+
+ def connect(self):
+ #Automatisch MAC-Adresse laden
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ def get_pos(self, cap, height):
+ #zeitanfang = time.time()
+
+ success = False
+
+ while(success == False):
+ try:
+ img = cap.read()
+
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+ keypoints = detector.detect(gray)
+
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+ success = True
+
+ xTrans, yTrans = self.calc_trans(x,y, height)
+ except Exception:
+ continue
+
+ #print("Blob X: %f Blob Y: %f" %(x,y))
+ #zeitende = time.time()
+ #print("Dauer Programmausführung:",(zeitende-zeitanfang))
+ #print("Get_Pos: X,Y:", xTrans,yTrans)
+
+ return xTrans,yTrans
+
+ def calc_offset(self, sphero, cap, height):
+ sphero.roll(100,0)
+ sphero.wait(1)
+ sphero.roll(0,0)
+ sphero.wait(0.5)
+
+ ref = self.get_pos(cap, height)
+ print("calc_offset: Ref Punkt x,y", ref)
+
+ sphero.wait(1)
+ sphero.roll(100,180)
+ sphero.wait(1)
+ sphero.roll(0,0)
+
+ startpunkt = self.get_pos(cap, height)
+ print("calc_offset: Startpunkt x,y", startpunkt)
+
+ ref = np.array(ref)
+ startpunkt = np.array(startpunkt)
+
+ start_ref = ref-startpunkt
+
+ phi = np.arctan2(start_ref[1],start_ref[0])
+ phi = np.degrees(phi)
+ phi = int(-phi)
+
+ print("Phi ohne alles:", phi)
+
+ phi = self.phi_in_range(phi)
+
+ print("Calc_Offset: ", phi)
+
+ return phi
+
+ def calc_av(self,istPos, sollPos, cap, height):
+ startPos = istPos
+
+ startPos = np.array(startPos)
+ sollPos = np.array(sollPos)
+
+ pktSpheroKord = sollPos - startPos
+
+ phi = np.arctan2(pktSpheroKord[1], pktSpheroKord[0])
+ phi = np.degrees(phi)
+
+ phiZiel = int(phi)
+
+ phiZiel = self.phi_in_range(phiZiel)
+
+ v = 100
+
+ #print("Calc_AV: a,v", phiZiel, v)
+
+ return phiZiel, v
+
+ def drive_to(self,sphero, targetPos, aOffset, cap, height):
+ dmin = 20
+ pos = self.get_pos(cap, height)
+ pos = np.array(pos)
+
+ diff = targetPos - pos
+
+ d = np.linalg.norm(diff, ord = 2)
+
+ ar = []
+ ar.append(0)
+
+ i = 1
+
+ while d > dmin:
+ a,v = self.calc_av(pos,targetPos, cap, height)
+
+ aR = -aOffset - a #Fallunterscheidung?
+ ar.append((self.phi_in_range(aR)))
+
+ #Fahrbefehl
+ if(ar[i] != ar[i-1]):
+ sphero.roll(v, ar[i])
+ sphero.wait(0.05)
+ else:
+ sphero.wait(0.05)
+
+ #Aktuelle Pos
+ pos = self.get_pos(cap, height)
+ pos = np.array(pos)
+
+ #Abweichung
+ diff = targetPos - pos
+ diff = np.array(diff)
+ d = np.linalg.norm(diff, ord = 2)
+
+ i = i + 1
+
+ sphero.roll(0,0)
+ print("Ziel Erreicht")
+
+ return
+
+ def calc_trans(self,x,y, height):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+
+ def phi_in_range(self,phi):
+ while(phi < 0):
+ phi = phi + 360
+ while(phi > 360):
+ phi = phi - 360
+ return phi
+############################################################################
+#%% Class Wavefrontplanner
+class waveFrontPlanner:
+ ############################################################################
+ # WAVEFRONT ALGORITHM
+ # Adapted to Python Code By Darin Velarde
+ # Fri Jan 29 13:56:53 PST 2010
+ # from C code from John Palmisano
+ # (www.societyofrobots.com)
+ ############################################################################
+
+ def __init__(self, mapOfWorld, slow=False):
+ self.__slow = slow
+ self.__mapOfWorld = mapOfWorld
+ if str(type(mapOfWorld)).find("numpy") != -1:
+ #If its a numpy array
+ self.__height, self.__width = self.__mapOfWorld.shape
+ else:
+ self.__height, self.__width = len(self.__mapOfWorld), len(self.__mapOfWorld[0])
+
+ self.__nothing = 000
+ self.__wall = 999
+ self.__goal = 1
+ self.__path = "PATH"
+
+ self.__finalPath = []
+
+ #Robot value
+ self.__robot = 254
+ #Robot default Location
+ self.__robot_x = 0
+ self.__robot_y = 0
+
+ #default goal location
+ self.__goal_x = 8
+ self.__goal_y = 9
+
+ #temp variables
+ self.__temp_A = 0
+ self.__temp_B = 0
+ self.__counter = 0
+ self.__steps = 0 #determine how processor intensive the algorithm was
+
+ #when searching for a node with a lower value
+ self.__minimum_node = 250
+ self.__min_node_location = 250
+ self.__new_state = 1
+ self.__old_state = 1
+ self.__reset_min = 250 #above this number is a special (wall or robot)
+ ###########################################################################
+
+ def setRobotPosition(self, x, y):
+ """
+ Sets the robot's current position
+
+ """
+
+ self.__robot_x = x
+ self.__robot_y = y
+ ###########################################################################
+
+ def setGoalPosition(self, x, y):
+ """
+ Sets the goal position.
+
+ """
+
+ self.__goal_x = x
+ self.__goal_y = y
+ ###########################################################################
+
+ def robotPosition(self):
+ return (self.__robot_x, self.__robot_y)
+ ###########################################################################
+
+ def goalPosition(self):
+ return (self.__goal_x, self.__goal_y)
+ ###########################################################################
+
+ def run(self, prnt=False):
+ """
+ The entry point for the robot algorithm to use wavefront propagation.
+
+ """
+
+ path = []
+ while self.__mapOfWorld[self.__robot_x][self.__robot_y] != self.__goal:
+ if self.__steps > 10000:
+ print ("Cannot find a path.")
+ return
+ #find new location to go to
+ self.__new_state = self.propagateWavefront()
+ #update location of robot
+ if self.__new_state == 1:
+ self.__robot_x -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" % (self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 2:
+ self.__robot_y += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 3:
+ self.__robot_x += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 4:
+ self.__robot_y -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ self.__old_state = self.__new_state
+ msg = "Found the goal in %i steps:\n" % self.__steps
+ msg += "mapOfWorld size= %i %i\n\n" % (self.__height, self.__width)
+ if prnt:
+ print(msg)
+ self.printMap()
+ return path
+ ###########################################################################
+
+ def propagateWavefront(self, prnt=False):
+ self.unpropagate()
+ #Old robot location was deleted, store new robot location in mapOfWorld
+ self.__mapOfWorld[self.__robot_x][self.__robot_y] = self.__robot
+ self.__path = self.__robot
+ #start location to begin scan at goal location
+ self.__mapOfWorld[self.__goal_x][self.__goal_y] = self.__goal
+ counter = 0
+ while counter < 200: #allows for recycling until robot is found
+ x = 0
+ y = 0
+ #time.sleep(0.00001)
+ #while the mapOfWorld hasnt been fully scanned
+ while x < self.__height and y < self.__width:
+ #if this location is a wall or the goal, just ignore it
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal:
+ #a full trail to the robot has been located, finished!
+ minLoc = self.minSurroundingNodeValue(x, y)
+ if minLoc < self.__reset_min and \
+ self.__mapOfWorld[x][y] == self.__robot:
+ if prnt:
+ print("Finished Wavefront:\n")
+ self.printMap()
+ # Tell the robot to move after this return.
+ return self.__min_node_location
+ #record a value in to this node
+ elif self.__minimum_node != self.__reset_min:
+ #if this isnt here, 'nothing' will go in the location
+ self.__mapOfWorld[x][y] = self.__minimum_node + 1
+ #go to next node and/or row
+ y += 1
+ if y == self.__width and x != self.__height:
+ x += 1
+ y = 0
+ #print self.__robot_x, self.__robot_y
+ if prnt:
+ print("Sweep #: %i\n" % (counter + 1))
+ self.printMap()
+ self.__steps += 1
+ counter += 1
+ return 0
+ ###########################################################################
+
+ def unpropagate(self):
+ """
+ clears old path to determine new path
+ stay within boundary
+
+ """
+
+ for x in range(0, self.__height):
+ for y in range(0, self.__width):
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal and \
+ self.__mapOfWorld[x][y] != self.__path:
+ #if this location is a wall or goal, just ignore it
+ self.__mapOfWorld[x][y] = self.__nothing #clear that space
+ ###########################################################################
+
+ def minSurroundingNodeValue(self, x, y):
+ """
+ this method looks at a node and returns the lowest value around that
+ node.
+
+ """
+
+ #reset minimum
+ self.__minimum_node = self.__reset_min
+ #down
+ if x < self.__height -1:
+ if self.__mapOfWorld[x + 1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x + 1][y] != self.__nothing:
+ #find the lowest number node, and exclude empty nodes (0's)
+ self.__minimum_node = self.__mapOfWorld[x + 1][y]
+ self.__min_node_location = 3
+ #up
+ if x > 0:
+ if self.__mapOfWorld[x-1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x-1][y] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x-1][y]
+ self.__min_node_location = 1
+ #right
+ if y < self.__width -1:
+ if self.__mapOfWorld[x][y + 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y + 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y + 1]
+ self.__min_node_location = 2
+ #left
+ if y > 0:
+ if self.__mapOfWorld[x][y - 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y - 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y-1]
+ self.__min_node_location = 4
+ return self.__minimum_node
+ ###########################################################################
+
+ def printMap(self):
+ """
+ Prints out the map of this instance of the class.
+
+ """
+
+ msg = ''
+ for temp_B in range(0, self.__height):
+ for temp_A in range(0, self.__width):
+ if self.__mapOfWorld[temp_B][temp_A] == self.__wall:
+ msg += "%04s" % "[#]"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__robot:
+ msg += "%04s" % "-"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__goal:
+ msg += "%04s" % "G"
+ else:
+ msg += "%04s" % str(self.__mapOfWorld[temp_B][temp_A])
+ msg += "\n\n"
+ msg += "\n\n"
+ print(msg)
+ #
+ if self.__slow == True:
+ time.sleep(0.05)
+
+ def plotMap(self,img,path):
+ #Programm Koordinaten
+ print("Plotten")
+
+ imgTrans = cv2.transpose(img) # X und Y-Achse im Bild tauschen
+ imgPlot, ax1 = plt.subplots()
+
+ ax1.set_title('Programm Koordinaten')
+ ax1.imshow(imgTrans)
+ ax1.set_xlabel('[px]')
+ ax1.set_ylabel('[px]')
+ ax1.scatter(path[:,0], path[:,1], color='r')
+
+ #Bild Koordinaten
+ imgPlot2, ax2 = plt.subplots()
+ ax2.set_title('Bild Koordinaten')
+ ax2.set_xlabel('[px]')
+ ax2.set_ylabel('[px]')
+ ax2.imshow(img)
+ ax2.scatter(path[:,1], path[:,0], color='r')
+
+ plt.show()
+
+ return
+
+ def getPathWelt(self,path,height,Mapobj):
+
+ pathWeltX, pathWeltY = Mapobj.calc_trans_welt(path[:,1], path[:,0], height)
+ pathEckenX = []
+ pathEckenY = []
+
+ for i,px in enumerate(pathWeltX):
+ if (i < len(pathWeltX)-1) & (i > 0):
+ if px != pathWeltX[i+1]:
+ if pathWeltY[i]!=pathWeltY[i-1]:
+ pathEckenX.append(px)
+ pathEckenY.append(pathWeltY[i])
+ if pathWeltY[i] != pathWeltY[i+1]:
+ if pathWeltX[i]!=pathWeltX[i-1]:
+ pathEckenX.append(px)
+ pathEckenY.append(pathWeltY[i])
+
+ pathEckenX.append(pathWeltX[-1])
+ pathEckenY.append(pathWeltY[-1])
+
+
+ uebergabePath = []
+ for i in range(0,len(pathEckenX)):
+ uebergabePath.append((pathEckenX[i],pathEckenY[i]))
+
+ print("Ecken: ", uebergabePath)
+
+ return uebergabePath
+
+############################################################################
+#%% Class Map
+class mapCreate:
+ ############################################################################
+
+ def create_map(self, scale, cap):
+
+ # Map erstellen
+
+ # Img Objekt
+ #cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+ img = cap.read()
+
+ #Karte von Bild
+ #img = cv2.imread("D:/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/map/map.jpg")
+ print('Original Dimensions : ',img.shape)
+
+ scale_percent = 100-scale # percent of original size
+ width = int(img.shape[1] * scale_percent / 100)
+ height = int(img.shape[0] * scale_percent / 100)
+ dim = (width, height)
+
+ # resize image
+ resized = cv2.resize(img, dim, interpolation = cv2.INTER_AREA)
+ print('Resized Dimensions : ',resized.shape)
+
+ gray = cv2.cvtColor(resized, cv2.COLOR_RGB2GRAY)
+ mask = cv2.inRange(gray, np.array([50]), np.array([255]))
+
+ plt.plot(mask)
+
+ mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+ mask_list= np.ndarray.tolist(mask)
+
+ #Markiere alle leeren Zellen mit 000 und alle Zellen mit Hinderniss mit 999
+ for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['999' if j > 200 else '000' for j in mask_list[i]]
+
+ mapOfWorld = np.array(mapOfWorld, dtype = int)
+ mapOfWorldSized = mapOfWorld.copy()
+
+ e = 1
+ #Hindernisse Größer machen
+ for i in range(0,mapOfWorld.shape[0]):
+ for j in range(0,mapOfWorld.shape[1]):
+ for r in range(0,e):
+ try:
+ if (mapOfWorldSized[i][j] == 999):
+ mapOfWorld[i][j+e] = 999
+ mapOfWorld[i+e][j] = 999
+ mapOfWorld[i-e][j] = 999
+ mapOfWorld[i][j-e] = 999
+ mapOfWorld[i+e][j+e] = 999
+ mapOfWorld[i+e][j-e] = 999
+ mapOfWorld[i-e][j+e] = 999
+ mapOfWorld[i-e][j-e] = 999
+ except Exception:
+ continue
+
+ return mapOfWorld,img
+ ############################################################################
+
+ def scale_koord(self, sx, sy, gx, gy,scale):
+ scale_percent = 100-scale # percent of original size
+
+ sx = int(sx*scale_percent/100)
+ sy = int(sy*scale_percent/100)
+ gx = int(gx*scale_percent/100)
+ gy = int(gy*scale_percent/100)
+
+ return sx,sy,gx,gy
+ ############################################################################
+
+ def rescale_koord(self, path,scale):
+ scale_percent = 100-scale # percent of original size100
+
+ path = np.array(path)
+
+ for i in range(len(path)):
+ path[i] = path[i]*100/scale_percent
+
+ return path
+ ############################################################################
+
+ def calc_trans_welt(self, x, y, height):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+###############################################################################
+#%% Main Funktion
+def main(args = None):
+ #Verbindung herstellen, Node erstellen
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ #rate = node.create_rate(5)
+
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ #cap = VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+ cap = VideoCapture(4)
+ Map = mapCreate()
+
+ img = cap.read()
+ height, width = img.shape[:2]
+
+ scale = 90
+
+ sphero.setLEDColor(255,0,0)
+ sphero.wait(1)
+ sphero.setBackLEDIntensity(0)
+ sphero.stabilization(True)
+
+ aOffset = node.calc_offset(sphero, cap, height)
+
+ sphero.wait(1)
+
+ #while(True):
+
+ mapWorld,img = Map.create_map(scale, cap)
+ height, width = img.shape[:2]
+
+ #Befehle für WaveFrontPlanner/Maperstellung
+ sy,sx = node.get_pos(cap,height) #Aktuelle Roboter Pos in Welt
+ sy, sx = node.calc_trans(sy,sx,height)
+
+ gy = int(input("Bitte geben Sie die X-Zielkoordinate im Bild Koordinatensystem ein:")) #X-Koordinate im Weltkoordinaten System
+ gx = int(input("Bitte geben Sie die Y-Zielkoordinate im Bild Koordinatensystem ein:")) #Y-Koordinate im Weltkordinaten System
+
+ sx,sy,gx,gy = Map.scale_koord(sx,sy,gx,gy,scale) #Kordinaten Tauschen X,Y
+
+ start = time.time()
+ planner = waveFrontPlanner(mapWorld)
+ planner.setGoalPosition(gx,gy)
+ planner.setRobotPosition(sx,sy)
+ path = planner.run(False)
+ end = time.time()
+ print("Took %f seconds to run wavefront simulation" % (end - start))
+
+ path = Map.rescale_koord(path, scale)
+
+ planner.plotMap(img,path)
+
+ pathWelt = planner.getPathWelt(path,height,Map)
+
+ for p in pathWelt:
+ node.drive_to(sphero,p,aOffset,cap, height)
+ sphero.wait(1)
+
+ print("Geschafft")
+
+ #if cv2.waitKey(10) & 0xFF == ord('q'):
+ # break
+
+ rclpy.shutdown()
+############################################################################
+#%% Main
+if __name__ == '__main__':
+ main()
+############################################################################
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_alt.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_alt.py
new file mode 100644
index 0000000000000000000000000000000000000000..3b4ea7911e77fbc716fc3c714888fe11f6c6d3ab
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_alt.py
@@ -0,0 +1,323 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import json
+from sensor_msgs.msg import Imu
+import threading
+from std_msgs.msg import String
+from geometry_msgs.msg import Quaternion, Vector3
+from box import Box
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import datetime
+import cv2
+import time
+import queue, threading, time
+
+# Set up the detector with default parameters.
+detector = cv2.SimpleBlobDetector()
+
+# Setup SimpleBlobDetector parameters.
+params = cv2.SimpleBlobDetector_Params()
+
+# Change thresholds
+#params.minThreshold = 5
+#params.maxThreshold = 500
+
+#FIlter by Color
+params.filterByColor = True
+params.blobColor = 255
+
+# Filter by Area.
+params.filterByArea = True
+params.minArea = 10
+params.maxArea = 200
+
+# Filter by Circularity
+#params.filterByCircularity = True
+#params.minCircularity = 0.5
+#params.maxCircularity = 1
+
+# Filter by Convexity
+#params.filterByConvexity = True
+#params.minConvexity = 0.7
+#params.maxConvexity = 1
+
+# Filter by Inertia
+#params.filterByInertia = True
+#params.minInertiaRatio = 0.01
+
+# Create a detector with the parameters
+detector = cv2.SimpleBlobDetector_create(params)
+
+# bufferless VideoCapture
+class VideoCapture:
+ def __init__(self, name):
+ self.cap = cv2.VideoCapture(name)
+ self.q = queue.Queue()
+ t = threading.Thread(target=self._reader)
+ t.daemon = True
+ t.start()
+ # read frames as soon as they are available, keeping only most recent one
+ def _reader(self):
+ while True:
+ ret, frame = self.cap.read()
+ if not ret:
+ break
+ if not self.q.empty():
+ try:
+ self.q.get_nowait() # discard previous (unprocessed) frame
+ except queue.Empty:
+ pass
+ self.q.put(frame)
+
+ def read(self):
+ return self.q.get()
+
+class MyNode(Node):
+
+ def __init__(self):
+ super().__init__("sphero_mini")
+
+ def connect(self):
+ #Automatisch MAC-Adresse laden
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ def get_pos(self, cap, height):
+ zeitanfang = time.time()
+
+ img = cap.read()
+
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+ keypoints = detector.detect(gray)
+
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+
+ #print("Blob X: %f Blob Y: %f" %(x,y))
+
+ zeitende = time.time()
+ #print("Dauer Programmausführung:",(zeitende-zeitanfang))
+
+ xTrans, yTrans = self.calc_trans(x,y, height)
+
+ #print("Get_Pos: X,Y:", xTrans,yTrans)
+
+ return xTrans,yTrans
+
+ def calc_offset(self, sphero, cap, height):
+ sphero.roll(100,0)
+ sphero.wait(2)
+ sphero.roll(0,0)
+ sphero.wait(0.5)
+
+ ref = self.get_pos(cap, height)
+ print("calc_offset: Ref Punkt x,y", ref)
+
+ sphero.wait(1)
+ sphero.roll(100,180)
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+ startpunkt = self.get_pos(cap, height)
+ print("calc_offset: Startpunkt x,y", startpunkt)
+
+ ref = np.array(ref)
+ startpunkt = np.array(startpunkt)
+
+ start_ref = ref-startpunkt
+
+ phi = np.arctan2(start_ref[1],start_ref[0])
+ phi = np.degrees(phi)
+ phi = int(phi)
+
+ print("Phi ohne alles:", phi)
+
+ if(phi < 0):
+ phi = phi + 360
+
+ if(phi > 360):
+ phi = phi - 360
+
+ print("Calc_Offset: ", phi)
+
+ return phi
+
+ def calc_av(self, sollPos, cap, height):
+
+ startPos = self.get_pos(cap, height)
+
+ startPos = np.array(startPos)
+ sollPos = np.array(sollPos)
+
+ pktSpheroKord = sollPos - startPos
+
+ phi = np.arctan2(pktSpheroKord[1], pktSpheroKord[0])
+ phi = np.degrees(phi)
+
+ phiZiel = int(phi)
+
+ if(phiZiel < 0):
+ phiZiel = phiZiel + 360
+
+ if(phi > 360):
+ phi = phi - 360
+
+ v = 100
+
+ print("Calc_AV: a,v", phiZiel, v)
+
+ return phiZiel, v
+
+ def drive_to(self,sphero, targetPos, aOffset, cap, height):
+ dmin = 50
+ pos = self.get_pos(cap, height)
+ pos = np.array(pos)
+
+ diff = targetPos - pos
+
+ d = np.linalg.norm(diff, ord = 2)
+
+ while d > dmin:
+ a,v = self.calc_av(targetPos, cap, height)
+ ar = aOffset + a #Fallunterscheidung?
+
+ pos = self.get_pos(cap, height)
+ pos = np.array(pos)
+ diff = targetPos - pos
+ diff = np.array(diff)
+ d = np.linalg.norm(diff, ord = 2)
+
+ sphero.roll(v, ar)
+ time.sleep(0.1)
+
+ sphero.roll(0,ar)
+
+ return
+
+ def calc_trans(self,x,y, height):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+
+def main(args = None):
+ #Verbindung herstellen, Node erstellen
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ rate = node.create_rate(5)
+
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ cap = VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+ img = cap.read()
+ height, width = img.shape[:2]
+
+ sphero.setLEDColor(255,0,0)
+ sphero.wait(1)
+ sphero.setBackLEDIntensity(100)
+ sphero.stabilization(True)
+
+ aOffset = node.calc_offset(sphero, cap, height)
+
+ sphero.roll(100,0+aOffset)
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+ sphero.wait(5)
+
+ targetPos = 117,430
+
+ node.drive_to(sphero,targetPos,aOffset,cap, height)
+
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
+
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_komisch.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_komisch.py
new file mode 100644
index 0000000000000000000000000000000000000000..7c5dc3e024792433ecfc8652b43a8c8b3281bc30
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_komisch.py
@@ -0,0 +1,305 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import json
+from sensor_msgs.msg import Imu
+import threading
+from std_msgs.msg import String
+from geometry_msgs.msg import Quaternion, Vector3
+from box import Box
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import datetime
+import cv2
+import time
+
+
+# Img Objekt
+cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+# Set up the detector with default parameters.
+detector = cv2.SimpleBlobDetector()
+
+# Setup SimpleBlobDetector parameters.
+params = cv2.SimpleBlobDetector_Params()
+
+# Change thresholds
+#params.minThreshold = 5
+#params.maxThreshold = 500
+
+#FIlter by Color
+params.filterByColor = True
+params.blobColor = 255
+
+# Filter by Area.
+#params.filterByArea = True
+#params.minArea = 100
+#params.maxArea = 1000
+
+# Filter by Circularity
+#params.filterByCircularity = True
+#params.minCircularity = 0.5
+#params.maxCircularity = 1
+
+# Filter by Convexity
+#params.filterByConvexity = True
+#params.minConvexity = 0.7
+#params.maxConvexity = 1
+
+# Filter by Inertia
+#params.filterByInertia = True
+#params.minInertiaRatio = 0.01
+
+# Create a detector with the parameters
+detector = cv2.SimpleBlobDetector_create(params)
+
+class MyNode(Node):
+ #Konstrukter Node Objekt
+ def __init__(self):
+ super().__init__("sphero_mini")
+ #self.publisher_ = self.create_publisher(String,"Imu",5)
+
+ def connect(self):
+ #Automatisch MAC-Adresse laden
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ def positionauslesen(self):
+ zeitanfang = time.time()
+
+ x = (-1)
+ y = (-1)
+
+ while x < 0 and y < 0:
+
+ success, img = cap.read()
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+ # Detect blobs.
+ keypoints = detector.detect(gray)
+
+ #print(keypoints)
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+
+ #print("Blob X: %f Blob Y: %f" %(x,y))
+ zeitende = time.time()
+ print("Dauer Programmausführung:",(zeitende-zeitanfang))
+ return (x,y)
+
+ def ref_winkel(self, sphero):
+
+ startpunkt = self.positionauslesen()
+
+ sphero.roll(100,0)
+ sphero.wait(2)
+ sphero.roll(0,0)
+ sphero.wait(0.5)
+
+ ref = self.positionauslesen()
+
+ sphero.wait(0.5)
+ sphero.roll(100,180)
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+ soll = (startpunkt[0]+100,startpunkt[1])
+
+ soll = np.array(soll)
+ ref = np.array(ref)
+ startpunkt = np.array(startpunkt)
+
+ start_ref = ref-startpunkt
+ start_soll = soll-startpunkt
+
+ y = start_ref[0] * start_soll[1] - start_ref[1] * start_soll[0]
+ x = start_ref[0] * start_soll[0] + start_ref[1] * start_soll[1]
+
+ phi = np.arctan2(y,x)
+ phi = np.degrees(phi)
+ phi = int(phi)
+
+ if(phi < 0 ):
+ phi = phi + 360
+
+ return phi
+
+ def fahre_ziel(self, sphero, x,y, phiAbw):
+
+ startPos = self.positionauslesen()
+ sollPos = (x,y)
+
+ startPos = np.array(startPos)
+ sollPos = np.array(sollPos)
+
+ pktSpheroKord = sollPos - startPos
+
+ phi = np.arctan2(pktSpheroKord[1], pktSpheroKord[0])
+ phi = np.degrees(phi)
+
+ phi = int(phi)
+
+ phiZiel = phiAbw - phi
+
+ if(phiZiel < 0 ):
+ phiZiel = phiZiel + 360
+
+ print("Winkel Sphero System ", phi)
+
+ #sollPos = (x,y)
+ #startPos = self.positionauslesen()
+ #refPos = (startPos[0]+100,startPos[1])
+
+ #sollPos = np.array(sollPos)
+ #refPos = np.array(refPos)
+ #startPos = np.array(startPos)
+
+ #start_ref = refPos-startPos
+ #start_soll = sollPos-startPos
+
+ #y = start_ref[0] * start_soll[1] - start_ref[1] * start_soll[0]
+ #x = start_ref[0] * start_soll[0] + start_ref[1] * start_soll[1]
+
+ #phi = np.arctan2(y,x)
+ #phi = np.degrees(phi)
+ #phiZiel = phiAbw - (int(phi) * -1)
+
+ print("Zielwinkel: ", phiZiel)
+
+ while(not(np.allclose(startPos, sollPos, atol = 50))):
+
+ sphero.roll(100, (phiZiel))
+
+ aktPos = self.positionauslesen()
+ aktPos = np.array(aktPos)
+
+ time.sleep(0.1)
+
+ print("Ziel erreicht")
+
+ sphero.roll(0, (0))
+
+ return
+
+
+def main(args = None):
+ #Verbindung herstellen, Node erstellen
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ rate = node.create_rate(5)
+
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ sphero.setLEDColor(255,0,0)
+ sphero.wait(1)
+ sphero.setBackLEDIntensity(100)
+ sphero.stabilization(True)
+
+ #Differenzwinkel auslesen
+ phiAbw = node.ref_winkel(sphero)
+ print("Abweichender Winkel Initialfahrt:", phiAbw)
+
+ sphero.wait(5)
+ sphero.roll(100, phiAbw)
+
+ sphero.wait(5)
+ node.fahre_ziel(sphero, 94,94, phiAbw)
+
+ sphero.wait(5)
+ node.fahre_ziel(sphero, 134,462, phiAbw)
+
+ sphero.wait(5)
+ node.fahre_ziel(sphero, 581,372, phiAbw)
+
+ sphero.wait(5)
+ node.fahre_ziel(sphero, 331,308, phiAbw)
+
+ rclpy.spin()
+ rclpy.shutdown()
+
+if __name__ == '__main__':
+ main()
+
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_refwinkel.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_refwinkel.py
new file mode 100644
index 0000000000000000000000000000000000000000..78066a4c2fa4da3a7a78b3faca6b5427ae979e42
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_refwinkel.py
@@ -0,0 +1,243 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import json
+from sensor_msgs.msg import Imu
+import threading
+from std_msgs.msg import String
+from geometry_msgs.msg import Quaternion, Vector3
+from box import Box
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import datetime
+
+class MyNode(Node):
+ #Konstrukter Node Objekt
+ def __init__(self):
+ super().__init__("sphero_mini")
+ #self.publisher_ = self.create_publisher(String,"Imu",5)
+
+ def connect(self):
+ #Automatisch MAC-Adresse laden
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ def positionauslesen(self):
+ # Standard imports
+ import cv2
+ import numpy as np;
+
+ # Img Objekt
+ cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+ # Set up the detector with default parameters.
+ detector = cv2.SimpleBlobDetector()
+
+ # Setup SimpleBlobDetector parameters.
+ params = cv2.SimpleBlobDetector_Params()
+
+ # Change thresholds
+ #params.minThreshold = 5
+ #params.maxThreshold = 500
+
+ #FIlter by Color
+ params.filterByColor = True
+ params.blobColor = 255
+
+ # Filter by Area.
+ #params.filterByArea = True
+ #params.minArea = 100
+ #params.maxArea = 1000
+
+ # Filter by Circularity
+ #params.filterByCircularity = True
+ #params.minCircularity = 0.5
+ #params.maxCircularity = 1
+
+ # Filter by Convexity
+ #params.filterByConvexity = True
+ #params.minConvexity = 0.7
+ #params.maxConvexity = 1
+
+ # Filter by Inertia
+ #params.filterByInertia = True
+ #params.minInertiaRatio = 0.01
+
+ # Create a detector with the parameters
+ detector = cv2.SimpleBlobDetector_create(params)
+
+ success, img = cap.read()
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+
+ # Detect blobs.
+ keypoints = detector.detect(gray)
+
+ #print(keypoints)
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+ #keypointss = keyPoint.sizekeyPoints
+
+ #print("Blob X: %f Blob Y: %f" %(x,y))
+
+ # Draw detected blobs as red circles.
+ # cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures
+ # the size of the circle corresponds to the size of blob
+
+ im_with_keypoints = cv2.drawKeypoints(gray, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
+
+ # Show blobs
+ cv2.imshow("Keypoints", im_with_keypoints)
+
+ return (x,y)
+
+ def ref_winkel(self, sphero, x,y):
+
+ startpunkt = self.positionauslesen()
+
+ sphero.roll(100,0)
+ sphero.wait(2)
+ sphero.roll(0,0)
+ sphero.wait(0.5)
+
+ ref = self.positionauslesen()
+
+ #print(ref)
+
+ sphero.wait(0.5)
+ sphero.roll(100,180)
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+ soll = (x,y)
+
+ soll = np.array(soll)
+ ref = np.array(ref)
+ startpunkt = np.array(startpunkt)
+
+ start_ref = ref-startpunkt
+ start_soll = soll-startpunkt
+ phi = np.arccos(np.dot(start_ref,start_soll) / (np.linalg.norm(start_ref)*np.linalg.norm(start_soll)))
+ return phi
+
+def main(args = None):
+ #Verbindung herstellen, Node erstellen
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ rate = node.create_rate(5)
+
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ #Simple Moves, Erste Bewegung erkennen
+ #try:oll = (0,0)
+ # while rclpy.ok():
+
+ #sphero.positionauslesen()
+ #soll = 0,0
+ phi = node.ref_winkel(sphero, 0,0)
+
+ phi = np.degrees(phi)
+
+ phi = int(phi)
+
+ print("Abweihender Winkel:", phi)
+
+ sphero.wait(0.5)
+ sphero.roll(100, (0 + phi))
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+# except KeyboardInterrupt:
+ # pass
+
+ rclpy.shutdown()
+ # thread.join()
+
+if __name__ == '__main__':
+ main()
+
+ #!/usr/bin/env python3
+
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_testfahrt.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_testfahrt.py
new file mode 100644
index 0000000000000000000000000000000000000000..29f2473b20a9c782704e15851518da8ec90ee823
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node_testfahrt.py
@@ -0,0 +1,183 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+import json
+from sensor_msgs.msg import Imu
+import threading
+from std_msgs.msg import String
+from geometry_msgs.msg import Quaternion, Vector3
+from box import Box
+import numpy as np
+from sphero_mini_controller.treiber import SpheroMini
+import datetime
+
+
+class MyNode(Node):
+
+ def __init__(self):
+ super().__init__("sphero_mini")
+ self.publisher_ = self.create_publisher(String,"Imu",5)
+ #self.get_logger().info("Hello from ROS2")
+
+ def connect(self):
+ #conf_file_path = file("/sphero_conf.json")
+ #with open("sphero_conf.json", "r") as f:
+ # cfg = Box(json.load(f))
+
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ #print(i)
+
+ #self.publisher_.publish(i)
+
+
+
+def main(args = None):
+
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+ rate = node.create_rate(2)
+
+ try:
+ while rclpy.ok():
+
+ sphero.configureSensorMask(
+ IMU_yaw=True,
+ IMU_pitch=True,
+ IMU_roll=True,
+ IMU_acc_y=True,
+ IMU_acc_z=True,
+ IMU_acc_x=True,
+ IMU_gyro_x=True,
+ IMU_gyro_y=True,
+ #IMU_gyro_z=True
+ )
+ sphero.configureSensorStream()
+
+ sensors_values = node.get_sensors_data(sphero)
+ #rclpy.logdebug(sensors_values)
+ node.publish_imu(sensors_values, node)
+ sphero.setLEDColor(red = 0, green = 255, blue = 0) # Turn LEDs green
+
+ # Aiming:
+ sphero.setLEDColor(red = 0, green = 0, blue = 0) # Turn main LED off
+ sphero.stabilization(False) # Turn off stabilization
+ sphero.setBackLEDIntensity(255) # turn back LED on
+ sphero.wait(3) # Non-blocking pau`se
+ sphero.resetHeading() # Reset heading
+ sphero.stabilization(True) # Turn on stabilization
+ sphero.setBackLEDIntensity(0) # Turn back LED off
+
+ # Move around:
+ sphero.setLEDColor(red = 0, green = 0, blue = 255) # Turn main LED blue
+ sphero.roll(100, 0) # roll forwards (heading = 0) at speed = 50
+
+ sphero.wait(3) # Keep rolling for three seconds
+
+ sphero.roll(0, 0) # stop
+ sphero.wait(1) # Allow time to stop
+
+ sphero.setLEDColor(red = 0, green = 255, blue = 0) # Turn main LED green
+ sphero.roll(-100, 0) # Keep facing forwards but roll backwards at speed = 50
+ sphero.wait(3) # Keep rolling for three seconds
+
+ sphero.roll(0, 0) # stop
+ sphero.wait(1) # Allow time to stop
+
+ rate.sleep()
+
+ except KeyboardInterrupt:
+ pass
+
+ rclpy.shutdown()
+ thread.join()
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/pfadplanung.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/pfadplanung.py
new file mode 100644
index 0000000000000000000000000000000000000000..d07a2a00e56b7626e47d9ae9ba004c28b8fa3a94
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/pfadplanung.py
@@ -0,0 +1,223 @@
+
+def planInitialPath(start, goal):
+
+ from matplotlib import pyplot as ppl
+ from matplotlib import cm
+ import random, sys, math, os.path
+ from matplotlib.pyplot import imread
+
+ #Implementation of RRT
+
+ MAP_IMG = './map.jpg' #Black and white image for a map
+ MIN_NUM_VERT = 20 # Minimum number of vertex in the graph
+ MAX_NUM_VERT = 1500 # Maximum number of vertex in the graph
+ STEP_DISTANCE = 10 # Maximum distance between two vertex
+ SEED = None # For random numbers
+
+ def rapidlyExploringRandomTree(ax, img, start, goal, seed=None):
+ hundreds = 100
+ seed = random.seed(seed)
+ #print("Zufallsseed: ", seed)
+ points = []
+ graph = []
+ points.append(start)
+ graph.append((start, []))
+ print('Generating and conecting random points')
+ occupied = True
+ phaseTwo = False
+
+ # Phase two values (points 5 step distances around the goal point)
+ minX = max(goal[0] - 5 * STEP_DISTANCE, 0)
+ maxX = min(goal[0] + 5 * STEP_DISTANCE, len(img[0]) - 1)
+ minY = max(goal[1] - 5 * STEP_DISTANCE, 0)
+ maxY = min(goal[1] + 5 * STEP_DISTANCE, len(img) - 1)
+
+ i = 0
+ while (goal not in points) and (len(points) < MAX_NUM_VERT):
+ if (i % 100) == 0:
+ print(i, 'points randomly generated')
+
+ if (len(points) % hundreds) == 0:
+ print(len(points), 'vertex generated')
+ hundreds = hundreds + 100
+
+ while (occupied):
+ if phaseTwo and (random.random() > 0.8):
+ point = [random.randint(minX, maxX), random.randint(minY, maxY)]
+ else:
+ point = [random.randint(0, len(img[0]) - 1), random.randint(0, len(img) - 1)]
+
+ if (img[point[1]][point[0]][0] * 255 == 255):
+ occupied = False
+
+ occupied = True
+
+ nearest = findNearestPoint(points, point)
+ newPoints = connectPoints(point, nearest, img)
+ addToGraph(ax, graph, newPoints, point)
+ newPoints.pop(0) # The first element is already in the points list
+ points.extend(newPoints)
+ ppl.draw()
+ i = i + 1
+
+ if len(points) >= MIN_NUM_VERT:
+ if not phaseTwo:
+ print('Phase Two')
+ phaseTwo = True
+
+ if phaseTwo:
+ nearest = findNearestPoint(points, goal)
+ newPoints = connectPoints(goal, nearest, img)
+ addToGraph(ax, graph, newPoints, goal)
+ newPoints.pop(0)
+ points.extend(newPoints)
+ ppl.draw()
+
+ if goal in points:
+ print('Goal found, total vertex in graph:', len(points), 'total random points generated:', i)
+
+ path = searchPath(graph, start, [start])
+
+ for i in range(len(path) - 1):
+ ax.plot([path[i][0], path[i + 1][0]], [path[i][1], path[i + 1][1]], color='g', linestyle='-',
+ linewidth=2)
+ ppl.draw()
+
+ print('Showing resulting map')
+ print('Final path:', path)
+ print('The final path is made from:', len(path), 'connected points')
+ else:
+ path = None
+ print('Reached maximum number of vertex and goal was not found')
+ print('Total vertex in graph:', len(points), 'total random points generated:', i)
+ print('Showing resulting map')
+
+ ppl.show()
+ return path
+
+ def searchPath(graph, point, path):
+ for i in graph:
+ if point == i[0]:
+ p = i
+
+ if p[0] == graph[-1][0]:
+ return path
+
+ for link in p[1]:
+ path.append(link)
+ finalPath = searchPath(graph, link, path)
+
+ if finalPath != None:
+ return finalPath
+ else:
+ path.pop()
+
+ def addToGraph(ax, graph, newPoints, point):
+ if len(newPoints) > 1: # If there is anything to add to the graph
+ for p in range(len(newPoints) - 1):
+ nearest = [nearest for nearest in graph if (nearest[0] == [newPoints[p][0], newPoints[p][1]])]
+ nearest[0][1].append(newPoints[p + 1])
+ graph.append((newPoints[p + 1], []))
+
+ if not p == 0:
+ ax.plot(newPoints[p][0], newPoints[p][1], '+k') # First point is already painted
+ ax.plot([newPoints[p][0], newPoints[p + 1][0]], [newPoints[p][1], newPoints[p + 1][1]], color='k',
+ linestyle='-', linewidth=1)
+
+ if point in newPoints:
+ ax.plot(point[0], point[1], '.g') # Last point is green
+ else:
+ ax.plot(newPoints[p + 1][0], newPoints[p + 1][1], '+k') # Last point is not green
+
+ def connectPoints(a, b, img):
+ newPoints = []
+ newPoints.append([b[0], b[1]])
+ step = [(a[0] - b[0]) / float(STEP_DISTANCE), (a[1] - b[1]) / float(STEP_DISTANCE)]
+
+ # Set small steps to check for walls
+ pointsNeeded = int(math.floor(max(math.fabs(step[0]), math.fabs(step[1]))))
+
+ if math.fabs(step[0]) > math.fabs(step[1]):
+ if step[0] >= 0:
+ step = [1, step[1] / math.fabs(step[0])]
+ else:
+ step = [-1, step[1] / math.fabs(step[0])]
+
+ else:
+ if step[1] >= 0:
+ step = [step[0] / math.fabs(step[1]), 1]
+ else:
+ step = [step[0] / math.fabs(step[1]), -1]
+
+ blocked = False
+ for i in range(pointsNeeded + 1): # Creates points between graph and solitary point
+ for j in range(STEP_DISTANCE): # Check if there are walls between points
+ coordX = round(newPoints[i][0] + step[0] * j)
+ coordY = round(newPoints[i][1] + step[1] * j)
+
+ if coordX == a[0] and coordY == a[1]:
+ break
+ if coordY >= len(img) or coordX >= len(img[0]):
+ break
+ if img[int(coordY)][int(coordX)][0] * 255 < 255:
+ blocked = True
+ if blocked:
+ break
+
+ if blocked:
+ break
+ if not (coordX == a[0] and coordY == a[1]):
+ newPoints.append(
+ [newPoints[i][0] + (step[0] * STEP_DISTANCE), newPoints[i][1] + (step[1] * STEP_DISTANCE)])
+
+ if not blocked:
+ newPoints.append([a[0], a[1]])
+ return newPoints
+
+ def findNearestPoint(points, point):
+ best = (sys.maxsize, sys.maxsize, sys.maxsize)
+ for p in points:
+ if p == point:
+ continue
+ dist = math.sqrt((p[0] - point[0]) ** 2 + (p[1] - point[1]) ** 2)
+ if dist < best[2]:
+ best = (p[0], p[1], dist)
+ return (best[0], best[1])
+
+ # Standard imports
+ import cv2
+ import numpy as np;
+
+ # Img Objekt
+ cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+ val, frame = cap.read()
+ inverted_image = np.invert(frame)
+ cv2.imwrite("map.jpg", inverted_image)
+
+ print('Loading map... with file \'', MAP_IMG, '\'')
+ img = imread(MAP_IMG)
+ fig = ppl.gcf()
+ fig.clf()
+ ax = fig.add_subplot(1, 1, 1)
+ ax.imshow(img, cmap=cm.Greys_r)
+ ax.axis('image')
+ ppl.draw()
+ print('Map is', len(img[0]), 'x', len(img))
+
+ path = rapidlyExploringRandomTree(ax, img, start, goal, seed=SEED)
+
+ if len(sys.argv) > 2:
+ print('Only one argument is needed')
+ elif len(sys.argv) > 1:
+ if os.path.isfile(sys.argv[1]):
+ MAP_IMG = sys.argv[1]
+ else:
+ print(sys.argv[1], 'is not a file')
+
+ pathIndCntr = 0
+
+ return path, pathIndCntr
+
+start = (200,200)
+ziel = (450, 450)
+planInitialPath(start, ziel)
\ No newline at end of file
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/potential_field_planning (1).py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/potential_field_planning (1).py
new file mode 100644
index 0000000000000000000000000000000000000000..8f136b5ee3fbee7f9cd0d40fd368a4e10822030b
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/potential_field_planning (1).py
@@ -0,0 +1,199 @@
+"""
+
+Potential Field based path planner
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+Ref:
+https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
+
+"""
+
+from collections import deque
+import numpy as np
+import matplotlib.pyplot as plt
+
+# Parameters
+KP = 5.0 # attractive potential gain
+ETA = 100.0 # repulsive potential gain
+AREA_WIDTH = 30.0 # potential area width [m]
+# the number of previous positions used to check oscillations
+OSCILLATIONS_DETECTION_LENGTH = 3
+
+show_animation = True
+
+
+def calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy):
+ minx = min(min(ox), sx, gx) - AREA_WIDTH / 2.0
+ miny = min(min(oy), sy, gy) - AREA_WIDTH / 2.0
+ maxx = max(max(ox), sx, gx) + AREA_WIDTH / 2.0
+ maxy = max(max(oy), sy, gy) + AREA_WIDTH / 2.0
+ xw = int(round((maxx - minx) / reso))
+ yw = int(round((maxy - miny) / reso))
+
+ # calc each potential
+ pmap = [[0.0 for i in range(yw)] for i in range(xw)]
+
+ for ix in range(xw):
+ x = ix * reso + minx
+
+ for iy in range(yw):
+ y = iy * reso + miny
+ ug = calc_attractive_potential(x, y, gx, gy)
+ uo = calc_repulsive_potential(x, y, ox, oy, rr)
+ uf = ug + uo
+ pmap[ix][iy] = uf
+
+ return pmap, minx, miny
+
+
+def calc_attractive_potential(x, y, gx, gy):
+ return 0.5 * KP * np.hypot(x - gx, y - gy)
+
+
+def calc_repulsive_potential(x, y, ox, oy, rr):
+ # search nearest obstacle
+ minid = -1
+ dmin = float("inf")
+ for i, _ in enumerate(ox):
+ d = np.hypot(x - ox[i], y - oy[i])
+ if dmin >= d:
+ dmin = d
+ minid = i
+
+ # calc repulsive potential
+ dq = np.hypot(x - ox[minid], y - oy[minid])
+
+ if dq <= rr:
+ if dq <= 0.1:
+ dq = 0.1
+
+ return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2
+ else:
+ return 0.0
+
+
+def get_motion_model():
+ # dx, dy
+ motion = [[1, 0],
+ [0, 1],
+ [-1, 0],
+ [0, -1],
+ [-1, -1],
+ [-1, 1],
+ [1, -1],
+ [1, 1]]
+
+ return motion
+
+
+def oscillations_detection(previous_ids, ix, iy):
+ previous_ids.append((ix, iy))
+
+ if (len(previous_ids) > OSCILLATIONS_DETECTION_LENGTH):
+ previous_ids.popleft()
+
+ # check if contains any duplicates by copying into a set
+ previous_ids_set = set()
+ for index in previous_ids:
+ if index in previous_ids_set:
+ return True
+ else:
+ previous_ids_set.add(index)
+ return False
+
+
+def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
+
+ # calc potential field
+ pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy)
+
+ # search path
+ d = np.hypot(sx - gx, sy - gy)
+ ix = round((sx - minx) / reso)
+ iy = round((sy - miny) / reso)
+ gix = round((gx - minx) / reso)
+ giy = round((gy - miny) / reso)
+
+ if show_animation:
+ draw_heatmap(pmap)
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect('key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
+ plt.plot(ix, iy, "*k")
+ plt.plot(gix, giy, "*m")
+
+ rx, ry = [sx], [sy]
+ motion = get_motion_model()
+ previous_ids = deque()
+
+ while d >= reso:
+ minp = float("inf")
+ minix, miniy = -1, -1
+ for i, _ in enumerate(motion):
+ inx = int(ix + motion[i][0])
+ iny = int(iy + motion[i][1])
+ if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0:
+ p = float("inf") # outside area
+ print("outside potential!")
+ else:
+ p = pmap[inx][iny]
+ if minp > p:
+ minp = p
+ minix = inx
+ miniy = iny
+ ix = minix
+ iy = miniy
+ xp = ix * reso + minx
+ yp = iy * reso + miny
+ d = np.hypot(gx - xp, gy - yp)
+ rx.append(xp)
+ ry.append(yp)
+
+ if (oscillations_detection(previous_ids, ix, iy)):
+ print("Oscillation detected at ({},{})!".format(ix, iy))
+ break
+
+ if show_animation:
+ plt.plot(ix, iy, ".r")
+ plt.pause(0.01)
+
+ print("Goal!!")
+
+ return rx, ry
+
+
+def draw_heatmap(data):
+ data = np.array(data).T
+ plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)
+
+
+def main():
+ print("potential_field_planning start")
+
+ sx = 0.0 # start x position [m]
+ sy = 10.0 # start y positon [m]
+ gx = 30.0 # goal x position [m]
+ gy = 30.0 # goal y position [m]
+ grid_size = 0.5 # potential grid size [m]
+ robot_radius = 5.0 # robot radius [m]
+
+ ox = [15.0, 5.0, 20.0, 25.0] # obstacle x position list [m]
+ oy = [25.0, 15.0, 26.0, 25.0] # obstacle y position list [m]
+
+ if show_animation:
+ plt.grid(True)
+ plt.axis("equal")
+
+ # path generation
+ _, _ = potential_field_planning(
+ sx, sy, gx, gy, ox, oy, grid_size, robot_radius)
+
+ if show_animation:
+ plt.show()
+
+
+if __name__ == '__main__':
+ print(__file__ + " start!!")
+ main()
+ print(__file__ + " Done!!")
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/potential_field_planning.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/potential_field_planning.py
new file mode 100644
index 0000000000000000000000000000000000000000..c28279d83d3ea64eaee8592afabe70a8fe5372d1
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/potential_field_planning.py
@@ -0,0 +1,248 @@
+"""
+
+Potential Field based path planner
+
+author: Atsushi Sakai (@Atsushi_twi)
+
+Ref:
+https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
+
+"""
+
+from collections import deque
+import numpy as np
+import matplotlib.pyplot as plt
+
+# Parameters
+KP = 10.0 # attractive potential gain
+ETA = 300.0 # repulsive potential gain
+AREA_WIDTH = 704.0 # potential area width [m]
+# the number of previous positions used to check oscillations
+OSCILLATIONS_DETECTION_LENGTH = 3
+
+show_animation = True
+
+px = []
+py = []
+
+def calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy):
+ minx = min(min(ox), sx, gx) - AREA_WIDTH / 2.0
+ miny = min(min(oy), sy, gy) - AREA_WIDTH / 2.0
+ maxx = max(max(ox), sx, gx) + AREA_WIDTH / 2.0
+ maxy = max(max(oy), sy, gy) + AREA_WIDTH / 2.0
+ xw = int(round((maxx - minx) / reso))
+ yw = int(round((maxy - miny) / reso))
+
+ # calc each potential
+ pmap = [[0.0 for i in range(yw)] for i in range(xw)]
+
+ for ix in range(xw):
+ x = ix * reso + minx
+
+ for iy in range(yw):
+ y = iy * reso + miny
+ ug = calc_attractive_potential(x, y, gx, gy)
+ uo = calc_repulsive_potential(x, y, ox, oy, rr)
+ uf = ug + uo
+ pmap[ix][iy] = uf
+
+ return pmap, minx, miny
+
+
+def calc_attractive_potential(x, y, gx, gy):
+ return 0.5 * KP * np.hypot(x - gx, y - gy)
+
+
+def calc_repulsive_potential(x, y, ox, oy, rr):
+ # search nearest obstacle
+ minid = -1
+ dmin = float("inf")
+ for i, _ in enumerate(ox):
+ d = np.hypot(x - ox[i], y - oy[i])
+ if dmin >= d:
+ dmin = d
+ minid = i
+
+ # calc repulsive potential
+ dq = np.hypot(x - ox[minid], y - oy[minid])
+
+ if dq <= rr:
+ if dq <= 0.1:
+ dq = 0.1
+
+ return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2
+ else:
+ return 0.0
+
+
+def get_motion_model():
+ # dx, dy
+ motion = [[1, 0],
+ [0, 1],
+ [-1, 0],
+ [0, -1],
+ [-1, -1],
+ [-1, 1],
+ [1, -1],
+ [1, 1]]
+
+ return motion
+
+
+def oscillations_detection(previous_ids, ix, iy):
+ previous_ids.append((ix, iy))
+
+ if (len(previous_ids) > OSCILLATIONS_DETECTION_LENGTH):
+ previous_ids.popleft()
+
+ # check if contains any duplicates by copying into a set
+ previous_ids_set = set()
+ for index in previous_ids:
+ if index in previous_ids_set:
+ return True
+ else:
+ previous_ids_set.add(index)
+ return False
+
+
+def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
+
+ # calc potential field
+ pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr, sx, sy)
+
+ # search path
+ d = np.hypot(sx - gx, sy - gy)
+ ix = round((sx - minx) / reso)
+ iy = round((sy - miny) / reso)
+ gix = round((gx - minx) / reso)
+ giy = round((gy - miny) / reso)
+
+ if show_animation:
+ draw_heatmap(pmap)
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect('key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
+ plt.plot(ix, iy, "*k")
+ plt.plot(gix, giy, "*m")
+
+ rx, ry = [sx], [sy]
+ motion = get_motion_model()
+ previous_ids = deque()
+
+ while d >= reso:
+ minp = float("inf")
+ minix, miniy = -1, -1
+ for i, _ in enumerate(motion):
+ inx = int(ix + motion[i][0])
+ iny = int(iy + motion[i][1])
+ if inx >= len(pmap) or iny >= len(pmap[0]) or inx < 0 or iny < 0:
+ p = float("inf") # outside area
+ print("outside potential!")
+ else:
+ p = pmap[inx][iny]
+ if minp > p:
+ minp = p
+ minix = inx
+ miniy = iny
+ ix = minix
+ iy = miniy
+ xp = ix * reso + minx
+ yp = iy * reso + miny
+ d = np.hypot(gx - xp, gy - yp)
+ rx.append(xp)
+ ry.append(yp)
+
+ if (oscillations_detection(previous_ids, ix, iy)):
+ print("Oscillation detected at ({},{})!".format(ix, iy))
+ break
+
+ if show_animation:
+ px.append(ix)
+ py.append(iy)
+ plt.plot(ix, iy, ".r")
+ plt.pause(0.01)
+
+ print("Goal!!")
+
+ return rx, ry
+
+
+def draw_heatmap(data):
+ data = np.array(data).T
+ plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues)
+
+
+def create_map():
+ # Map erstellen
+ # Standard imports
+ import cv2
+ import numpy as np;
+
+ # Img Objekt
+ #cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+ #success, img = cap.read()
+
+ gray = cv2.imread("/home/ubuntu/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/map/map.jpg",cv2.IMREAD_GRAYSCALE)
+
+ #gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
+ mask = cv2.inRange(gray, np.array([90]), np.array([255]))
+
+ mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+
+ mask_list= np.ndarray.tolist(mask)
+
+ for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['W' if j > 200 else ' ' for j in mask_list[i]]
+
+ #mapOfWorld[200][200] = 'R' #Start Pos
+ #mapOfWorld[300][300] = 'G' #Ziel Pos
+
+ return mapOfWorld
+
+
+def create_obsticles(mapOfWorld):
+ ox = []
+ oy = []
+
+ mapOfWorld = np.array(mapOfWorld)
+
+ for i in range(0,mapOfWorld.shape[0]):
+ for j in range(0,mapOfWorld.shape[1]):
+ if mapOfWorld[i][j] == 'W':
+ ox.append(i)
+ oy.append(j)
+
+ return ox,oy
+
+def main():
+ print("potential_field_planning start")
+
+ mapOfWorld = create_map()
+
+ sx = 50 # start x position [m]
+ sy = 400 # start y positon [m]
+ gx = 600 # goal x position [m]
+ gy = 100 # goal y position [m]
+ grid_size = 10 # potential grid size [m]
+ robot_radius = 10 # robot radius [m]
+
+ ox, oy = create_obsticles(mapOfWorld)
+
+ if show_animation:
+ plt.grid(True)
+ plt.axis("equal")
+
+ # path generation
+ _, _ = potential_field_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_radius)
+
+ if show_animation:
+ plt.show()
+
+ print("X_Pos", px)
+ print("Y_Pos", py)
+
+if __name__ == '__main__':
+ print(__file__ + " start!!")
+ main()
+ print(__file__ + " Done!!")
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/simple_moves.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/simple_moves.py
new file mode 100644
index 0000000000000000000000000000000000000000..b1e9f0e01674843c28ab6564ff572a695ff74d8b
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/simple_moves.py
@@ -0,0 +1,34 @@
+"""
+Simple moves
+------------
+
+This module contains functions to have the Sphero mini do some simple movements (e.g. circle).
+"""
+
+import sys
+
+def forward(sphero):
+ # Aiming:
+ sphero.setLEDColor(red = 0, green = 0, blue = 0) # Turn main LED off
+ sphero.stabilization(False) # Turn off stabilization
+ sphero.setBackLEDIntensity(255) # turn back LED on
+ sphero.wait(3) # Non-blocking pau`se
+ sphero.resetHeading() # Reset heading
+ sphero.stabilization(True) # Turn on stabilization
+ sphero.setBackLEDIntensity(0) # Turn back LED off
+
+ # Move around:
+ sphero.setLEDColor(red = 0, green = 0, blue = 255) # Turn main LED blue
+ sphero.roll(100, 0) # roll forwards (heading = 0) at speed = 50
+
+ sphero.wait(3) # Keep rolling for three seconds
+
+ sphero.roll(0, 0) # stop
+ sphero.wait(1) # Allow time to stop
+
+ sphero.setLEDColor(red = 0, green = 255, blue = 0) # Turn main LED green
+ sphero.roll(-100, 0) # Keep facing forwards but roll backwards at speed = 50
+ sphero.wait(3) # Keep rolling for three seconds
+
+ sphero.roll(0, 0) # stop
+ sphero.wait(1) # Allow time to stop
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/sphero.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/sphero.py
new file mode 100644
index 0000000000000000000000000000000000000000..b5138d1b23a13a1117038f444cc8a0de0dbc0078
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/sphero.py
@@ -0,0 +1,22 @@
+import numpy as np
+def ref_winkel(sphero, soll):
+
+ startpunkt = sphero.positionsauslesen
+
+ sphero.roll(100,0)
+ sphero.wait(2)
+ sphero.roll(0,0)
+ sphero.wait(0.5)
+
+ ref = sphero.positionsauslesen
+
+ sphero.wait(0.5)
+ sphero.roll(100,0)
+ sphero.wait(2)
+ sphero.roll(0,0)
+
+ start_ref = ref-startpunkt
+ start_soll = soll-startpunkt
+ phi = np.arccos(start_ref*start_soll / (np.linalg.norm(start_ref)*np.linalg.norm(start_soll)))
+ return phi
+
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/sphero_main.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/sphero_main.py
new file mode 100644
index 0000000000000000000000000000000000000000..37e337b8ed0ee0faf7ea8c2c1b698cc6e693f1f8
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/sphero_main.py
@@ -0,0 +1,137 @@
+#!/usr/bin/env python3
+
+import json
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+from geometry_msgs.msg import Quaternion, Vector3
+from box import Box
+import numpy as np
+from core import SpheroMini
+#from simple_moves import forward
+
+def connect():
+ conf_file_path = rclpy.get_param("/conf_file_path")
+ with open(conf_file_path, 'r') as f:
+ cfg = Box(json.load(f))
+
+ # Connect:
+ sphero = SpheroMini(cfg.MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+
+def disconnect(sphero):
+ sphero.sleep()
+ sphero.disconnect()
+
+
+def create_quaternion(roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+
+def create_angular_veolocity_vector3(groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = gyaw
+
+ return v
+
+
+def create_linear_acc_vector3(xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+
+def get_sensors_data(sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+
+def publish_imu(pub, sensors_values):
+ i = Imu()
+
+ i.header.stamp = rclpy.Time.now()
+ i.orientation = create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+ pub.publish(i)
+
+
+def main(sphero):
+ rclpy.init_node('sphero', anonymous=True, log_level=rclpy.DEBUG)
+ rate = rclpy.Rate(10) # 10 Hz
+
+ pub = rclpy.Publisher("/imu", Imu, queue_size=5)
+
+ sphero.configureSensorMask(
+ IMU_yaw=True,
+ IMU_pitch=True,
+ IMU_roll=True,
+ IMU_acc_y=True,
+ IMU_acc_z=True,
+ IMU_acc_x=True,
+ IMU_gyro_x=True,
+ IMU_gyro_y=True,
+ #IMU_gyro_z=True
+ )
+ sphero.configureSensorStream()
+
+ while not rclpy.is_shutdown():
+ sensors_values = get_sensors_data(sphero)
+ #rclpy.logdebug(sensors_values)
+ publish_imu(pub, sensors_values)
+ sphero.setLEDColor(red = 0, green = 255, blue = 0) # Turn LEDs green
+ rate.sleep()
+
+
+if __name__ == "__main__":
+ sphero = connect()
+ try:
+ main(sphero)
+ except Exception as e: # rclpy.ROSInterruptException
+ disconnect(sphero)
+ raise e
+
\ No newline at end of file
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/test.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/test.py
new file mode 100644
index 0000000000000000000000000000000000000000..b5c4cbbe779a35653aad3fe176e1a0b9802319fd
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/test.py
@@ -0,0 +1,23 @@
+#Map erstellen
+# Standard imports
+import cv2
+import numpy as np;
+
+# Img Objekt
+cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+success, img = cap.read()
+
+gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
+mask = cv2.inRange(gray, np.array([90]), np.array([255]))
+
+mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+
+mask_list= np.ndarray.tolist(mask)
+
+for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['W' if j > 200 else ' ' for j in mask_list[i]]
+
+print(mapOfWorld)
+
+
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/treiber.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/treiber.py
new file mode 100644
index 0000000000000000000000000000000000000000..ec3435051de5aa72fc393d224b0fe1a53927b8b0
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/treiber.py
@@ -0,0 +1,638 @@
+from bluepy.btle import Peripheral
+from bluepy import btle
+from sphero_mini_controller._constants import *
+import struct
+import time
+import sys
+
+class SpheroMini():
+ def __init__(self, MACAddr, verbosity = 4, user_delegate = None):
+ '''
+ initialize class instance and then build collect BLE sevices and characteristics.
+ Also sends text string to Anti-DOS characteristic to prevent returning to sleep,
+ and initializes notifications (which is what the sphero uses to send data back to
+ the client).
+ '''
+ self.verbosity = verbosity # 0 = Silent,
+ # 1 = Connection/disconnection only
+ # 2 = Init messages
+ # 3 = Recieved commands
+ # 4 = Acknowledgements
+ self.sequence = 1
+ self.v_batt = None # will be updated with battery voltage when sphero.getBatteryVoltage() is called
+ self.firmware_version = [] # will be updated with firware version when sphero.returnMainApplicationVersion() is called
+
+ if self.verbosity > 0:
+ print("[INFO] Connecting to ", MACAddr)
+ self.p = Peripheral(MACAddr, "random") #connect
+
+ if self.verbosity > 1:
+ print("[INIT] Initializing")
+
+ # Subscribe to notifications
+ self.sphero_delegate = MyDelegate(self, user_delegate) # Pass a reference to this instance when initializing
+ self.p.setDelegate(self.sphero_delegate)
+
+ if self.verbosity > 1:
+ print("[INIT] Read all characteristics and descriptors")
+ # Get characteristics and descriptors
+ self.API_V2_characteristic = self.p.getCharacteristics(uuid="00010002-574f-4f20-5370-6865726f2121")[0]
+ self.AntiDOS_characteristic = self.p.getCharacteristics(uuid="00020005-574f-4f20-5370-6865726f2121")[0]
+ self.DFU_characteristic = self.p.getCharacteristics(uuid="00020002-574f-4f20-5370-6865726f2121")[0]
+ self.DFU2_characteristic = self.p.getCharacteristics(uuid="00020004-574f-4f20-5370-6865726f2121")[0]
+ self.API_descriptor = self.API_V2_characteristic.getDescriptors(forUUID=0x2902)[0]
+ self.DFU_descriptor = self.DFU_characteristic.getDescriptors(forUUID=0x2902)[0]
+
+ # The rest of this sequence was observed during bluetooth sniffing:
+ # Unlock code: prevent the sphero mini from going to sleep again after 10 seconds
+ if self.verbosity > 1:
+ print("[INIT] Writing AntiDOS characteristic unlock code")
+ self.AntiDOS_characteristic.write("usetheforce...band".encode(), withResponse=True)
+
+ # Enable DFU notifications:
+ if self.verbosity > 1:
+ print("[INIT] Configuring DFU descriptor")
+ self.DFU_descriptor.write(struct.pack('<bb', 0x01, 0x00), withResponse = True)
+
+ # No idea what this is for. Possibly a device ID of sorts? Read request returns '00 00 09 00 0c 00 02 02':
+ if self.verbosity > 1:
+ print("[INIT] Reading DFU2 characteristic")
+ _ = self.DFU2_characteristic.read()
+
+ # Enable API notifications:
+ if self.verbosity > 1:
+ print("[INIT] Configuring API dectriptor")
+ self.API_descriptor.write(struct.pack('<bb', 0x01, 0x00), withResponse = True)
+
+ self.wake()
+
+ # Finished initializing:
+ if self.verbosity > 1:
+ print("[INIT] Initialization complete\n")
+
+ def disconnect(self):
+ if self.verbosity > 0:
+ print("[INFO] Disconnecting")
+
+ self.p.disconnect()
+
+ def wake(self):
+ '''
+ Bring device out of sleep mode (can only be done if device was in sleep, not deep sleep).
+ If in deep sleep, the device should be connected to USB power to wake.
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Waking".format(self.sequence))
+
+ self._send(characteristic=self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=powerCommandIDs["wake"],
+ payload=[]) # empty payload
+
+ self.getAcknowledgement("Wake")
+
+ def sleep(self, deepSleep=False):
+ '''
+ Put device to sleep or deep sleep (deep sleep needs USB power connected to wake up)
+ '''
+ if deepSleep:
+ sleepCommID=powerCommandIDs["deepSleep"]
+ if self.verbosity > 0:
+ print("[INFO] Going into deep sleep. Connect USB power to wake.")
+ else:
+ sleepCommID=powerCommandIDs["sleep"]
+ self._send(characteristic=self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=sleepCommID,
+ payload=[]) #empty payload
+
+ def setLEDColor(self, red = None, green = None, blue = None):
+ '''
+ Set device LED color based on RGB vales (each can range between 0 and 0xFF)
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Setting main LED colour to [{}, {}, {}]".format(self.sequence, red, green, blue))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['userIO'], # 0x1a
+ commID = userIOCommandIDs["allLEDs"], # 0x0e
+ payload = [0x00, 0x0e, red, green, blue])
+
+ self.getAcknowledgement("LED/backlight")
+
+ def setBackLEDIntensity(self, brightness=None):
+ '''
+ Set device LED backlight intensity based on 0-255 values
+
+ NOTE: this is not the same as aiming - it only turns on the LED
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Setting backlight intensity to {}".format(self.sequence, brightness))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['userIO'],
+ commID = userIOCommandIDs["allLEDs"],
+ payload = [0x00, 0x01, brightness])
+
+ self.getAcknowledgement("LED/backlight")
+
+ def roll(self, speed=None, heading=None):
+ '''
+ Start to move the Sphero at a given direction and speed.
+ heading: integer from 0 - 360 (degrees)
+ speed: Integer from 0 - 255
+
+ Note: the zero heading should be set at startup with the resetHeading method. Otherwise, it may
+ seem that the sphero doesn't honor the heading argument
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Rolling with speed {} and heading {}".format(self.sequence, speed, heading))
+
+ if abs(speed) > 255:
+ print("WARNING: roll speed parameter outside of allowed range (-255 to +255)")
+
+ if speed < 0:
+ speed = -1*speed+256 # speed values > 256 in the send packet make the spero go in reverse
+
+ speedH = (speed & 0xFF00) >> 8
+ speedL = speed & 0xFF
+ headingH = (heading & 0xFF00) >> 8
+ headingL = heading & 0xFF
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['driving'],
+ commID = drivingCommands["driveWithHeading"],
+ payload = [speedL, headingH, headingL, speedH])
+
+ self.getAcknowledgement("Roll")
+
+ def resetHeading(self):
+ '''
+ Reset the heading zero angle to the current heading (useful during aiming)
+ Note: in order to manually rotate the sphero, you need to call stabilization(False).
+ Once the heading has been set, call stabilization(True).
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Resetting heading".format(self.sequence))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['driving'],
+ commID = drivingCommands["resetHeading"],
+ payload = []) #empty payload
+
+ self.getAcknowledgement("Heading")
+
+ def returnMainApplicationVersion(self):
+ '''
+ Sends command to return application data in a notification
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Requesting firmware version".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID = deviceID['systemInfo'],
+ commID = SystemInfoCommands['mainApplicationVersion'],
+ payload = []) # empty
+
+ self.getAcknowledgement("Firmware")
+
+ def getBatteryVoltage(self):
+ '''
+ Sends command to return battery voltage data in a notification.
+ Data printed to console screen by the handleNotifications() method in the MyDelegate class.
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Requesting battery voltage".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=powerCommandIDs['batteryVoltage'],
+ payload=[]) # empty
+
+ self.getAcknowledgement("Battery")
+
+ def stabilization(self, stab = True):
+ '''
+ Sends command to turn on/off the motor stabilization system (required when manually turning/aiming the sphero)
+ '''
+ if stab == True:
+ if self.verbosity > 2:
+ print("[SEND {}] Enabling stabilization".format(self.sequence))
+ val = 1
+ else:
+ if self.verbosity > 2:
+ print("[SEND {}] Disabling stabilization".format(self.sequence))
+ val = 0
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['driving'],
+ commID=drivingCommands['stabilization'],
+ payload=[val])
+
+ self.getAcknowledgement("Stabilization")
+
+ def wait(self, delay):
+ '''
+ This is a non-blocking delay command. It is similar to time.sleep(), except it allows asynchronous
+ notification handling to still be performed.
+ '''
+ start = time.time()
+ while(1):
+ self.p.waitForNotifications(0.001)
+ if time.time() - start > delay:
+ break
+
+ def _send(self, characteristic=None, devID=None, commID=None, payload=[]):
+ '''
+ A generic "send" method, which will be used by other methods to send a command ID, payload and
+ appropriate checksum to a specified device ID. Mainly useful because payloads are optional,
+ and can be of varying length, to convert packets to binary, and calculate and send the
+ checksum. For internal use only.
+
+ Packet structure has the following format (in order):
+
+ - Start byte: always 0x8D
+ - Flags byte: indicate response required, etc
+ - Virtual device ID: see _constants.py
+ - Command ID: see _constants.py
+ - Sequence number: Seems to be arbitrary. I suspect it is used to match commands to response packets (in which the number is echoed).
+ - Payload: Could be varying number of bytes (incl. none), depending on the command
+ - Checksum: See below for calculation
+ - End byte: always 0xD8
+
+ '''
+ sendBytes = [sendPacketConstants["StartOfPacket"],
+ sum([flags["resetsInactivityTimeout"], flags["requestsResponse"]]),
+ devID,
+ commID,
+ self.sequence] + payload # concatenate payload list
+
+ self.sequence += 1 # Increment sequence number, ensures we can identify response packets are for this command
+ if self.sequence > 255:
+ self.sequence = 0
+
+ # Compute and append checksum and add EOP byte:
+ # From Sphero docs: "The [checksum is the] modulo 256 sum of all the bytes
+ # from the device ID through the end of the data payload,
+ # bit inverted (1's complement)"
+ # For the sphero mini, the flag bits must be included too:
+ checksum = 0
+ for num in sendBytes[1:]:
+ checksum = (checksum + num) & 0xFF # bitwise "and to get modulo 256 sum of appropriate bytes
+ checksum = 0xff - checksum # bitwise 'not' to invert checksum bits
+ sendBytes += [checksum, sendPacketConstants["EndOfPacket"]] # concatenate
+
+ # Convert numbers to bytes
+ output = b"".join([x.to_bytes(1, byteorder='big') for x in sendBytes])
+
+ #send to specified characteristic:
+ characteristic.write(output, withResponse = True)
+
+ def getAcknowledgement(self, ack):
+ #wait up to 10 secs for correct acknowledgement to come in, including sequence number!
+ start = time.time()
+ while(1):
+ self.p.waitForNotifications(1)
+ if self.sphero_delegate.notification_seq == self.sequence-1: # use one less than sequence, because _send function increments it for next send.
+ if self.verbosity > 3:
+ print("[RESP {}] {}".format(self.sequence-1, self.sphero_delegate.notification_ack))
+ self.sphero_delegate.clear_notification()
+ break
+ elif self.sphero_delegate.notification_seq >= 0:
+ print("Unexpected ACK. Expected: {}/{}, received: {}/{}".format(
+ ack, self.sequence, self.sphero_delegate.notification_ack.split()[0],
+ self.sphero_delegate.notification_seq)
+ )
+ if time.time() > start + 10:
+ print("Timeout waiting for acknowledgement: {}/{}".format(ack, self.sequence))
+ break
+
+# =======================================================================
+# The following functions are experimental:
+# =======================================================================
+
+ def configureCollisionDetection(self,
+ xThreshold = 50,
+ yThreshold = 50,
+ xSpeed = 50,
+ ySpeed = 50,
+ deadTime = 50, # in 10 millisecond increments
+ method = 0x01, # Must be 0x01
+ callback = None):
+ '''
+ Appears to function the same as other Sphero models, however speed settings seem to have no effect.
+ NOTE: Setting to zero seems to cause bluetooth errors with the Sphero Mini/bluepy library - set to
+ 255 to make it effectively disabled.
+
+ deadTime disables future collisions for a short period of time to avoid repeat triggering by the same
+ event. Set in 10ms increments. So if deadTime = 50, that means the delay will be 500ms, or half a second.
+
+ From Sphero docs:
+
+ xThreshold/yThreshold: An 8-bit settable threshold for the X (left/right) and Y (front/back) axes
+ of Sphero.
+
+ xSpeed/ySpeed: An 8-bit settable speed value for the X and Y axes. This setting is ranged by the
+ speed, then added to xThreshold, yThreshold to generate the final threshold value.
+ '''
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring collision detection".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['configureCollision'],
+ payload=[method, xThreshold, xSpeed, yThreshold, ySpeed, deadTime])
+
+ self.collision_detection_callback = callback
+
+ self.getAcknowledgement("Collision")
+
+ def configureSensorStream(self): # Use default values
+ '''
+ Send command to configure sensor stream using default values as found during bluetooth
+ sniffing of the Sphero Edu app.
+
+ Must be called after calling configureSensorMask()
+ '''
+ bitfield1 = 0b00000000 # Unknown function - needs experimenting
+ bitfield2 = 0b00000000 # Unknown function - needs experimenting
+ bitfield3 = 0b00000000 # Unknown function - needs experimenting
+ bitfield4 = 0b00000000 # Unknown function - needs experimenting
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring sensor stream".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['configureSensorStream'],
+ payload=[bitfield1, bitfield1, bitfield1, bitfield1])
+
+ self.getAcknowledgement("Sensor")
+
+ def configureSensorMask(self,
+ sample_rate_divisor = 0x25, # Must be > 0
+ packet_count = 0,
+ IMU_pitch = False,
+ IMU_roll = False,
+ IMU_yaw = False,
+ IMU_acc_x = False,
+ IMU_acc_y = False,
+ IMU_acc_z = False,
+ IMU_gyro_x = False,
+ IMU_gyro_y = False,
+ IMU_gyro_z = False):
+
+ '''
+ Send command to configure sensor mask using default values as found during bluetooth
+ sniffing of the Sphero Edu app. From experimentation, it seems that these are he functions of each:
+
+ Sampling_rate_divisor. Slow data EG: Set to 0x32 to the divide data rate by 50. Setting below 25 (0x19) causes
+ bluetooth errors
+
+ Packet_count: Select the number of packets to transmit before ending the stream. Set to zero to stream infinitely
+
+ All IMU bool parameters: Toggle transmission of that value on or off (e.g. set IMU_acc_x = True to include the
+ X-axis accelerometer readings in the sensor stream)
+ '''
+
+ # Construct bitfields based on function parameters:
+ IMU_bitfield1 = (IMU_pitch<<2) + (IMU_roll<<1) + IMU_yaw
+ IMU_bitfield2 = ((IMU_acc_y<<7) + (IMU_acc_z<<6) + (IMU_acc_x<<5) + \
+ (IMU_gyro_y<<4) + (IMU_gyro_x<<2) + (IMU_gyro_z<<2))
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring sensor mask".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensorMask'],
+ payload=[0x00, # Unknown param - altering it seems to slow data rate. Possibly averages multiple readings?
+ sample_rate_divisor,
+ packet_count, # Packet count: select the number of packets to stop streaming after (zero = infinite)
+ 0b00, # Unknown param: seems to be another accelerometer bitfield? Z-acc, Y-acc
+ IMU_bitfield1,
+ IMU_bitfield2,
+ 0b00]) # reserved, Position?, Position?, velocity?, velocity?, Y-gyro, timer, reserved
+
+ self.getAcknowledgement("Mask")
+
+ '''
+ Since the sensor values arrive as unlabelled lists in the order that they appear in the bitfields above, we need
+ to create a list of sensors that have been configured.Once we have this list, then in the default_delegate class,
+ we can get sensor values as attributes of the sphero_mini class.
+ e.g. print(sphero.IMU_yaw) # displays the current yaw angle
+ '''
+
+ # Initialize dictionary with sensor names as keys and their bool values (set by the user) as values:
+ availableSensors = {"IMU_pitch" : IMU_pitch,
+ "IMU_roll" : IMU_roll,
+ "IMU_yaw" : IMU_yaw,
+ "IMU_acc_y" : IMU_acc_y,
+ "IMU_acc_z" : IMU_acc_z,
+ "IMU_acc_x" : IMU_acc_x,
+ "IMU_gyro_y" : IMU_gyro_y,
+ "IMU_gyro_x" : IMU_gyro_x,
+ "IMU_gyro_z" : IMU_gyro_z}
+
+ # Create list of of only sensors that have been "activated" (set as true in the method arguments):
+ self.configured_sensors = [name for name in availableSensors if availableSensors[name] == True]
+
+ def sensor1(self): # Use default values
+ '''
+ Unknown function. Observed in bluetooth sniffing.
+ '''
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensor1'],
+ payload=[0x01])
+
+ self.getAcknowledgement("Sensor1")
+
+ def sensor2(self): # Use default values
+ '''
+ Unknown function. Observed in bluetooth sniffing.
+ '''
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensor2'],
+ payload=[0x00])
+
+ self.getAcknowledgement("Sensor2")
+
+# =======================================================================
+
+class MyDelegate(btle.DefaultDelegate):
+
+ '''
+ This class handles notifications (both responses and asynchronous notifications).
+
+ Usage of this class is described in the Bluepy documentation
+
+ '''
+
+ def __init__(self, sphero_class, user_delegate):
+ self.sphero_class = sphero_class # for saving sensor values as attributes of sphero class instance
+ self.user_delegate = user_delegate # to directly notify users of callbacks
+ btle.DefaultDelegate.__init__(self)
+ self.clear_notification()
+ self.notificationPacket = []
+
+ def clear_notification(self):
+ self.notification_ack = "DEFAULT ACK"
+ self.notification_seq = -1
+
+ def bits_to_num(self, bits):
+ '''
+ This helper function decodes bytes from sensor packets into single precision floats. Encoding follows the
+ the IEEE-754 standard.
+ '''
+ num = int(bits, 2).to_bytes(len(bits) // 8, byteorder='little')
+ num = struct.unpack('f', num)[0]
+ return num
+
+ def handleNotification(self, cHandle, data):
+ '''
+ This method acts as an interrupt service routine. When a notification comes in, this
+ method is invoked, with the variable 'cHandle' being the handle of the characteristic that
+ sent the notification, and 'data' being the payload (sent one byte at a time, so the packet
+ needs to be reconstructed)
+
+ The method keeps appending bytes to the payload packet byte list until end-of-packet byte is
+ encountered. Note that this is an issue, because 0xD8 could be sent as part of the payload of,
+ say, the battery voltage notification. In future, a more sophisticated method will be required.
+ '''
+ # Allow the user to intercept and process data first..
+ if self.user_delegate != None:
+ if self.user_delegate.handleNotification(cHandle, data):
+ return
+
+ #print("Received notification with packet ", str(data))
+
+ for data_byte in data: # parse each byte separately (sometimes they arrive simultaneously)
+
+ self.notificationPacket.append(data_byte) # Add new byte to packet list
+
+ # If end of packet (need to find a better way to segment the packets):
+ if data_byte == sendPacketConstants['EndOfPacket']:
+ # Once full the packet has arrived, parse it:
+ # Packet structure is similar to the outgoing send packets (see docstring in sphero_mini._send())
+
+ # Attempt to unpack. Might fail if packet is too badly corrupted
+ try:
+ start, flags_bits, devid, commcode, seq, *notification_payload, chsum, end = self.notificationPacket
+ except ValueError:
+ print("Warning: notification packet unparseable ", self.notificationPacket)
+ self.notificationPacket = [] # Discard this packet
+ return # exit
+
+ # Compute and append checksum and add EOP byte:
+ # From Sphero docs: "The [checksum is the] modulo 256 sum of all the bytes
+ # from the device ID through the end of the data payload,
+ # bit inverted (1's complement)"
+ # For the sphero mini, the flag bits must be included too:
+ checksum_bytes = [flags_bits, devid, commcode, seq] + notification_payload
+ checksum = 0 # init
+ for num in checksum_bytes:
+ checksum = (checksum + num) & 0xFF # bitwise "and to get modulo 256 sum of appropriate bytes
+ checksum = 0xff - checksum # bitwise 'not' to invert checksum bits
+ if checksum != chsum: # check computed checksum against that recieved in the packet
+ print("Warning: notification packet checksum failed - ", str(self.notificationPacket))
+ self.notificationPacket = [] # Discard this packet
+ return # exit
+
+ # Check if response packet:
+ if flags_bits & flags['isResponse']: # it is a response
+
+ # Use device ID and command code to determine which command is being acknowledged:
+ if devid == deviceID['powerInfo'] and commcode == powerCommandIDs['wake']:
+ self.notification_ack = "Wake acknowledged" # Acknowledgement after wake command
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands['driveWithHeading']:
+ self.notification_ack = "Roll command acknowledged"
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands['stabilization']:
+ self.notification_ack = "Stabilization command acknowledged"
+
+ elif devid == deviceID['userIO'] and commcode == userIOCommandIDs['allLEDs']:
+ self.notification_ack = "LED/backlight color command acknowledged"
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands["resetHeading"]:
+ self.notification_ack = "Heading reset command acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["configureCollision"]:
+ self.notification_ack = "Collision detection configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["configureSensorStream"]:
+ self.notification_ack = "Sensor stream configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensorMask"]:
+ self.notification_ack = "Mask configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensor1"]:
+ self.notification_ack = "Sensor1 acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensor2"]:
+ self.notification_ack = "Sensor2 acknowledged"
+
+ elif devid == deviceID['powerInfo'] and commcode == powerCommandIDs['batteryVoltage']:
+ V_batt = notification_payload[2] + notification_payload[1]*256 + notification_payload[0]*65536
+ V_batt /= 100 # Notification gives V_batt in 10mV increments. Divide by 100 to get to volts.
+ self.notification_ack = "Battery voltage:" + str(V_batt) + "v"
+ self.sphero_class.v_batt = V_batt
+
+ elif devid == deviceID['systemInfo'] and commcode == SystemInfoCommands['mainApplicationVersion']:
+ version = '.'.join(str(x) for x in notification_payload)
+ self.notification_ack = "Firmware version: " + version
+ self.sphero_class.firmware_version = notification_payload
+
+ else:
+ self.notification_ack = "Unknown acknowledgement" #print(self.notificationPacket)
+ print(self.notificationPacket, "===================> Unknown ack packet")
+
+ self.notification_seq = seq
+
+ else: # Not a response packet - therefore, asynchronous notification (e.g. collision detection, etc):
+
+ # Collision detection:
+ if devid == deviceID['sensor'] and commcode == sensorCommands['collisionDetectedAsync']:
+ # The first four bytes are data that is still un-parsed. the remaining unsaved bytes are always zeros
+ _, _, _, _, _, _, axis, _, Y_mag, _, X_mag, *_ = notification_payload
+ if axis == 1:
+ dir = "Left/right"
+ else:
+ dir = 'Forward/back'
+ print("Collision detected:")
+ print("\tAxis: ", dir)
+ print("\tX_mag: ", X_mag)
+ print("\tY_mag: ", Y_mag)
+
+ if self.sphero_class.collision_detection_callback is not None:
+ self.notificationPacket = [] # need to clear packet, in case new notification comes in during callback
+ self.sphero_class.collision_detection_callback()
+
+ # Sensor response:
+ elif devid == deviceID['sensor'] and commcode == sensorCommands['sensorResponse']:
+ # Convert to binary, pad bytes with leading zeros:
+ val = ''
+ for byte in notification_payload:
+ val += format(int(bin(byte)[2:], 2), '#010b')[2:]
+
+ # Break into 32-bit chunks
+ nums = []
+ while(len(val) > 0):
+ num, val = val[:32], val[32:] # Slice off first 16 bits
+ nums.append(num)
+
+ # convert from raw bits to float:
+ nums = [self.bits_to_num(num) for num in nums]
+
+ # Set sensor values as class attributes:
+ for name, value in zip(self.sphero_class.configured_sensors, nums):
+ print("Setting sensor at .", name, str(value))
+ setattr(self.sphero_class, name, value)
+
+ # Unrecognized packet structure:
+ else:
+ self.notification_ack = "Unknown asynchronous notification" #print(self.notificationPacket)
+ print(str(self.notificationPacket) + " ===================> Unknown async packet")
+
+ self.notificationPacket = [] # Start new payload after this byte
\ No newline at end of file
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/wavefront.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/wavefront.py
new file mode 100644
index 0000000000000000000000000000000000000000..1da387793ce36c131ca7ecc83355afdd5b70da7d
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/wavefront.py
@@ -0,0 +1,154 @@
+#!/usr/bin/env python
+import time
+import sys
+
+def newSearch(mapOfWorld, goal, start):
+ heap = []
+ newheap = []
+ x, y = goal
+ lastwave = 3
+ # Start out by marking nodes around G with a 3
+ moves = [(x + 1, y), (x - 1, y), (x, y - 1), (x, y + 1)]
+
+ for move in moves:
+ if(mapOfWorld.positions[move] == ' '):
+ mapOfWorld.positions[move] = 3
+ heap.append(move)
+ for currentwave in range(4, 10000):
+ lastwave = lastwave + 1
+ while(heap != []):
+ position = heap.pop()
+ (x, y) = position
+ moves = [(x + 1, y), (x - 1, y), (x, y + 1), (x, y - 1)]
+ #x, y = position
+ for move in moves:
+ if(mapOfWorld.positions[move] != 'W'):
+ if(mapOfWorld.positions[move] == ' ' and mapOfWorld.positions[position] == currentwave - 1):
+ mapOfWorld.positions[move] = currentwave
+ newheap.append(move)
+ if(move == start):
+ return mapOfWorld, lastwave
+
+ #time.sleep(0.25)
+ #mapOfWorld.display()
+ #print heap
+ if(newheap == []):
+ print("Goal is unreachable")
+ return 1
+ heap = newheap
+ newheap = []
+
+def printf(format, *args):
+ sys.stdout.write(format % args)
+
+class Map(object):
+
+ def __init__(self, xdim, ydim, positions):
+ self.xdim = xdim
+ self.ydim = ydim
+ self.positions = positions
+ def display(self):
+ printf(" ")
+ for i in range(self.ydim):
+ printf("%3s", str(i))
+ print
+ for x in range(self.xdim):
+ printf("%2s", str(x))
+ for y in range(self.ydim):
+ printf("%3s", str(self.positions[(x, y)]))
+ print
+ # Navigate though the number-populated maze
+ def nav(self, start, current):
+ self.pos = start
+ finished = False
+
+ while(finished == False): # Run this code until we're at the goal
+ x, y = self.pos
+ self.positions[self.pos] = 'R' # Set the start on the map (this USUALLY keeps start the same)
+ # SOUTH NORTH WEST EAST
+ # v v v v
+ moves = [(x + 1, y), (x - 1, y), (x, y - 1), (x, y + 1)] # Establish our directions
+ moveDirections = ["South", "North", "West", "East"] # Create a corresponding list of the cardinal directions
+ """ We don't want least to be 0, because then nothing would be less than it.
+ However, in order to make our code more robust, we set it to one of the values,
+ so that we're comparing least to an actual value instead of an arbitrary number (like 10).
+ """
+ # Do the actual comparing, and give us the least index so we know which move was the least
+ for w in range(len(moves)):
+ move = moves[w]
+
+ # If the position has the current wave - 1 in it, move there.
+ if(self.positions[move] == current - 1):
+ self.least = self.positions[move]
+ leastIndex = w
+ # Or, if the position is the goal, stop the loop
+ elif(self.positions[move] == 'G'):
+ finished = True
+ leastIndex = w
+ # Decrement the current number so we can look for the next number
+ current = current - 1
+ self.positions[self.pos] = ' '
+ print( "Moved " + moveDirections[leastIndex])
+ self.pos = moves[leastIndex] # This will be converted to "move robot in x direction"
+
+ #time.sleep(0.25)
+ #self.display()
+ # Change the goal position (or wherever we stop) to an "!" to show that we've arrived.
+ self.positions[self.pos] = '!'
+ self.display()
+# Find the goal, given the map
+def findGoal(mapOfWorld):
+ positions = mapOfWorld.positions
+ for x in range(mapOfWorld.xdim):
+ for y in range(mapOfWorld.ydim):
+ if(mapOfWorld.positions[(x, y)] == 'G'):
+ return (x, y)
+# Find the start, given the map
+def findStart(mapOfWorld):
+ positions = mapOfWorld.positions
+ for x in range(mapOfWorld.xdim):
+ for y in range(mapOfWorld.ydim):
+ if(mapOfWorld.positions[(x, y)] == 'R'):
+
+ return (x, y)
+
+def convertMap(mapOfWorld):
+ positions = {}
+ xdim = len(mapOfWorld)
+ ydim = len(mapOfWorld[1])
+ for y in range(ydim):
+ for x in range(xdim):
+ positions[(x, y)] = mapOfWorld[x][y]
+
+ return Map(xdim, ydim, positions)
+
+# Map erstellen
+# Standard imports
+import cv2
+import numpy as np;
+
+# Img Objekt
+cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+
+success, img = cap.read()
+
+gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
+mask = cv2.inRange(gray, np.array([90]), np.array([255]))
+
+mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+
+mask_list= np.ndarray.tolist(mask)
+
+for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['W' if j > 200 else ' ' for j in mask_list[i]]
+
+mapOfWorld[200][200] = 'R' #Start Pos
+mapOfWorld[300][300] = 'G' #Ziel Pos
+
+print(mapOfWorld[200][200])
+print(mapOfWorld[300][300])
+
+mapOfLand = convertMap(mapOfWorld)
+mapOfLand.display()
+mapOfLand, lastwave = newSearch(mapOfLand, findGoal(mapOfLand), findStart(mapOfLand))
+mapOfLand.nav(findStart(mapOfLand), lastwave)
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/wavefront_coverage_path_planner.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/wavefront_coverage_path_planner.py
new file mode 100644
index 0000000000000000000000000000000000000000..85861402d61f643d8a0639c708a494cca6383170
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/wavefront_coverage_path_planner.py
@@ -0,0 +1,218 @@
+"""
+Distance/Path Transform Wavefront Coverage Path Planner
+
+author: Todd Tang
+paper: Planning paths of complete coverage of an unstructured environment
+ by a mobile robot - Zelinsky et.al.
+link: http://pinkwink.kr/attachment/cfile3.uf@1354654A4E8945BD13FE77.pdf
+"""
+
+import os
+import sys
+
+import matplotlib.pyplot as plt
+import numpy as np
+from scipy import ndimage
+
+do_animation = True
+
+
+def transform(
+ grid_map, src, distance_type='chessboard',
+ transform_type='path', alpha=0.01
+):
+ """transform
+
+ calculating transform of transform_type from src
+ in given distance_type
+
+ :param grid_map: 2d binary map
+ :param src: distance transform source
+ :param distance_type: type of distance used
+ :param transform_type: type of transform used
+ :param alpha: weight of Obstacle Transform used when using path_transform
+ """
+
+ n_rows, n_cols = grid_map.shape
+
+ if n_rows == 0 or n_cols == 0:
+ sys.exit('Empty grid_map.')
+
+ inc_order = [[0, 1], [1, 1], [1, 0], [1, -1],
+ [0, -1], [-1, -1], [-1, 0], [-1, 1]]
+ if distance_type == 'chessboard':
+ cost = [1, 1, 1, 1, 1, 1, 1, 1]
+ elif distance_type == 'eculidean':
+ cost = [1, np.sqrt(2), 1, np.sqrt(2), 1, np.sqrt(2), 1, np.sqrt(2)]
+ else:
+ sys.exit('Unsupported distance type.')
+
+ transform_matrix = float('inf') * np.ones_like(grid_map, dtype=float)
+ transform_matrix[src[0], src[1]] = 0
+ if transform_type == 'distance':
+ eT = np.zeros_like(grid_map)
+ elif transform_type == 'path':
+ eT = ndimage.distance_transform_cdt(1 - grid_map, distance_type)
+ else:
+ sys.exit('Unsupported transform type.')
+
+ # set obstacle transform_matrix value to infinity
+ for i in range(n_rows):
+ for j in range(n_cols):
+ if grid_map[i][j] == 1.0:
+ transform_matrix[i][j] = float('inf')
+ is_visited = np.zeros_like(transform_matrix, dtype=bool)
+ is_visited[src[0], src[1]] = True
+ traversal_queue = [src]
+ calculated = [(src[0] - 1) * n_cols + src[1]]
+
+ def is_valid_neighbor(g_i, g_j):
+ return 0 <= g_i < n_rows and 0 <= g_j < n_cols \
+ and not grid_map[g_i][g_j]
+
+ while traversal_queue:
+ i, j = traversal_queue.pop(0)
+ for k, inc in enumerate(inc_order):
+ ni = i + inc[0]
+ nj = j + inc[1]
+ if is_valid_neighbor(ni, nj):
+ is_visited[i][j] = True
+
+ # update transform_matrix
+ transform_matrix[i][j] = min(
+ transform_matrix[i][j],
+ transform_matrix[ni][nj] + cost[k] + alpha * eT[ni][nj])
+
+ if not is_visited[ni][nj] \
+ and ((ni - 1) * n_cols + nj) not in calculated:
+ traversal_queue.append((ni, nj))
+ calculated.append((ni - 1) * n_cols + nj)
+
+ return transform_matrix
+
+
+def get_search_order_increment(start, goal):
+ if start[0] >= goal[0] and start[1] >= goal[1]:
+ order = [[1, 0], [0, 1], [-1, 0], [0, -1],
+ [1, 1], [1, -1], [-1, 1], [-1, -1]]
+ elif start[0] <= goal[0] and start[1] >= goal[1]:
+ order = [[-1, 0], [0, 1], [1, 0], [0, -1],
+ [-1, 1], [-1, -1], [1, 1], [1, -1]]
+ elif start[0] >= goal[0] and start[1] <= goal[1]:
+ order = [[1, 0], [0, -1], [-1, 0], [0, 1],
+ [1, -1], [-1, -1], [1, 1], [-1, 1]]
+ elif start[0] <= goal[0] and start[1] <= goal[1]:
+ order = [[-1, 0], [0, -1], [0, 1], [1, 0],
+ [-1, -1], [-1, 1], [1, -1], [1, 1]]
+ else:
+ sys.exit('get_search_order_increment: cannot determine \
+ start=>goal increment order')
+ return order
+
+
+def wavefront(transform_matrix, start, goal):
+ """wavefront
+
+ performing wavefront coverage path planning
+
+ :param transform_matrix: the transform matrix
+ :param start: start point of planning
+ :param goal: goal point of planning
+ """
+
+ path = []
+ n_rows, n_cols = transform_matrix.shape
+
+ def is_valid_neighbor(g_i, g_j):
+ is_i_valid_bounded = 0 <= g_i < n_rows
+ is_j_valid_bounded = 0 <= g_j < n_cols
+ if is_i_valid_bounded and is_j_valid_bounded:
+ return not is_visited[g_i][g_j] and \
+ transform_matrix[g_i][g_j] != float('inf')
+ return False
+
+ inc_order = get_search_order_increment(start, goal)
+
+ current_node = start
+ is_visited = np.zeros_like(transform_matrix, dtype=bool)
+
+ while current_node != goal:
+ i, j = current_node
+ path.append((i, j))
+ is_visited[i][j] = True
+
+ max_T = float('-inf')
+ i_max = (-1, -1)
+ i_last = 0
+ for i_last in range(len(path)):
+ current_node = path[-1 - i_last] # get latest node in path
+ for ci, cj in inc_order:
+ ni, nj = current_node[0] + ci, current_node[1] + cj
+ if is_valid_neighbor(ni, nj) and \
+ transform_matrix[ni][nj] > max_T:
+ i_max = (ni, nj)
+ max_T = transform_matrix[ni][nj]
+
+ if i_max != (-1, -1):
+ break
+
+ if i_max == (-1, -1):
+ break
+ else:
+ current_node = i_max
+ if i_last != 0:
+ print('backtracing to', current_node)
+ path.append(goal)
+
+ return path
+
+
+def visualize_path(grid_map, start, goal, path): # pragma: no cover
+ oy, ox = start
+ gy, gx = goal
+ px, py = np.transpose(np.flipud(np.fliplr(path)))
+
+ if not do_animation:
+ plt.imshow(grid_map, cmap='Greys')
+ plt.plot(ox, oy, "-xy")
+ plt.plot(px, py, "-r")
+ plt.plot(gx, gy, "-pg")
+ plt.show()
+ else:
+ for ipx, ipy in zip(px, py):
+ plt.cla()
+ # for stopping simulation with the esc key.
+ plt.gcf().canvas.mpl_connect(
+ 'key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
+ plt.imshow(grid_map, cmap='Greys')
+ plt.plot(ox, oy, "-xb")
+ plt.plot(px, py, "-r")
+ plt.plot(gx, gy, "-pg")
+ plt.plot(ipx, ipy, "or")
+ plt.axis("equal")
+ plt.grid(True)
+ plt.pause(0.1)
+
+
+def main():
+ dir_path = os.path.dirname(os.path.realpath(__file__))
+ img = plt.imread(os.path.join(dir_path, 'map', 'test_2.png'))
+ img = 1 - img # revert pixel values
+
+ start = (43, 0)
+ goal = (0, 0)
+
+ # distance transform wavefront
+ DT = transform(img, goal, transform_type='distance')
+ DT_path = wavefront(DT, start, goal)
+ visualize_path(img, start, goal, DT_path)
+
+ # path transform wavefront
+ PT = transform(img, goal, transform_type='path', alpha=0.01)
+ PT_path = wavefront(PT, start, goal)
+ visualize_path(img, start, goal, PT_path)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/wavefrontgpt2.py b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/wavefrontgpt2.py
new file mode 100644
index 0000000000000000000000000000000000000000..ef76c9f457ef6872c3b242d9b6815864c9ed7a5c
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/wavefrontgpt2.py
@@ -0,0 +1,76 @@
+from queue import Queue
+
+def wavefront_planner(map_array, start_pos, goal_pos):
+ rows = len(map_array)
+ cols = len(map_array[0])
+
+ # Überprüfe, ob Start- und Zielposition innerhalb der Karte liegen
+ if (start_pos[0] < 0 or start_pos[0] >= rows or start_pos[1] < 0 or start_pos[1] >= cols or
+ goal_pos[0] < 0 or goal_pos[0] >= rows or goal_pos[1] < 0 or goal_pos[1] >= cols):
+ raise ValueError("Start or goal position is out of bounds.")
+
+ # Erzeuge eine Kopie der Karte für den Wavefront-Algorithmus
+ wavefront_map = [[-1] * cols for _ in range(rows)]
+
+ # Definiere die Bewegungsrichtungen (4-Wege-Bewegung: oben, unten, links, rechts)
+ directions = [(-1, 0), (1, 0), (0, -1), (0, 1)]
+
+ # Erzeuge eine Warteschlange für die Wellenfrontausbreitung
+ queue = Queue()
+ queue.put(goal_pos)
+
+ # Führe die Wellenfrontausbreitung durch
+ wavefront_map[goal_pos[0]][goal_pos[1]] = 0
+ while not queue.empty():
+ current_pos = queue.get()
+
+ # Überprüfe die Nachbarzellen
+ for direction in directions:
+ new_pos = (current_pos[0] + direction[0], current_pos[1] + direction[1])
+
+ # Überprüfe, ob die Nachbarzelle gültig und noch nicht besucht ist
+ if (0 <= new_pos[0] < rows and 0 <= new_pos[1] < cols and
+ wavefront_map[new_pos[0]][new_pos[1]] == -1 and map_array[new_pos[0]][new_pos[1]] == 0):
+ wavefront_map[new_pos[0]][new_pos[1]] = wavefront_map[current_pos[0]][current_pos[1]] + 1
+ queue.put(new_pos)
+
+ # Überprüfe, ob der Startpunkt erreichbar ist
+ if wavefront_map[start_pos[0]][start_pos[1]] == -1:
+ raise ValueError("Start position is unreachable.")
+
+ # Verfolge den Pfad basierend auf der Wellenfront
+ path = [start_pos]
+ current_pos = start_pos
+ while current_pos != goal_pos:
+ next_pos = None
+ min_distance = float('inf')
+
+ for direction in directions:
+ neighbor_pos = (current_pos[0] + direction[0], current_pos[1] + direction[1])
+
+ if (0 <= neighbor_pos[0] < rows and 0 <= neighbor_pos[1] < cols and
+ wavefront_map[neighbor_pos[0]][neighbor_pos[1]] < min_distance):
+ next_pos = neighbor_pos
+ min_distance = wavefront_map[neighbor_pos[0]][neighbor_pos[1]]
+
+ if next_pos is None:
+ raise ValueError("No path found.")
+
+ path.append(next_pos)
+ current_pos = next_pos
+
+ return path
+
+# Beispielverwendung
+map_array = [
+ [0, 0, 0, 0],
+ [0, 1, 1, 0],
+ [0, 0, 0, 0],
+ [0, 1, 1, 0],
+]
+
+start_pos = (0, 0)
+goal_pos = (1, 3)
+
+path = wavefront_planner(map_array, start_pos, goal_pos)
+print("Path:", path)
diff --git a/ros2_ws/install/sphero_mini_controller/lib/sphero_mini_controller/sphero_mini b/ros2_ws/install/sphero_mini_controller/lib/sphero_mini_controller/sphero_mini
new file mode 100644
index 0000000000000000000000000000000000000000..01ae998b4f9a5df3b55c3a10b2f5bbbc5fc171e8
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/sphero_mini_controller/sphero_mini
@@ -0,0 +1,33 @@
+#!/usr/bin/python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'sphero-mini-controller==0.0.0','console_scripts','sphero_mini'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'sphero-mini-controller==0.0.0'
+
+try:
+ from importlib.metadata import distribution
+except ImportError:
+ try:
+ from importlib_metadata import distribution
+ except ImportError:
+ from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+ dist_name, _, _ = spec.partition('==')
+ matches = (
+ entry_point
+ for entry_point in distribution(dist_name).entry_points
+ if entry_point.group == group and entry_point.name == name
+ )
+ return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+ sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+ sys.exit(load_entry_point('sphero-mini-controller==0.0.0', 'console_scripts', 'sphero_mini')())
diff --git a/ros2_ws/install/sphero_mini_controller/lib/sphero_mini_controller/test_node b/ros2_ws/install/sphero_mini_controller/lib/sphero_mini_controller/test_node
new file mode 100644
index 0000000000000000000000000000000000000000..42b5555ce7a5ab5c9ccab6e382562d82c4e11a22
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/lib/sphero_mini_controller/test_node
@@ -0,0 +1,33 @@
+#!/usr/bin/python3
+# EASY-INSTALL-ENTRY-SCRIPT: 'sphero-mini-controller==0.0.0','console_scripts','test_node'
+import re
+import sys
+
+# for compatibility with easy_install; see #2198
+__requires__ = 'sphero-mini-controller==0.0.0'
+
+try:
+ from importlib.metadata import distribution
+except ImportError:
+ try:
+ from importlib_metadata import distribution
+ except ImportError:
+ from pkg_resources import load_entry_point
+
+
+def importlib_load_entry_point(spec, group, name):
+ dist_name, _, _ = spec.partition('==')
+ matches = (
+ entry_point
+ for entry_point in distribution(dist_name).entry_points
+ if entry_point.group == group and entry_point.name == name
+ )
+ return next(matches).load()
+
+
+globals().setdefault('load_entry_point', importlib_load_entry_point)
+
+
+if __name__ == '__main__':
+ sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
+ sys.exit(load_entry_point('sphero-mini-controller==0.0.0', 'console_scripts', 'test_node')())
diff --git a/ros2_ws/install/sphero_mini_controller/share/ament_index/resource_index/packages/sphero_mini_controller b/ros2_ws/install/sphero_mini_controller/share/ament_index/resource_index/packages/sphero_mini_controller
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2_ws/install/sphero_mini_controller/share/colcon-core/packages/sphero_mini_controller b/ros2_ws/install/sphero_mini_controller/share/colcon-core/packages/sphero_mini_controller
new file mode 100644
index 0000000000000000000000000000000000000000..4fb1cbf964f229c49b5b6b9bf5ce6c251451f9e1
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/colcon-core/packages/sphero_mini_controller
@@ -0,0 +1 @@
+_constants:bluepy:bluepy.btle:box:cv2:geometry_msgs.msg:matplotlib.pyplot:numpy:queue:rclpy:sensor_msgs.msg:struct:sys:threading:time:treiber
\ No newline at end of file
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.dsv b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.dsv
new file mode 100644
index 0000000000000000000000000000000000000000..79d4c95b55cb72a17c9be498c3758478e2c7bb8d
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.dsv
@@ -0,0 +1 @@
+prepend-non-duplicate;AMENT_PREFIX_PATH;
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.ps1 b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.ps1
new file mode 100644
index 0000000000000000000000000000000000000000..26b99975794bb42ea3d6a17150e313cbfc45fc24
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.ps1
@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value AMENT_PREFIX_PATH "$env:COLCON_CURRENT_PREFIX"
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.sh b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.sh
new file mode 100644
index 0000000000000000000000000000000000000000..f3041f688a623ea5c66e65c917bc503e5fae6dc9
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.sh
@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value AMENT_PREFIX_PATH "$COLCON_CURRENT_PREFIX"
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.dsv b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.dsv
new file mode 100644
index 0000000000000000000000000000000000000000..257067d44dde1ca226afa26a3def27b5e9ee7cbb
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.dsv
@@ -0,0 +1 @@
+prepend-non-duplicate;PYTHONPATH;lib/python3.10/site-packages
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.ps1 b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.ps1
new file mode 100644
index 0000000000000000000000000000000000000000..caffe83fd5f7176279ca966b9f2d43b5e275c9b2
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.ps1
@@ -0,0 +1,3 @@
+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\lib/python3.10/site-packages"
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.sh b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.sh
new file mode 100644
index 0000000000000000000000000000000000000000..660c34836dfb678ad2d087f61a86a435b6715d2b
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.sh
@@ -0,0 +1,3 @@
+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value PYTHONPATH "$COLCON_CURRENT_PREFIX/lib/python3.10/site-packages"
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.bash b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.bash
new file mode 100644
index 0000000000000000000000000000000000000000..ba62e9b8979eb16b0f9c9dc4aedaca7582d2d4a3
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.bash
@@ -0,0 +1,31 @@
+# generated from colcon_bash/shell/template/package.bash.em
+
+# This script extends the environment for this package.
+
+# a bash script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ # the prefix is two levels up from the package specific share directory
+ _colcon_package_bash_COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." > /dev/null && pwd)"
+else
+ _colcon_package_bash_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_bash_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo ". \"$1\""
+ fi
+ . "$@"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source sh script of this package
+_colcon_package_bash_source_script "$_colcon_package_bash_COLCON_CURRENT_PREFIX/share/sphero_mini_controller/package.sh"
+
+unset _colcon_package_bash_source_script
+unset _colcon_package_bash_COLCON_CURRENT_PREFIX
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.dsv b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.dsv
new file mode 100644
index 0000000000000000000000000000000000000000..2e5c1c2d2b0a1c60797a05178ac6529a7239d39d
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.dsv
@@ -0,0 +1,6 @@
+source;share/sphero_mini_controller/hook/pythonpath.ps1
+source;share/sphero_mini_controller/hook/pythonpath.dsv
+source;share/sphero_mini_controller/hook/pythonpath.sh
+source;share/sphero_mini_controller/hook/ament_prefix_path.ps1
+source;share/sphero_mini_controller/hook/ament_prefix_path.dsv
+source;share/sphero_mini_controller/hook/ament_prefix_path.sh
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.ps1 b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.ps1
new file mode 100644
index 0000000000000000000000000000000000000000..66a433ec81a81ddcc339ec1919cdbad24df67c36
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.ps1
@@ -0,0 +1,116 @@
+# generated from colcon_powershell/shell/template/package.ps1.em
+
+# function to append a value to a variable
+# which uses colons as separators
+# duplicates as well as leading separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_append_unique_value {
+ param (
+ $_listname,
+ $_value
+ )
+
+ # get values from variable
+ if (Test-Path Env:$_listname) {
+ $_values=(Get-Item env:$_listname).Value
+ } else {
+ $_values=""
+ }
+ $_duplicate=""
+ # start with no values
+ $_all_values=""
+ # iterate over existing values in the variable
+ if ($_values) {
+ $_values.Split(";") | ForEach {
+ # not an empty string
+ if ($_) {
+ # not a duplicate of _value
+ if ($_ -eq $_value) {
+ $_duplicate="1"
+ }
+ if ($_all_values) {
+ $_all_values="${_all_values};$_"
+ } else {
+ $_all_values="$_"
+ }
+ }
+ }
+ }
+ # append only non-duplicates
+ if (!$_duplicate) {
+ # avoid leading separator
+ if ($_all_values) {
+ $_all_values="${_all_values};${_value}"
+ } else {
+ $_all_values="${_value}"
+ }
+ }
+
+ # export the updated variable
+ Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+function colcon_prepend_unique_value {
+ param (
+ $_listname,
+ $_value
+ )
+
+ # get values from variable
+ if (Test-Path Env:$_listname) {
+ $_values=(Get-Item env:$_listname).Value
+ } else {
+ $_values=""
+ }
+ # start with the new value
+ $_all_values="$_value"
+ # iterate over existing values in the variable
+ if ($_values) {
+ $_values.Split(";") | ForEach {
+ # not an empty string
+ if ($_) {
+ # not a duplicate of _value
+ if ($_ -ne $_value) {
+ # keep non-duplicate values
+ $_all_values="${_all_values};$_"
+ }
+ }
+ }
+ }
+ # export the updated variable
+ Set-Item env:\$_listname -Value "$_all_values"
+}
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+function colcon_package_source_powershell_script {
+ param (
+ $_colcon_package_source_powershell_script
+ )
+ # source script with conditional trace output
+ if (Test-Path $_colcon_package_source_powershell_script) {
+ if ($env:COLCON_TRACE) {
+ echo ". '$_colcon_package_source_powershell_script'"
+ }
+ . "$_colcon_package_source_powershell_script"
+ } else {
+ Write-Error "not found: '$_colcon_package_source_powershell_script'"
+ }
+}
+
+
+# a powershell script is able to determine its own path
+# the prefix is two levels up from the package specific share directory
+$env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.FullName
+
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/sphero_mini_controller/hook/pythonpath.ps1"
+colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX\share/sphero_mini_controller/hook/ament_prefix_path.ps1"
+
+Remove-Item Env:\COLCON_CURRENT_PREFIX
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.sh b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.sh
new file mode 100644
index 0000000000000000000000000000000000000000..ccf55cf346e95c4a74b8a9920895946946143fa7
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.sh
@@ -0,0 +1,87 @@
+# generated from colcon_core/shell/template/package.sh.em
+
+# This script extends the environment for this package.
+
+# function to prepend a value to a variable
+# which uses colons as separators
+# duplicates as well as trailing separators are avoided
+# first argument: the name of the result variable
+# second argument: the value to be prepended
+_colcon_prepend_unique_value() {
+ # arguments
+ _listname="$1"
+ _value="$2"
+
+ # get values from variable
+ eval _values=\"\$$_listname\"
+ # backup the field separator
+ _colcon_prepend_unique_value_IFS=$IFS
+ IFS=":"
+ # start with the new value
+ _all_values="$_value"
+ # workaround SH_WORD_SPLIT not being set in zsh
+ if [ "$(command -v colcon_zsh_convert_to_array)" ]; then
+ colcon_zsh_convert_to_array _values
+ fi
+ # iterate over existing values in the variable
+ for _item in $_values; do
+ # ignore empty strings
+ if [ -z "$_item" ]; then
+ continue
+ fi
+ # ignore duplicates of _value
+ if [ "$_item" = "$_value" ]; then
+ continue
+ fi
+ # keep non-duplicate values
+ _all_values="$_all_values:$_item"
+ done
+ unset _item
+ # restore the field separator
+ IFS=$_colcon_prepend_unique_value_IFS
+ unset _colcon_prepend_unique_value_IFS
+ # export the updated variable
+ eval export $_listname=\"$_all_values\"
+ unset _all_values
+ unset _values
+
+ unset _value
+ unset _listname
+}
+
+# since a plain shell script can't determine its own path when being sourced
+# either use the provided COLCON_CURRENT_PREFIX
+# or fall back to the build time prefix (if it exists)
+_colcon_package_sh_COLCON_CURRENT_PREFIX="/home/ubuntu/ros2_ws/install/sphero_mini_controller"
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ if [ ! -d "$_colcon_package_sh_COLCON_CURRENT_PREFIX" ]; then
+ echo "The build time path \"$_colcon_package_sh_COLCON_CURRENT_PREFIX\" doesn't exist. Either source a script for a different shell or set the environment variable \"COLCON_CURRENT_PREFIX\" explicitly." 1>&2
+ unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+ return 1
+ fi
+ COLCON_CURRENT_PREFIX="$_colcon_package_sh_COLCON_CURRENT_PREFIX"
+fi
+unset _colcon_package_sh_COLCON_CURRENT_PREFIX
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_sh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo "# . \"$1\""
+ fi
+ . "$@"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# source sh hooks
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/sphero_mini_controller/hook/pythonpath.sh"
+_colcon_package_sh_source_script "$COLCON_CURRENT_PREFIX/share/sphero_mini_controller/hook/ament_prefix_path.sh"
+
+unset _colcon_package_sh_source_script
+unset COLCON_CURRENT_PREFIX
+
+# do not unset _colcon_prepend_unique_value since it might be used by non-primary shell hooks
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.xml b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..abe0e77d18dda0c557f4e3de050a4bd1f9d4ffc4
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>sphero_mini_controller</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
+ <license>TODO: License declaration</license>
+
+ <depend>rclpy</depend>
+ <depend>sensor_msgs.msg</depend>
+ <depend>geometry_msgs.msg</depend>
+ <depend>box</depend>
+ <depend>numpy</depend>
+ <depend>treiber</depend>
+ <depend>bluepy.btle</depend>
+ <depend>bluepy</depend>
+ <depend>_constants</depend>
+ <depend>struct</depend>
+ <depend>time</depend>
+ <depend>sys</depend>
+ <depend>threading</depend>
+ <depend>cv2</depend>
+ <depend>queue</depend>
+ <depend>matplotlib.pyplot</depend>
+
+ <test_depend>ament_copyright</test_depend>
+ <test_depend>ament_flake8</test_depend>
+ <test_depend>ament_pep257</test_depend>
+ <test_depend>python3-pytest</test_depend>
+
+ <export>
+ <build_type>ament_python</build_type>
+ </export>
+</package>
diff --git a/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.zsh b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.zsh
new file mode 100644
index 0000000000000000000000000000000000000000..4229ad78a6154edcea070a12484f10a3844976a2
--- /dev/null
+++ b/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.zsh
@@ -0,0 +1,42 @@
+# generated from colcon_zsh/shell/template/package.zsh.em
+
+# This script extends the environment for this package.
+
+# a zsh script is able to determine its own path if necessary
+if [ -z "$COLCON_CURRENT_PREFIX" ]; then
+ # the prefix is two levels up from the package specific share directory
+ _colcon_package_zsh_COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)"
+else
+ _colcon_package_zsh_COLCON_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
+fi
+
+# function to source another script with conditional trace output
+# first argument: the path of the script
+# additional arguments: arguments to the script
+_colcon_package_zsh_source_script() {
+ if [ -f "$1" ]; then
+ if [ -n "$COLCON_TRACE" ]; then
+ echo ". \"$1\""
+ fi
+ . "$@"
+ else
+ echo "not found: \"$1\"" 1>&2
+ fi
+}
+
+# function to convert array-like strings into arrays
+# to workaround SH_WORD_SPLIT not being set
+colcon_zsh_convert_to_array() {
+ local _listname=$1
+ local _dollar="$"
+ local _split="{="
+ local _to_array="(\"$_dollar$_split$_listname}\")"
+ eval $_listname=$_to_array
+}
+
+# source sh script of this package
+_colcon_package_zsh_source_script "$_colcon_package_zsh_COLCON_CURRENT_PREFIX/share/sphero_mini_controller/package.sh"
+unset convert_zsh_to_array
+
+unset _colcon_package_zsh_source_script
+unset _colcon_package_zsh_COLCON_CURRENT_PREFIX
diff --git a/ros2_ws/log/COLCON_IGNORE b/ros2_ws/log/COLCON_IGNORE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2_ws/log/build_2023-07-05_16-11-36/events.log b/ros2_ws/log/build_2023-07-05_16-11-36/events.log
new file mode 100644
index 0000000000000000000000000000000000000000..b51af15d2c6aeb5902f174f4d5b3b2642ab55014
--- /dev/null
+++ b/ros2_ws/log/build_2023-07-05_16-11-36/events.log
@@ -0,0 +1,38 @@
+[0.000000] (-) TimerEvent: {}
+[0.000094] (sphero_mini_controller) JobQueued: {'identifier': 'sphero_mini_controller', 'dependencies': OrderedDict()}
+[0.000145] (sphero_mini_controller) JobStarted: {'identifier': 'sphero_mini_controller'}
+[0.098612] (-) TimerEvent: {}
+[0.198900] (-) TimerEvent: {}
+[0.299181] (-) TimerEvent: {}
+[0.399461] (-) TimerEvent: {}
+[0.499733] (-) TimerEvent: {}
+[0.600065] (-) TimerEvent: {}
+[0.613340] (sphero_mini_controller) Command: {'cmd': ['/usr/bin/python3', 'setup.py', 'egg_info', '--egg-base', '../../build/sphero_mini_controller', 'build', '--build-base', '/home/ubuntu/ros2_ws/build/sphero_mini_controller/build', 'install', '--record', '/home/ubuntu/ros2_ws/build/sphero_mini_controller/install.log', '--single-version-externally-managed'], 'cwd': '/home/ubuntu/ros2_ws/src/sphero_mini_controller', 'env': {'LANGUAGE': 'de_DE:en', 'USER': 'ubuntu', 'LC_TIME': 'de_DE.UTF-8', 'XDG_SESSION_TYPE': 'x11', 'SHLVL': '1', 'LD_LIBRARY_PATH': '/opt/ros/humble/opt/rviz_ogre_vendor/lib:/opt/ros/humble/lib/x86_64-linux-gnu:/opt/ros/humble/lib', 'HOME': '/home/ubuntu', 'OLDPWD': '/home/ubuntu', 'DESKTOP_SESSION': 'ubuntu', 'ROS_PYTHON_VERSION': '3', 'GNOME_SHELL_SESSION_MODE': 'ubuntu', 'GTK_MODULES': 'gail:atk-bridge', 'LC_MONETARY': 'de_DE.UTF-8', 'SYSTEMD_EXEC_PID': '2511', 'DBUS_SESSION_BUS_ADDRESS': 'unix:path=/run/user/999/bus', 'COLORTERM': 'truecolor', 'IM_CONFIG_PHASE': '1', 'ROS_DISTRO': 'humble', 'LOGNAME': 'ubuntu', '_': '/usr/bin/colcon', 'ROS_VERSION': '2', 'XDG_SESSION_CLASS': 'user', 'USERNAME': 'ubuntu', 'TERM': 'xterm-256color', 'GNOME_DESKTOP_SESSION_ID': 'this-is-deprecated', 'ROS_LOCALHOST_ONLY': '0', 'WINDOWPATH': '2', 'PATH': '/opt/ros/humble/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:/snap/bin', 'SESSION_MANAGER': 'local/ubuntu:@/tmp/.ICE-unix/2486,unix/ubuntu:/tmp/.ICE-unix/2486', 'PAPERSIZE': 'a4', 'XDG_MENU_PREFIX': 'gnome-', 'LC_ADDRESS': 'de_DE.UTF-8', 'GNOME_TERMINAL_SCREEN': '/org/gnome/Terminal/screen/e134bf1a_3465_43a9_88f4_21d27316a303', 'XDG_RUNTIME_DIR': '/run/user/999', 'DISPLAY': ':0', 'LANG': 'de_DE.UTF-8', 'XDG_CURRENT_DESKTOP': 'ubuntu:GNOME', 'LC_TELEPHONE': 'de_DE.UTF-8', 'XMODIFIERS': '@im=ibus', 'XDG_SESSION_DESKTOP': 'ubuntu', 'XAUTHORITY': '/run/user/999/gdm/Xauthority', 'GNOME_TERMINAL_SERVICE': ':1.129', 'SSH_AGENT_LAUNCHER': 'gnome-keyring', 'SSH_AUTH_SOCK': '/run/user/999/keyring/ssh', 'AMENT_PREFIX_PATH': '/opt/ros/humble', 'SHELL': '/bin/bash', 'LC_NAME': 'de_DE.UTF-8', 'QT_ACCESSIBILITY': '1', 'GDMSESSION': 'ubuntu', 'WEBOTS_HOME': '/usr/local/webots', 'LC_MEASUREMENT': 'de_DE.UTF-8', 'GPG_AGENT_INFO': '/run/user/999/gnupg/S.gpg-agent:0:1', 'LC_IDENTIFICATION': 'de_DE.UTF-8', 'QT_IM_MODULE': 'ibus', 'PWD': '/home/ubuntu/ros2_ws/build/sphero_mini_controller', 'XDG_CONFIG_DIRS': '/etc/xdg/xdg-ubuntu:/etc/xdg', 'XDG_DATA_DIRS': '/usr/share/ubuntu:/usr/share/gnome:/usr/local/share/:/usr/share/:/var/lib/snapd/desktop', 'PYTHONPATH': '/home/ubuntu/ros2_ws/build/sphero_mini_controller/prefix_override:/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages:/opt/ros/humble/lib/python3.10/site-packages:/opt/ros/humble/local/lib/python3.10/dist-packages', 'LC_NUMERIC': 'de_DE.UTF-8', 'LC_PAPER': 'de_DE.UTF-8', 'COLCON': '1', 'VTE_VERSION': '6800'}, 'shell': False}
+[0.700191] (-) TimerEvent: {}
+[0.800481] (-) TimerEvent: {}
+[0.837582] (sphero_mini_controller) StdoutLine: {'line': b'running egg_info\n'}
+[0.838134] (sphero_mini_controller) StdoutLine: {'line': b'writing ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/PKG-INFO\n'}
+[0.838269] (sphero_mini_controller) StdoutLine: {'line': b'writing dependency_links to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/dependency_links.txt\n'}
+[0.838506] (sphero_mini_controller) StdoutLine: {'line': b'writing entry points to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/entry_points.txt\n'}
+[0.838674] (sphero_mini_controller) StdoutLine: {'line': b'writing requirements to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/requires.txt\n'}
+[0.838808] (sphero_mini_controller) StdoutLine: {'line': b'writing top-level names to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/top_level.txt\n'}
+[0.840404] (sphero_mini_controller) StdoutLine: {'line': b"reading manifest file '../../build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt'\n"}
+[0.841553] (sphero_mini_controller) StdoutLine: {'line': b"writing manifest file '../../build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt'\n"}
+[0.841722] (sphero_mini_controller) StdoutLine: {'line': b'running build\n'}
+[0.841821] (sphero_mini_controller) StdoutLine: {'line': b'running build_py\n'}
+[0.842619] (sphero_mini_controller) StdoutLine: {'line': b'copying sphero_mini_controller/my_first_node.py -> /home/ubuntu/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller\n'}
+[0.843943] (sphero_mini_controller) StdoutLine: {'line': b'running install\n'}
+[0.844204] (sphero_mini_controller) StdoutLine: {'line': b'running install_lib\n'}
+[0.844828] (sphero_mini_controller) StdoutLine: {'line': b'copying /home/ubuntu/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node.py -> /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller\n'}
+[0.846179] (sphero_mini_controller) StdoutLine: {'line': b'byte-compiling /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node.py to my_first_node.cpython-310.pyc\n'}
+[0.850045] (sphero_mini_controller) StdoutLine: {'line': b'running install_data\n'}
+[0.851406] (sphero_mini_controller) StdoutLine: {'line': b'running install_egg_info\n'}
+[0.852604] (sphero_mini_controller) StdoutLine: {'line': b"removing '/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info' (and everything under it)\n"}
+[0.852894] (sphero_mini_controller) StdoutLine: {'line': b'Copying ../../build/sphero_mini_controller/sphero_mini_controller.egg-info to /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info\n'}
+[0.853897] (sphero_mini_controller) StdoutLine: {'line': b'running install_scripts\n'}
+[0.869686] (sphero_mini_controller) StdoutLine: {'line': b'Installing sphero_mini script to /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/sphero_mini_controller\n'}
+[0.870533] (sphero_mini_controller) StdoutLine: {'line': b"writing list of installed files to '/home/ubuntu/ros2_ws/build/sphero_mini_controller/install.log'\n"}
+[0.894265] (sphero_mini_controller) CommandEnded: {'returncode': 0}
+[0.900811] (-) TimerEvent: {}
+[0.911226] (sphero_mini_controller) JobEnded: {'identifier': 'sphero_mini_controller', 'rc': 0}
+[0.911779] (-) EventReactorShutdown: {}
diff --git a/ros2_ws/log/build_2023-07-05_16-11-36/logger_all.log b/ros2_ws/log/build_2023-07-05_16-11-36/logger_all.log
new file mode 100644
index 0000000000000000000000000000000000000000..79ba79dcfe52c9e6f9790fbc93f4d081a7b27893
--- /dev/null
+++ b/ros2_ws/log/build_2023-07-05_16-11-36/logger_all.log
@@ -0,0 +1,126 @@
+[0.285s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build']
+[0.285s] DEBUG:colcon:Parsed command line arguments: Namespace(log_base=None, log_level=None, verb_name='build', build_base='build', install_base='install', merge_install=False, symlink_install=False, test_result_base=None, continue_on_error=False, executor='parallel', parallel_workers=8, event_handlers=None, ignore_user_meta=False, metas=['./colcon.meta'], base_paths=['.'], packages_ignore=None, packages_ignore_regex=None, paths=None, packages_up_to=None, packages_up_to_regex=None, packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_select_by_dep=None, packages_skip_by_dep=None, packages_skip_up_to=None, packages_select_build_failed=False, packages_skip_build_finished=False, packages_select_test_failures=False, packages_skip_test_passed=False, packages_select=None, packages_skip=None, packages_select_regex=None, packages_skip_regex=None, packages_start=None, packages_end=None, allow_overriding=[], cmake_args=None, cmake_target=None, cmake_target_skip_unavailable=False, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, ament_cmake_args=None, catkin_cmake_args=None, catkin_skip_building_tests=False, verb_parser=<colcon_defaults.argument_parser.defaults.DefaultArgumentsDecorator object at 0x7f34ebc57490>, verb_extension=<colcon_core.verb.build.BuildVerb object at 0x7f34ebc56f20>, main=<bound method BuildVerb.main of <colcon_core.verb.build.BuildVerb object at 0x7f34ebc56f20>>)
+[0.322s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters
+[0.322s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters
+[0.322s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters
+[0.322s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters
+[0.322s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover
+[0.322s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover
+[0.322s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/ubuntu/ros2_ws'
+[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install']
+[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore'
+[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install'
+[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg']
+[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg'
+[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta']
+[0.322s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta'
+[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros']
+[0.323s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros'
+[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python']
+[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake'
+[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python'
+[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py']
+[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py'
+[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install']
+[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore'
+[0.332s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install']
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore'
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install']
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore'
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ignore', 'ignore_ament_install']
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore'
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ignore_ament_install'
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_pkg']
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_pkg'
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['colcon_meta']
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'colcon_meta'
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['ros']
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'ros'
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['cmake', 'python']
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'cmake'
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python'
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extensions ['python_setup_py']
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src) by extension 'python_setup_py'
+[0.333s] Level 1:colcon.colcon_core.package_identification:_identify(src/sphero_mini_controller) by extensions ['ignore', 'ignore_ament_install']
+[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/sphero_mini_controller) by extension 'ignore'
+[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/sphero_mini_controller) by extension 'ignore_ament_install'
+[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/sphero_mini_controller) by extensions ['colcon_pkg']
+[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/sphero_mini_controller) by extension 'colcon_pkg'
+[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/sphero_mini_controller) by extensions ['colcon_meta']
+[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/sphero_mini_controller) by extension 'colcon_meta'
+[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/sphero_mini_controller) by extensions ['ros']
+[0.334s] Level 1:colcon.colcon_core.package_identification:_identify(src/sphero_mini_controller) by extension 'ros'
+[0.338s] DEBUG:colcon.colcon_core.package_identification:Package 'src/sphero_mini_controller' with type 'ros.ament_python' and name 'sphero_mini_controller'
+[0.338s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults
+[0.338s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover
+[0.338s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults
+[0.338s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover
+[0.338s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults
+[0.359s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters
+[0.359s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover
+[0.364s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 302 installed packages in /opt/ros/humble
+[0.365s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults
+[0.418s] Level 5:colcon.colcon_core.verb:set package 'sphero_mini_controller' build argument 'cmake_args' from command line to 'None'
+[0.418s] Level 5:colcon.colcon_core.verb:set package 'sphero_mini_controller' build argument 'cmake_target' from command line to 'None'
+[0.418s] Level 5:colcon.colcon_core.verb:set package 'sphero_mini_controller' build argument 'cmake_target_skip_unavailable' from command line to 'False'
+[0.418s] Level 5:colcon.colcon_core.verb:set package 'sphero_mini_controller' build argument 'cmake_clean_cache' from command line to 'False'
+[0.418s] Level 5:colcon.colcon_core.verb:set package 'sphero_mini_controller' build argument 'cmake_clean_first' from command line to 'False'
+[0.418s] Level 5:colcon.colcon_core.verb:set package 'sphero_mini_controller' build argument 'cmake_force_configure' from command line to 'False'
+[0.418s] Level 5:colcon.colcon_core.verb:set package 'sphero_mini_controller' build argument 'ament_cmake_args' from command line to 'None'
+[0.418s] Level 5:colcon.colcon_core.verb:set package 'sphero_mini_controller' build argument 'catkin_cmake_args' from command line to 'None'
+[0.418s] Level 5:colcon.colcon_core.verb:set package 'sphero_mini_controller' build argument 'catkin_skip_building_tests' from command line to 'False'
+[0.418s] DEBUG:colcon.colcon_core.verb:Building package 'sphero_mini_controller' with the following arguments: {'ament_cmake_args': None, 'build_base': '/home/ubuntu/ros2_ws/build/sphero_mini_controller', 'catkin_cmake_args': None, 'catkin_skip_building_tests': False, 'cmake_args': None, 'cmake_clean_cache': False, 'cmake_clean_first': False, 'cmake_force_configure': False, 'cmake_target': None, 'cmake_target_skip_unavailable': False, 'install_base': '/home/ubuntu/ros2_ws/install/sphero_mini_controller', 'merge_install': False, 'path': '/home/ubuntu/ros2_ws/src/sphero_mini_controller', 'symlink_install': False, 'test_result_base': None}
+[0.419s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor
+[0.421s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete
+[0.421s] INFO:colcon.colcon_ros.task.ament_python.build:Building ROS package in '/home/ubuntu/ros2_ws/src/sphero_mini_controller' with build type 'ament_python'
+[0.421s] Level 1:colcon.colcon_core.shell:create_environment_hook('sphero_mini_controller', 'ament_prefix_path')
+[0.427s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems
+[0.427s] INFO:colcon.colcon_core.shell:Creating environment hook '/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.ps1'
+[0.428s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.dsv'
+[0.429s] INFO:colcon.colcon_core.shell:Creating environment hook '/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/ament_prefix_path.sh'
+[0.431s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
+[0.431s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
+[0.671s] INFO:colcon.colcon_core.task.python.build:Building Python package in '/home/ubuntu/ros2_ws/src/sphero_mini_controller'
+[0.672s] INFO:colcon.colcon_core.shell:Skip shell extension 'powershell' for command environment: Not usable outside of PowerShell
+[0.672s] DEBUG:colcon.colcon_core.shell:Skip shell extension 'dsv' for command environment
+[1.036s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoking command in '/home/ubuntu/ros2_ws/src/sphero_mini_controller': PYTHONPATH=/home/ubuntu/ros2_ws/build/sphero_mini_controller/prefix_override:/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 setup.py egg_info --egg-base ../../build/sphero_mini_controller build --build-base /home/ubuntu/ros2_ws/build/sphero_mini_controller/build install --record /home/ubuntu/ros2_ws/build/sphero_mini_controller/install.log --single-version-externally-managed
+[1.317s] DEBUG:colcon.colcon_core.event_handler.log_command:Invoked command in '/home/ubuntu/ros2_ws/src/sphero_mini_controller' returned '0': PYTHONPATH=/home/ubuntu/ros2_ws/build/sphero_mini_controller/prefix_override:/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 setup.py egg_info --egg-base ../../build/sphero_mini_controller build --build-base /home/ubuntu/ros2_ws/build/sphero_mini_controller/build install --record /home/ubuntu/ros2_ws/build/sphero_mini_controller/install.log --single-version-externally-managed
+[1.323s] Level 1:colcon.colcon_core.environment:checking '/home/ubuntu/ros2_ws/install/sphero_mini_controller' for CMake module files
+[1.324s] Level 1:colcon.colcon_core.environment:checking '/home/ubuntu/ros2_ws/install/sphero_mini_controller' for CMake config files
+[1.325s] Level 1:colcon.colcon_core.environment:checking '/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib'
+[1.325s] Level 1:colcon.colcon_core.environment:checking '/home/ubuntu/ros2_ws/install/sphero_mini_controller/bin'
+[1.325s] Level 1:colcon.colcon_core.environment:checking '/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/pkgconfig/sphero_mini_controller.pc'
+[1.325s] Level 1:colcon.colcon_core.environment:checking '/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages'
+[1.325s] Level 1:colcon.colcon_core.shell:create_environment_hook('sphero_mini_controller', 'pythonpath')
+[1.327s] INFO:colcon.colcon_core.shell:Creating environment hook '/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.ps1'
+[1.327s] INFO:colcon.colcon_core.shell:Creating environment descriptor '/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.dsv'
+[1.327s] INFO:colcon.colcon_core.shell:Creating environment hook '/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/hook/pythonpath.sh'
+[1.328s] Level 1:colcon.colcon_core.environment:checking '/home/ubuntu/ros2_ws/install/sphero_mini_controller/bin'
+[1.328s] Level 1:colcon.colcon_core.environment:create_environment_scripts_only(sphero_mini_controller)
+[1.329s] INFO:colcon.colcon_core.shell:Creating package script '/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.ps1'
+[1.330s] INFO:colcon.colcon_core.shell:Creating package descriptor '/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.dsv'
+[1.331s] INFO:colcon.colcon_core.shell:Creating package script '/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.sh'
+[1.331s] INFO:colcon.colcon_core.shell:Creating package script '/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.bash'
+[1.332s] INFO:colcon.colcon_core.shell:Creating package script '/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/sphero_mini_controller/package.zsh'
+[1.333s] Level 1:colcon.colcon_core.environment:create_file_with_runtime_dependencies(/home/ubuntu/ros2_ws/install/sphero_mini_controller/share/colcon-core/packages/sphero_mini_controller)
+[1.333s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop
+[1.334s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed
+[1.334s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0'
+[1.334s] DEBUG:colcon.colcon_core.event_reactor:joining thread
+[1.341s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems
+[1.341s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems
+[1.341s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2'
+[1.356s] DEBUG:colcon.colcon_core.event_reactor:joined thread
+[1.358s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/ubuntu/ros2_ws/install/local_setup.ps1'
+[1.359s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/ubuntu/ros2_ws/install/_local_setup_util_ps1.py'
+[1.361s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/ubuntu/ros2_ws/install/setup.ps1'
+[1.362s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/ubuntu/ros2_ws/install/local_setup.sh'
+[1.363s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/ubuntu/ros2_ws/install/_local_setup_util_sh.py'
+[1.363s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/ubuntu/ros2_ws/install/setup.sh'
+[1.365s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/ubuntu/ros2_ws/install/local_setup.bash'
+[1.366s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/ubuntu/ros2_ws/install/setup.bash'
+[1.367s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/ubuntu/ros2_ws/install/local_setup.zsh'
+[1.368s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/ubuntu/ros2_ws/install/setup.zsh'
diff --git a/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/command.log b/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/command.log
new file mode 100644
index 0000000000000000000000000000000000000000..4f3d645a95ba6297270da17046f956b89cf8380d
--- /dev/null
+++ b/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/command.log
@@ -0,0 +1,2 @@
+Invoking command in '/home/ubuntu/ros2_ws/src/sphero_mini_controller': PYTHONPATH=/home/ubuntu/ros2_ws/build/sphero_mini_controller/prefix_override:/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 setup.py egg_info --egg-base ../../build/sphero_mini_controller build --build-base /home/ubuntu/ros2_ws/build/sphero_mini_controller/build install --record /home/ubuntu/ros2_ws/build/sphero_mini_controller/install.log --single-version-externally-managed
+Invoked command in '/home/ubuntu/ros2_ws/src/sphero_mini_controller' returned '0': PYTHONPATH=/home/ubuntu/ros2_ws/build/sphero_mini_controller/prefix_override:/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 setup.py egg_info --egg-base ../../build/sphero_mini_controller build --build-base /home/ubuntu/ros2_ws/build/sphero_mini_controller/build install --record /home/ubuntu/ros2_ws/build/sphero_mini_controller/install.log --single-version-externally-managed
diff --git a/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/stderr.log b/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/stderr.log
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/stdout.log b/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/stdout.log
new file mode 100644
index 0000000000000000000000000000000000000000..e0f7d75bc0319cdf3b92b2221e732950b19b046e
--- /dev/null
+++ b/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/stdout.log
@@ -0,0 +1,22 @@
+running egg_info
+writing ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/PKG-INFO
+writing dependency_links to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/dependency_links.txt
+writing entry points to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/entry_points.txt
+writing requirements to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/requires.txt
+writing top-level names to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/top_level.txt
+reading manifest file '../../build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt'
+writing manifest file '../../build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt'
+running build
+running build_py
+copying sphero_mini_controller/my_first_node.py -> /home/ubuntu/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller
+running install
+running install_lib
+copying /home/ubuntu/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node.py -> /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller
+byte-compiling /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node.py to my_first_node.cpython-310.pyc
+running install_data
+running install_egg_info
+removing '/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info' (and everything under it)
+Copying ../../build/sphero_mini_controller/sphero_mini_controller.egg-info to /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info
+running install_scripts
+Installing sphero_mini script to /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/sphero_mini_controller
+writing list of installed files to '/home/ubuntu/ros2_ws/build/sphero_mini_controller/install.log'
diff --git a/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/stdout_stderr.log b/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/stdout_stderr.log
new file mode 100644
index 0000000000000000000000000000000000000000..e0f7d75bc0319cdf3b92b2221e732950b19b046e
--- /dev/null
+++ b/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/stdout_stderr.log
@@ -0,0 +1,22 @@
+running egg_info
+writing ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/PKG-INFO
+writing dependency_links to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/dependency_links.txt
+writing entry points to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/entry_points.txt
+writing requirements to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/requires.txt
+writing top-level names to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/top_level.txt
+reading manifest file '../../build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt'
+writing manifest file '../../build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt'
+running build
+running build_py
+copying sphero_mini_controller/my_first_node.py -> /home/ubuntu/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller
+running install
+running install_lib
+copying /home/ubuntu/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node.py -> /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller
+byte-compiling /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node.py to my_first_node.cpython-310.pyc
+running install_data
+running install_egg_info
+removing '/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info' (and everything under it)
+Copying ../../build/sphero_mini_controller/sphero_mini_controller.egg-info to /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info
+running install_scripts
+Installing sphero_mini script to /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/sphero_mini_controller
+writing list of installed files to '/home/ubuntu/ros2_ws/build/sphero_mini_controller/install.log'
diff --git a/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/streams.log b/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/streams.log
new file mode 100644
index 0000000000000000000000000000000000000000..c3cd89373169e0c66b063bac913fdb295878e014
--- /dev/null
+++ b/ros2_ws/log/build_2023-07-05_16-11-36/sphero_mini_controller/streams.log
@@ -0,0 +1,24 @@
+[0.614s] Invoking command in '/home/ubuntu/ros2_ws/src/sphero_mini_controller': PYTHONPATH=/home/ubuntu/ros2_ws/build/sphero_mini_controller/prefix_override:/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 setup.py egg_info --egg-base ../../build/sphero_mini_controller build --build-base /home/ubuntu/ros2_ws/build/sphero_mini_controller/build install --record /home/ubuntu/ros2_ws/build/sphero_mini_controller/install.log --single-version-externally-managed
+[0.838s] running egg_info
+[0.838s] writing ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/PKG-INFO
+[0.838s] writing dependency_links to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/dependency_links.txt
+[0.838s] writing entry points to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/entry_points.txt
+[0.839s] writing requirements to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/requires.txt
+[0.839s] writing top-level names to ../../build/sphero_mini_controller/sphero_mini_controller.egg-info/top_level.txt
+[0.840s] reading manifest file '../../build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt'
+[0.841s] writing manifest file '../../build/sphero_mini_controller/sphero_mini_controller.egg-info/SOURCES.txt'
+[0.842s] running build
+[0.842s] running build_py
+[0.843s] copying sphero_mini_controller/my_first_node.py -> /home/ubuntu/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller
+[0.844s] running install
+[0.844s] running install_lib
+[0.845s] copying /home/ubuntu/ros2_ws/build/sphero_mini_controller/build/lib/sphero_mini_controller/my_first_node.py -> /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller
+[0.846s] byte-compiling /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller/my_first_node.py to my_first_node.cpython-310.pyc
+[0.850s] running install_data
+[0.851s] running install_egg_info
+[0.853s] removing '/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info' (and everything under it)
+[0.853s] Copying ../../build/sphero_mini_controller/sphero_mini_controller.egg-info to /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages/sphero_mini_controller-0.0.0-py3.10.egg-info
+[0.854s] running install_scripts
+[0.870s] Installing sphero_mini script to /home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/sphero_mini_controller
+[0.870s] writing list of installed files to '/home/ubuntu/ros2_ws/build/sphero_mini_controller/install.log'
+[0.894s] Invoked command in '/home/ubuntu/ros2_ws/src/sphero_mini_controller' returned '0': PYTHONPATH=/home/ubuntu/ros2_ws/build/sphero_mini_controller/prefix_override:/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages:${PYTHONPATH} /usr/bin/python3 setup.py egg_info --egg-base ../../build/sphero_mini_controller build --build-base /home/ubuntu/ros2_ws/build/sphero_mini_controller/build install --record /home/ubuntu/ros2_ws/build/sphero_mini_controller/install.log --single-version-externally-managed
diff --git a/ros2_ws/log/latest b/ros2_ws/log/latest
new file mode 100644
index 0000000000000000000000000000000000000000..3244609549432b76ee2b151871ad47cc1e1f0f2d
Binary files /dev/null and b/ros2_ws/log/latest differ
diff --git a/ros2_ws/log/latest_build b/ros2_ws/log/latest_build
new file mode 100644
index 0000000000000000000000000000000000000000..ca67e1aa835f2f7f207f34c5b460777363f7af47
Binary files /dev/null and b/ros2_ws/log/latest_build differ
diff --git a/ros2_ws/src/sphero_mini_controller/.vscode/c_cpp_properties.json b/ros2_ws/src/sphero_mini_controller/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000000000000000000000000000000000000..af5a8df28dd58b3113f960a5986bbe0612c4a286
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/.vscode/c_cpp_properties.json
@@ -0,0 +1,20 @@
+{
+ "configurations": [
+ {
+ "browse": {
+ "databaseFilename": "${default}",
+ "limitSymbolsToIncludedHeaders": false
+ },
+ "includePath": [
+ "/opt/ros/humble/include/**",
+ "/usr/include/**"
+ ],
+ "name": "ROS",
+ "intelliSenseMode": "gcc-x64",
+ "compilerPath": "/usr/bin/gcc",
+ "cStandard": "gnu11",
+ "cppStandard": "c++14"
+ }
+ ],
+ "version": 4
+}
\ No newline at end of file
diff --git a/ros2_ws/src/sphero_mini_controller/.vscode/settings.json b/ros2_ws/src/sphero_mini_controller/.vscode/settings.json
new file mode 100644
index 0000000000000000000000000000000000000000..2acdf0c3dc18f14096514c0a8cba87088761a3cf
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/.vscode/settings.json
@@ -0,0 +1,13 @@
+{
+ "python.autoComplete.extraPaths": [
+ "/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages",
+ "/opt/ros/humble/lib/python3.10/site-packages",
+ "/opt/ros/humble/local/lib/python3.10/dist-packages"
+ ],
+ "python.analysis.extraPaths": [
+ "/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages",
+ "/opt/ros/humble/lib/python3.10/site-packages",
+ "/opt/ros/humble/local/lib/python3.10/dist-packages"
+ ],
+ "ros.distro": "humble"
+}
\ No newline at end of file
diff --git a/ros2_ws/src/sphero_mini_controller/package.xml b/ros2_ws/src/sphero_mini_controller/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..abe0e77d18dda0c557f4e3de050a4bd1f9d4ffc4
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/package.xml
@@ -0,0 +1,35 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+ <name>sphero_mini_controller</name>
+ <version>0.0.0</version>
+ <description>TODO: Package description</description>
+ <maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
+ <license>TODO: License declaration</license>
+
+ <depend>rclpy</depend>
+ <depend>sensor_msgs.msg</depend>
+ <depend>geometry_msgs.msg</depend>
+ <depend>box</depend>
+ <depend>numpy</depend>
+ <depend>treiber</depend>
+ <depend>bluepy.btle</depend>
+ <depend>bluepy</depend>
+ <depend>_constants</depend>
+ <depend>struct</depend>
+ <depend>time</depend>
+ <depend>sys</depend>
+ <depend>threading</depend>
+ <depend>cv2</depend>
+ <depend>queue</depend>
+ <depend>matplotlib.pyplot</depend>
+
+ <test_depend>ament_copyright</test_depend>
+ <test_depend>ament_flake8</test_depend>
+ <test_depend>ament_pep257</test_depend>
+ <test_depend>python3-pytest</test_depend>
+
+ <export>
+ <build_type>ament_python</build_type>
+ </export>
+</package>
diff --git a/ros2_ws/src/sphero_mini_controller/resource/sphero_conf.json b/ros2_ws/src/sphero_mini_controller/resource/sphero_conf.json
new file mode 100644
index 0000000000000000000000000000000000000000..a490b4b798c696938452dac610e3cfac087c6279
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/resource/sphero_conf.json
@@ -0,0 +1,3 @@
+{
+ "MAC_ADDRESS": "C6:69:72:CD:BC:6D"
+}
diff --git a/ros2_ws/src/sphero_mini_controller/resource/sphero_mini_controller b/ros2_ws/src/sphero_mini_controller/resource/sphero_mini_controller
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2_ws/src/sphero_mini_controller/setup.cfg b/ros2_ws/src/sphero_mini_controller/setup.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..54dced33ec1eaa5f3aa8fec3c45d4ccc9afdc2a0
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/sphero_mini_controller
+[install]
+install_scripts=$base/lib/sphero_mini_controller
diff --git a/ros2_ws/src/sphero_mini_controller/setup.py b/ros2_ws/src/sphero_mini_controller/setup.py
new file mode 100644
index 0000000000000000000000000000000000000000..60d245ad79ffb3a6cf68f974fd2806e38e9b957a
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/setup.py
@@ -0,0 +1,27 @@
+from setuptools import setup
+
+package_name = 'sphero_mini_controller'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=[package_name],
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='ubuntu',
+ maintainer_email='ubuntu@todo.todo',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ "sphero_mini = sphero_mini_controller.sphero_mini:main"
+
+ ],
+ },
+)
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/.vscode/c_cpp_properties.json b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000000000000000000000000000000000000..af5a8df28dd58b3113f960a5986bbe0612c4a286
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/.vscode/c_cpp_properties.json
@@ -0,0 +1,20 @@
+{
+ "configurations": [
+ {
+ "browse": {
+ "databaseFilename": "${default}",
+ "limitSymbolsToIncludedHeaders": false
+ },
+ "includePath": [
+ "/opt/ros/humble/include/**",
+ "/usr/include/**"
+ ],
+ "name": "ROS",
+ "intelliSenseMode": "gcc-x64",
+ "compilerPath": "/usr/bin/gcc",
+ "cStandard": "gnu11",
+ "cppStandard": "c++14"
+ }
+ ],
+ "version": 4
+}
\ No newline at end of file
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/.vscode/settings.json b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/.vscode/settings.json
new file mode 100644
index 0000000000000000000000000000000000000000..c7c0d891180b53a0c36591808f3201d9656bd763
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/.vscode/settings.json
@@ -0,0 +1,12 @@
+{
+ "python.autoComplete.extraPaths": [
+ "/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages",
+ "/opt/ros/humble/lib/python3.10/site-packages",
+ "/opt/ros/humble/local/lib/python3.10/dist-packages"
+ ],
+ "python.analysis.extraPaths": [
+ "/home/ubuntu/ros2_ws/install/sphero_mini_controller/lib/python3.10/site-packages",
+ "/opt/ros/humble/lib/python3.10/site-packages",
+ "/opt/ros/humble/local/lib/python3.10/dist-packages"
+ ]
+}
\ No newline at end of file
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/WavefrontPlanner.py b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/WavefrontPlanner.py
new file mode 100644
index 0000000000000000000000000000000000000000..9904dd94b1bd926376bc75e671e1687a80a9df4f
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/WavefrontPlanner.py
@@ -0,0 +1,389 @@
+############################################################################
+# WAVEFRONT ALGORITHM
+# Adapted to Python Code By Darin Velarde
+# Fri Jan 29 13:56:53 PST 2010
+# from C code from John Palmisano
+# (www.societyofrobots.com)
+############################################################################
+import cv2
+import numpy as np
+import matplotlib.pyplot as plt
+import time
+
+class waveFrontPlanner:
+ ############################################################################
+ # WAVEFRONT ALGORITHM
+ # Adapted to Python Code By Darin Velarde
+ # Fri Jan 29 13:56:53 PST 2010
+ # from C code from John Palmisano
+ # (www.societyofrobots.com)
+ ############################################################################
+
+ def __init__(self, mapOfWorld, slow=False):
+ self.__slow = slow
+ self.__mapOfWorld = mapOfWorld
+ if str(type(mapOfWorld)).find("numpy") != -1:
+ #If its a numpy array
+ self.__height, self.__width = self.__mapOfWorld.shape
+ else:
+ self.__height, self.__width = len(self.__mapOfWorld), len(self.__mapOfWorld[0])
+
+ self.__nothing = 000
+ self.__wall = 999
+ self.__goal = 1
+ self.__path = "PATH"
+
+ self.__finalPath = []
+
+ #Robot value
+ self.__robot = 254
+ #Robot default Location
+ self.__robot_x = 0
+ self.__robot_y = 0
+
+ #default goal location
+ self.__goal_x = 8
+ self.__goal_y = 9
+
+ #temp variables
+ self.__temp_A = 0
+ self.__temp_B = 0
+ self.__counter = 0
+ self.__steps = 0 #determine how processor intensive the algorithm was
+
+ #when searching for a node with a lower value
+ self.__minimum_node = 250
+ self.__min_node_location = 250
+ self.__new_state = 1
+ self.__old_state = 1
+ self.__reset_min = 250 #above this number is a special (wall or robot)
+ ###########################################################################
+
+ def setRobotPosition(self, x, y):
+ """
+ Sets the robot's current position
+
+ """
+
+ self.__robot_x = x
+ self.__robot_y = y
+ ###########################################################################
+
+ def setGoalPosition(self, x, y):
+ """
+ Sets the goal position.
+
+ """
+
+ self.__goal_x = x
+ self.__goal_y = y
+ ###########################################################################
+
+ def robotPosition(self):
+ return (self.__robot_x, self.__robot_y)
+ ###########################################################################
+
+ def goalPosition(self):
+ return (self.__goal_x, self.__goal_y)
+ ###########################################################################
+
+ def run(self, prnt=False):
+ """
+ The entry point for the robot algorithm to use wavefront propagation.
+
+ """
+
+ path = []
+ while self.__mapOfWorld[self.__robot_x][self.__robot_y] != self.__goal:
+ if self.__steps > 20000:
+ print ("Cannot find a path.")
+ return
+ #find new location to go to
+ self.__new_state = self.propagateWavefront()
+ #update location of robot
+ if self.__new_state == 1:
+ self.__robot_x -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" % (self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 2:
+ self.__robot_y += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 3:
+ self.__robot_x += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 4:
+ self.__robot_y -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ self.__old_state = self.__new_state
+ msg = "Found the goal in %i steps:\n" % self.__steps
+ msg += "mapOfWorld size= %i %i\n\n" % (self.__height, self.__width)
+ if prnt:
+ print(msg)
+ self.printMap()
+ return path
+ ###########################################################################
+
+ def propagateWavefront(self, prnt=False):
+ self.unpropagate()
+ #Old robot location was deleted, store new robot location in mapOfWorld
+ self.__mapOfWorld[self.__robot_x][self.__robot_y] = self.__robot
+ self.__path = self.__robot
+ #start location to begin scan at goal location
+ self.__mapOfWorld[self.__goal_x][self.__goal_y] = self.__goal
+ counter = 0
+ while counter < 200: #allows for recycling until robot is found
+ x = 0
+ y = 0
+ #time.sleep(0.00001)
+ #while the mapOfWorld hasnt been fully scanned
+ while x < self.__height and y < self.__width:
+ #if this location is a wall or the goal, just ignore it
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal:
+ #a full trail to the robot has been located, finished!
+ minLoc = self.minSurroundingNodeValue(x, y)
+ if minLoc < self.__reset_min and \
+ self.__mapOfWorld[x][y] == self.__robot:
+ if prnt:
+ print("Finished Wavefront:\n")
+ self.printMap()
+ # Tell the robot to move after this return.
+ return self.__min_node_location
+ #record a value in to this node
+ elif self.__minimum_node != self.__reset_min:
+ #if this isnt here, 'nothing' will go in the location
+ self.__mapOfWorld[x][y] = self.__minimum_node + 1
+ #go to next node and/or row
+ y += 1
+ if y == self.__width and x != self.__height:
+ x += 1
+ y = 0
+ #print self.__robot_x, self.__robot_y
+ if prnt:
+ print("Sweep #: %i\n" % (counter + 1))
+ self.printMap()
+ self.__steps += 1
+ counter += 1
+ return 0
+ ###########################################################################
+
+ def unpropagate(self):
+ """
+ clears old path to determine new path
+ stay within boundary
+
+ """
+
+ for x in range(0, self.__height):
+ for y in range(0, self.__width):
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal and \
+ self.__mapOfWorld[x][y] != self.__path:
+ #if this location is a wall or goal, just ignore it
+ self.__mapOfWorld[x][y] = self.__nothing #clear that space
+ ###########################################################################
+
+ def minSurroundingNodeValue(self, x, y):
+ """
+ this method looks at a node and returns the lowest value around that
+ node.
+
+ """
+
+ #reset minimum
+ self.__minimum_node = self.__reset_min
+ #down
+ if x < self.__height -1:
+ if self.__mapOfWorld[x + 1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x + 1][y] != self.__nothing:
+ #find the lowest number node, and exclude empty nodes (0's)
+ self.__minimum_node = self.__mapOfWorld[x + 1][y]
+ self.__min_node_location = 3
+ #up
+ if x > 0:
+ if self.__mapOfWorld[x-1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x-1][y] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x-1][y]
+ self.__min_node_location = 1
+ #right
+ if y < self.__width -1:
+ if self.__mapOfWorld[x][y + 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y + 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y + 1]
+ self.__min_node_location = 2
+ #left
+ if y > 0:
+ if self.__mapOfWorld[x][y - 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y - 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y-1]
+ self.__min_node_location = 4
+ return self.__minimum_node
+ ###########################################################################
+
+ def printMap(self):
+ """
+ Prints out the map of this instance of the class.
+
+ """
+
+ msg = ''
+ for temp_B in range(0, self.__height):
+ for temp_A in range(0, self.__width):
+ if self.__mapOfWorld[temp_B][temp_A] == self.__wall:
+ msg += "%04s" % "[#]"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__robot:
+ msg += "%04s" % "-"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__goal:
+ msg += "%04s" % "G"
+ else:
+ msg += "%04s" % str(self.__mapOfWorld[temp_B][temp_A])
+ msg += "\n\n"
+ msg += "\n\n"
+ print(msg)
+ #
+ if self.__slow == True:
+ time.sleep(0.05)
+############################################################################
+
+class mapCreate:
+ ############################################################################
+
+ def create_map(self, scale,img):
+
+ print('Original Dimensions : ',img.shape)
+
+ scale_percent = 100-scale # percent of original size
+ width = int(img.shape[1] * scale_percent / 100)
+ height = int(img.shape[0] * scale_percent / 100)
+ dim = (width, height)
+
+ # resize image
+ resized = cv2.resize(img, dim, interpolation = cv2.INTER_AREA)
+ print('Resized Dimensions : ',resized.shape)
+
+ gray = cv2.cvtColor(resized, cv2.COLOR_RGB2GRAY)
+ mask = cv2.inRange(gray, np.array([200]), np.array([255]))
+
+ mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+ mask_list= np.ndarray.tolist(mask)
+
+ #Markiere alle leeren Zellen mit 000 und alle Zellen mit Hinderniss mit 999
+ for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['999' if j > 200 else '000' for j in mask_list[i]]
+
+ mapOfWorld = np.array(mapOfWorld, dtype = int)
+ mapOfWorldSized = mapOfWorld.copy()
+
+ e = 3
+ #Hindernisse Größer machen
+ for i in range(0,mapOfWorld.shape[0]):
+ for j in range(0,mapOfWorld.shape[1]):
+ for r in range(0,e):
+ try:
+ if (mapOfWorldSized[i][j] == 999):
+ mapOfWorld[i][j+e] = 999
+ mapOfWorld[i+e][j] = 999
+ mapOfWorld[i-e][j] = 999
+ mapOfWorld[i][j-e] = 999
+ mapOfWorld[i+e][j+e] = 999
+ mapOfWorld[i+e][j-e] = 999
+ mapOfWorld[i-e][j+e] = 999
+ mapOfWorld[i-e][j-e] = 999
+ except Exception:
+ continue
+
+ return mapOfWorld
+ ############################################################################
+
+ def scale_koord(self, sx, sy, gx, gy,scale):
+ scale_percent = 100-scale # percent of original size
+
+ sx = int(sx*scale_percent/100)
+ sy = int(sy*scale_percent/100)
+ gx = int(gx*scale_percent/100)
+ gy = int(gy*scale_percent/100)
+
+ return sx,sy,gx,gy
+ ############################################################################
+
+ def rescale_koord(self, path,scale):
+ scale_percent = 100-scale # percent of original size100
+
+ path = np.array(path)
+
+ for i in range(len(path)):
+ path[i] = path[i]*100/scale_percent
+
+ return path
+ ############################################################################
+
+ def calc_trans(self, x, y, height):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+###############################################################################
+
+if __name__ == "__main__":
+ """
+ X is vertical, Y is horizontal
+
+ """
+
+ Map = mapCreate() #Map Objekt erzeugen
+
+ #Verkleinerungsfaktor in %
+ scale = 90
+
+ img = cv2.imread("D:/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/karte.jpg", cv2.COLOR_RGB2GRAY)
+ #cap = cv2.VideoCapture(2)
+ mapWorld = Map.create_map(scale, img)
+ height, width = img.shape[:2]
+
+ #Angaben in Weltkoordinaten
+ sy = 300 #X-Koordinate im Weltkoordinaten System
+ sx = 200 #Y-Koordinate im Weltkoordinaten System
+
+ gy = 700 #X-Koordinate im Weltkordinaten System
+ gx = 240 #Y-Koordinate im Weltkoordinaten System
+
+ sx,sy,gx,gy = Map.scale_koord(sx,sy,gx,gy,scale) #Kordinaten Tauschen X,Y
+
+ start = time.time()
+ planner = waveFrontPlanner(mapWorld)
+ planner.setGoalPosition(gx,gy)
+ planner.setRobotPosition(sx,sy)
+ path = planner.run(False)
+ end = time.time()
+ print("Took %f seconds to run wavefront simulation" % (end - start))
+
+ path = Map.rescale_koord(path, scale)
+ #print(path)
+#%% Plot
+ #Programm Koordinaten
+ imgTrans = cv2.transpose(img) # X und Y-Achse im Bild tauschen
+ imgPlot, ax1 = plt.subplots()
+
+ ax1.set_title('Programm Koordinaten')
+ ax1.imshow(imgTrans)
+ ax1.set_xlabel('[px]')
+ ax1.set_ylabel('[px]')
+ ax1.scatter(path[:,0], path[:,1], color='r')
+
+ #Bild Koordinaten
+ imgPlot2, ax2 = plt.subplots()
+ ax2.set_title('Bild Koordinaten')
+ ax2.set_xlabel('[px]')
+ ax2.set_ylabel('[px]')
+ ax2.imshow(img)
+ ax2.scatter(path[:,1], path[:,0], color='r')
+
+#%% Sphero Pfad
+ pathWeltX, pathWeltY = Map.calc_trans(path[:,1], path[:,0], height)
\ No newline at end of file
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/__init__.py b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/__init__.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/__pycache__/_constants.cpython-310.pyc b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/__pycache__/_constants.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..7a7dc990c992e83d0131942566db7b7d4f80a057
Binary files /dev/null and b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/__pycache__/_constants.cpython-310.pyc differ
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/__pycache__/core.cpython-310.pyc b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/__pycache__/core.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..268efa0a4ba0ab4916f275a675473967f0d05ef1
Binary files /dev/null and b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/__pycache__/core.cpython-310.pyc differ
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/__pycache__/simple_moves.cpython-310.pyc b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/__pycache__/simple_moves.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..1aac01a6d648c63bd3f5ffbf474fde28d479087f
Binary files /dev/null and b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/__pycache__/simple_moves.cpython-310.pyc differ
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/_constants.py b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/_constants.py
new file mode 100644
index 0000000000000000000000000000000000000000..fa8a0e875eed9f484945c1eeaf2fda3fd98503a4
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/_constants.py
@@ -0,0 +1,72 @@
+'''
+Known Peripheral UUIDs, obtained by querying using the Bluepy module:
+=====================================================================
+Anti DOS Characteristic <00020005-574f-4f20-5370-6865726f2121>
+Battery Level Characteristic <Battery Level>
+Peripheral Preferred Connection Parameters Characteristic <Peripheral Preferred Connection Parameters>
+API V2 Characteristic <00010002-574f-4f20-5370-6865726f2121>
+DFU Control Characteristic <00020002-574f-4f20-5370-6865726f2121>
+Name Characteristic <Device Name>
+Appearance Characteristic <Appearance>
+DFU Info Characteristic <00020004-574f-4f20-5370-6865726f2121>
+Service Changed Characteristic <Service Changed>
+Unknown1 Characteristic <00020003-574f-4f20-5370-6865726f2121>
+Unknown2 Characteristic <00010003-574f-4f20-5370-6865726f2121>
+
+The rest of the values saved in the dictionaries below, were borrowed from
+@igbopie's javacript library, which is available at https://github.com/igbopie/spherov2.js
+
+'''
+
+deviceID = {"apiProcessor": 0x10, # 16
+ "systemInfo": 0x11, # 17
+ "powerInfo": 0x13, # 19
+ "driving": 0x16, # 22
+ "animatronics": 0x17, # 23
+ "sensor": 0x18, # 24
+ "something": 0x19, # 25
+ "userIO": 0x1a, # 26
+ "somethingAPI": 0x1f} # 31
+
+SystemInfoCommands = {"mainApplicationVersion": 0x00, # 00
+ "bootloaderVersion": 0x01, # 01
+ "something": 0x06, # 06
+ "something2": 0x13, # 19
+ "something6": 0x12, # 18
+ "something7": 0x28} # 40
+
+sendPacketConstants = {"StartOfPacket": 0x8d, # 141
+ "EndOfPacket": 0xd8} # 216
+
+userIOCommandIDs = {"allLEDs": 0x0e} # 14
+
+flags= {"isResponse": 0x01, # 0x01
+ "requestsResponse": 0x02, # 0x02
+ "requestsOnlyErrorResponse": 0x04, # 0x04
+ "resetsInactivityTimeout": 0x08} # 0x08
+
+powerCommandIDs={"deepSleep": 0x00, # 0
+ "sleep": 0x01, # 01
+ "batteryVoltage": 0x03, # 03
+ "wake": 0x0D, # 13
+ "something": 0x05, # 05
+ "something2": 0x10, # 16
+ "something3": 0x04, # 04
+ "something4": 0x1E} # 30
+
+drivingCommands={"rawMotor": 0x01, # 1
+ "resetHeading": 0x06, # 6
+ "driveAsSphero": 0x04, # 4
+ "driveAsRc": 0x02, # 2
+ "driveWithHeading": 0x07, # 7
+ "stabilization": 0x0C} # 12
+
+sensorCommands={'sensorMask': 0x00, # 00
+ 'sensorResponse': 0x02, # 02
+ 'configureCollision': 0x11, # 17
+ 'collisionDetectedAsync': 0x12, # 18
+ 'resetLocator': 0x13, # 19
+ 'enableCollisionAsync': 0x14, # 20
+ 'sensor1': 0x0F, # 15
+ 'sensor2': 0x17, # 23
+ 'configureSensorStream': 0x0C} # 12
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/blobErkennung.py b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/blobErkennung.py
new file mode 100644
index 0000000000000000000000000000000000000000..4b0c0cd6c73880c65396252247ee3ad195d155cb
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/blobErkennung.py
@@ -0,0 +1,66 @@
+#!/usr/bin/env python3
+
+# Standard imports
+import cv2
+import numpy as np;
+
+# Img Objekt
+#cap = cv2.VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+cap = cv2.VideoCapture(4)
+
+# Set up the detector with default parameters.
+detector = cv2.SimpleBlobDetector()
+
+# Setup SimpleBlobDetector parameters.
+params = cv2.SimpleBlobDetector_Params()
+
+# Change thresholds
+#params.minThreshold = 5
+#params.maxThreshold = 500
+
+#FIlter by Color
+params.filterByColor = True
+params.blobColor = 255
+
+# Filter by Area.
+params.filterByArea = True
+params.minArea = 20
+params.maxArea = 200
+
+# Filter by Circularity
+#params.filterByCircularity = True
+#params.minCircularity = 0.5
+#params.maxCircularity = 1
+
+# Filter by Convexity
+#params.filterByConvexity = True
+#params.minConvexity = 0.7
+#params.maxConvexity = 1
+
+# Filter by Inertia
+params.filterByInertia = True
+params.minInertiaRatio = 0.5
+
+# Create a detector with the parameters
+detector = cv2.SimpleBlobDetector_create(params)
+
+while True:
+
+ success, img = cap.read()
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+
+ # Detect blobs.
+ keypoints = detector.detect(gray)
+
+ #print(keypoints)
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+
+ im_with_keypoints = cv2.drawKeypoints(gray, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
+
+ # Show blobs
+ cv2.imshow("Keypoints", im_with_keypoints)
+
+ if cv2.waitKey(10) & 0xFF == ord('q'):
+ break
\ No newline at end of file
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/coreROS2.py b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/coreROS2.py
new file mode 100644
index 0000000000000000000000000000000000000000..ec3435051de5aa72fc393d224b0fe1a53927b8b0
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/coreROS2.py
@@ -0,0 +1,638 @@
+from bluepy.btle import Peripheral
+from bluepy import btle
+from sphero_mini_controller._constants import *
+import struct
+import time
+import sys
+
+class SpheroMini():
+ def __init__(self, MACAddr, verbosity = 4, user_delegate = None):
+ '''
+ initialize class instance and then build collect BLE sevices and characteristics.
+ Also sends text string to Anti-DOS characteristic to prevent returning to sleep,
+ and initializes notifications (which is what the sphero uses to send data back to
+ the client).
+ '''
+ self.verbosity = verbosity # 0 = Silent,
+ # 1 = Connection/disconnection only
+ # 2 = Init messages
+ # 3 = Recieved commands
+ # 4 = Acknowledgements
+ self.sequence = 1
+ self.v_batt = None # will be updated with battery voltage when sphero.getBatteryVoltage() is called
+ self.firmware_version = [] # will be updated with firware version when sphero.returnMainApplicationVersion() is called
+
+ if self.verbosity > 0:
+ print("[INFO] Connecting to ", MACAddr)
+ self.p = Peripheral(MACAddr, "random") #connect
+
+ if self.verbosity > 1:
+ print("[INIT] Initializing")
+
+ # Subscribe to notifications
+ self.sphero_delegate = MyDelegate(self, user_delegate) # Pass a reference to this instance when initializing
+ self.p.setDelegate(self.sphero_delegate)
+
+ if self.verbosity > 1:
+ print("[INIT] Read all characteristics and descriptors")
+ # Get characteristics and descriptors
+ self.API_V2_characteristic = self.p.getCharacteristics(uuid="00010002-574f-4f20-5370-6865726f2121")[0]
+ self.AntiDOS_characteristic = self.p.getCharacteristics(uuid="00020005-574f-4f20-5370-6865726f2121")[0]
+ self.DFU_characteristic = self.p.getCharacteristics(uuid="00020002-574f-4f20-5370-6865726f2121")[0]
+ self.DFU2_characteristic = self.p.getCharacteristics(uuid="00020004-574f-4f20-5370-6865726f2121")[0]
+ self.API_descriptor = self.API_V2_characteristic.getDescriptors(forUUID=0x2902)[0]
+ self.DFU_descriptor = self.DFU_characteristic.getDescriptors(forUUID=0x2902)[0]
+
+ # The rest of this sequence was observed during bluetooth sniffing:
+ # Unlock code: prevent the sphero mini from going to sleep again after 10 seconds
+ if self.verbosity > 1:
+ print("[INIT] Writing AntiDOS characteristic unlock code")
+ self.AntiDOS_characteristic.write("usetheforce...band".encode(), withResponse=True)
+
+ # Enable DFU notifications:
+ if self.verbosity > 1:
+ print("[INIT] Configuring DFU descriptor")
+ self.DFU_descriptor.write(struct.pack('<bb', 0x01, 0x00), withResponse = True)
+
+ # No idea what this is for. Possibly a device ID of sorts? Read request returns '00 00 09 00 0c 00 02 02':
+ if self.verbosity > 1:
+ print("[INIT] Reading DFU2 characteristic")
+ _ = self.DFU2_characteristic.read()
+
+ # Enable API notifications:
+ if self.verbosity > 1:
+ print("[INIT] Configuring API dectriptor")
+ self.API_descriptor.write(struct.pack('<bb', 0x01, 0x00), withResponse = True)
+
+ self.wake()
+
+ # Finished initializing:
+ if self.verbosity > 1:
+ print("[INIT] Initialization complete\n")
+
+ def disconnect(self):
+ if self.verbosity > 0:
+ print("[INFO] Disconnecting")
+
+ self.p.disconnect()
+
+ def wake(self):
+ '''
+ Bring device out of sleep mode (can only be done if device was in sleep, not deep sleep).
+ If in deep sleep, the device should be connected to USB power to wake.
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Waking".format(self.sequence))
+
+ self._send(characteristic=self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=powerCommandIDs["wake"],
+ payload=[]) # empty payload
+
+ self.getAcknowledgement("Wake")
+
+ def sleep(self, deepSleep=False):
+ '''
+ Put device to sleep or deep sleep (deep sleep needs USB power connected to wake up)
+ '''
+ if deepSleep:
+ sleepCommID=powerCommandIDs["deepSleep"]
+ if self.verbosity > 0:
+ print("[INFO] Going into deep sleep. Connect USB power to wake.")
+ else:
+ sleepCommID=powerCommandIDs["sleep"]
+ self._send(characteristic=self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=sleepCommID,
+ payload=[]) #empty payload
+
+ def setLEDColor(self, red = None, green = None, blue = None):
+ '''
+ Set device LED color based on RGB vales (each can range between 0 and 0xFF)
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Setting main LED colour to [{}, {}, {}]".format(self.sequence, red, green, blue))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['userIO'], # 0x1a
+ commID = userIOCommandIDs["allLEDs"], # 0x0e
+ payload = [0x00, 0x0e, red, green, blue])
+
+ self.getAcknowledgement("LED/backlight")
+
+ def setBackLEDIntensity(self, brightness=None):
+ '''
+ Set device LED backlight intensity based on 0-255 values
+
+ NOTE: this is not the same as aiming - it only turns on the LED
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Setting backlight intensity to {}".format(self.sequence, brightness))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['userIO'],
+ commID = userIOCommandIDs["allLEDs"],
+ payload = [0x00, 0x01, brightness])
+
+ self.getAcknowledgement("LED/backlight")
+
+ def roll(self, speed=None, heading=None):
+ '''
+ Start to move the Sphero at a given direction and speed.
+ heading: integer from 0 - 360 (degrees)
+ speed: Integer from 0 - 255
+
+ Note: the zero heading should be set at startup with the resetHeading method. Otherwise, it may
+ seem that the sphero doesn't honor the heading argument
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Rolling with speed {} and heading {}".format(self.sequence, speed, heading))
+
+ if abs(speed) > 255:
+ print("WARNING: roll speed parameter outside of allowed range (-255 to +255)")
+
+ if speed < 0:
+ speed = -1*speed+256 # speed values > 256 in the send packet make the spero go in reverse
+
+ speedH = (speed & 0xFF00) >> 8
+ speedL = speed & 0xFF
+ headingH = (heading & 0xFF00) >> 8
+ headingL = heading & 0xFF
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['driving'],
+ commID = drivingCommands["driveWithHeading"],
+ payload = [speedL, headingH, headingL, speedH])
+
+ self.getAcknowledgement("Roll")
+
+ def resetHeading(self):
+ '''
+ Reset the heading zero angle to the current heading (useful during aiming)
+ Note: in order to manually rotate the sphero, you need to call stabilization(False).
+ Once the heading has been set, call stabilization(True).
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Resetting heading".format(self.sequence))
+
+ self._send(characteristic = self.API_V2_characteristic,
+ devID = deviceID['driving'],
+ commID = drivingCommands["resetHeading"],
+ payload = []) #empty payload
+
+ self.getAcknowledgement("Heading")
+
+ def returnMainApplicationVersion(self):
+ '''
+ Sends command to return application data in a notification
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Requesting firmware version".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID = deviceID['systemInfo'],
+ commID = SystemInfoCommands['mainApplicationVersion'],
+ payload = []) # empty
+
+ self.getAcknowledgement("Firmware")
+
+ def getBatteryVoltage(self):
+ '''
+ Sends command to return battery voltage data in a notification.
+ Data printed to console screen by the handleNotifications() method in the MyDelegate class.
+ '''
+ if self.verbosity > 2:
+ print("[SEND {}] Requesting battery voltage".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['powerInfo'],
+ commID=powerCommandIDs['batteryVoltage'],
+ payload=[]) # empty
+
+ self.getAcknowledgement("Battery")
+
+ def stabilization(self, stab = True):
+ '''
+ Sends command to turn on/off the motor stabilization system (required when manually turning/aiming the sphero)
+ '''
+ if stab == True:
+ if self.verbosity > 2:
+ print("[SEND {}] Enabling stabilization".format(self.sequence))
+ val = 1
+ else:
+ if self.verbosity > 2:
+ print("[SEND {}] Disabling stabilization".format(self.sequence))
+ val = 0
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['driving'],
+ commID=drivingCommands['stabilization'],
+ payload=[val])
+
+ self.getAcknowledgement("Stabilization")
+
+ def wait(self, delay):
+ '''
+ This is a non-blocking delay command. It is similar to time.sleep(), except it allows asynchronous
+ notification handling to still be performed.
+ '''
+ start = time.time()
+ while(1):
+ self.p.waitForNotifications(0.001)
+ if time.time() - start > delay:
+ break
+
+ def _send(self, characteristic=None, devID=None, commID=None, payload=[]):
+ '''
+ A generic "send" method, which will be used by other methods to send a command ID, payload and
+ appropriate checksum to a specified device ID. Mainly useful because payloads are optional,
+ and can be of varying length, to convert packets to binary, and calculate and send the
+ checksum. For internal use only.
+
+ Packet structure has the following format (in order):
+
+ - Start byte: always 0x8D
+ - Flags byte: indicate response required, etc
+ - Virtual device ID: see _constants.py
+ - Command ID: see _constants.py
+ - Sequence number: Seems to be arbitrary. I suspect it is used to match commands to response packets (in which the number is echoed).
+ - Payload: Could be varying number of bytes (incl. none), depending on the command
+ - Checksum: See below for calculation
+ - End byte: always 0xD8
+
+ '''
+ sendBytes = [sendPacketConstants["StartOfPacket"],
+ sum([flags["resetsInactivityTimeout"], flags["requestsResponse"]]),
+ devID,
+ commID,
+ self.sequence] + payload # concatenate payload list
+
+ self.sequence += 1 # Increment sequence number, ensures we can identify response packets are for this command
+ if self.sequence > 255:
+ self.sequence = 0
+
+ # Compute and append checksum and add EOP byte:
+ # From Sphero docs: "The [checksum is the] modulo 256 sum of all the bytes
+ # from the device ID through the end of the data payload,
+ # bit inverted (1's complement)"
+ # For the sphero mini, the flag bits must be included too:
+ checksum = 0
+ for num in sendBytes[1:]:
+ checksum = (checksum + num) & 0xFF # bitwise "and to get modulo 256 sum of appropriate bytes
+ checksum = 0xff - checksum # bitwise 'not' to invert checksum bits
+ sendBytes += [checksum, sendPacketConstants["EndOfPacket"]] # concatenate
+
+ # Convert numbers to bytes
+ output = b"".join([x.to_bytes(1, byteorder='big') for x in sendBytes])
+
+ #send to specified characteristic:
+ characteristic.write(output, withResponse = True)
+
+ def getAcknowledgement(self, ack):
+ #wait up to 10 secs for correct acknowledgement to come in, including sequence number!
+ start = time.time()
+ while(1):
+ self.p.waitForNotifications(1)
+ if self.sphero_delegate.notification_seq == self.sequence-1: # use one less than sequence, because _send function increments it for next send.
+ if self.verbosity > 3:
+ print("[RESP {}] {}".format(self.sequence-1, self.sphero_delegate.notification_ack))
+ self.sphero_delegate.clear_notification()
+ break
+ elif self.sphero_delegate.notification_seq >= 0:
+ print("Unexpected ACK. Expected: {}/{}, received: {}/{}".format(
+ ack, self.sequence, self.sphero_delegate.notification_ack.split()[0],
+ self.sphero_delegate.notification_seq)
+ )
+ if time.time() > start + 10:
+ print("Timeout waiting for acknowledgement: {}/{}".format(ack, self.sequence))
+ break
+
+# =======================================================================
+# The following functions are experimental:
+# =======================================================================
+
+ def configureCollisionDetection(self,
+ xThreshold = 50,
+ yThreshold = 50,
+ xSpeed = 50,
+ ySpeed = 50,
+ deadTime = 50, # in 10 millisecond increments
+ method = 0x01, # Must be 0x01
+ callback = None):
+ '''
+ Appears to function the same as other Sphero models, however speed settings seem to have no effect.
+ NOTE: Setting to zero seems to cause bluetooth errors with the Sphero Mini/bluepy library - set to
+ 255 to make it effectively disabled.
+
+ deadTime disables future collisions for a short period of time to avoid repeat triggering by the same
+ event. Set in 10ms increments. So if deadTime = 50, that means the delay will be 500ms, or half a second.
+
+ From Sphero docs:
+
+ xThreshold/yThreshold: An 8-bit settable threshold for the X (left/right) and Y (front/back) axes
+ of Sphero.
+
+ xSpeed/ySpeed: An 8-bit settable speed value for the X and Y axes. This setting is ranged by the
+ speed, then added to xThreshold, yThreshold to generate the final threshold value.
+ '''
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring collision detection".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['configureCollision'],
+ payload=[method, xThreshold, xSpeed, yThreshold, ySpeed, deadTime])
+
+ self.collision_detection_callback = callback
+
+ self.getAcknowledgement("Collision")
+
+ def configureSensorStream(self): # Use default values
+ '''
+ Send command to configure sensor stream using default values as found during bluetooth
+ sniffing of the Sphero Edu app.
+
+ Must be called after calling configureSensorMask()
+ '''
+ bitfield1 = 0b00000000 # Unknown function - needs experimenting
+ bitfield2 = 0b00000000 # Unknown function - needs experimenting
+ bitfield3 = 0b00000000 # Unknown function - needs experimenting
+ bitfield4 = 0b00000000 # Unknown function - needs experimenting
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring sensor stream".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['configureSensorStream'],
+ payload=[bitfield1, bitfield1, bitfield1, bitfield1])
+
+ self.getAcknowledgement("Sensor")
+
+ def configureSensorMask(self,
+ sample_rate_divisor = 0x25, # Must be > 0
+ packet_count = 0,
+ IMU_pitch = False,
+ IMU_roll = False,
+ IMU_yaw = False,
+ IMU_acc_x = False,
+ IMU_acc_y = False,
+ IMU_acc_z = False,
+ IMU_gyro_x = False,
+ IMU_gyro_y = False,
+ IMU_gyro_z = False):
+
+ '''
+ Send command to configure sensor mask using default values as found during bluetooth
+ sniffing of the Sphero Edu app. From experimentation, it seems that these are he functions of each:
+
+ Sampling_rate_divisor. Slow data EG: Set to 0x32 to the divide data rate by 50. Setting below 25 (0x19) causes
+ bluetooth errors
+
+ Packet_count: Select the number of packets to transmit before ending the stream. Set to zero to stream infinitely
+
+ All IMU bool parameters: Toggle transmission of that value on or off (e.g. set IMU_acc_x = True to include the
+ X-axis accelerometer readings in the sensor stream)
+ '''
+
+ # Construct bitfields based on function parameters:
+ IMU_bitfield1 = (IMU_pitch<<2) + (IMU_roll<<1) + IMU_yaw
+ IMU_bitfield2 = ((IMU_acc_y<<7) + (IMU_acc_z<<6) + (IMU_acc_x<<5) + \
+ (IMU_gyro_y<<4) + (IMU_gyro_x<<2) + (IMU_gyro_z<<2))
+
+ if self.verbosity > 2:
+ print("[SEND {}] Configuring sensor mask".format(self.sequence))
+
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensorMask'],
+ payload=[0x00, # Unknown param - altering it seems to slow data rate. Possibly averages multiple readings?
+ sample_rate_divisor,
+ packet_count, # Packet count: select the number of packets to stop streaming after (zero = infinite)
+ 0b00, # Unknown param: seems to be another accelerometer bitfield? Z-acc, Y-acc
+ IMU_bitfield1,
+ IMU_bitfield2,
+ 0b00]) # reserved, Position?, Position?, velocity?, velocity?, Y-gyro, timer, reserved
+
+ self.getAcknowledgement("Mask")
+
+ '''
+ Since the sensor values arrive as unlabelled lists in the order that they appear in the bitfields above, we need
+ to create a list of sensors that have been configured.Once we have this list, then in the default_delegate class,
+ we can get sensor values as attributes of the sphero_mini class.
+ e.g. print(sphero.IMU_yaw) # displays the current yaw angle
+ '''
+
+ # Initialize dictionary with sensor names as keys and their bool values (set by the user) as values:
+ availableSensors = {"IMU_pitch" : IMU_pitch,
+ "IMU_roll" : IMU_roll,
+ "IMU_yaw" : IMU_yaw,
+ "IMU_acc_y" : IMU_acc_y,
+ "IMU_acc_z" : IMU_acc_z,
+ "IMU_acc_x" : IMU_acc_x,
+ "IMU_gyro_y" : IMU_gyro_y,
+ "IMU_gyro_x" : IMU_gyro_x,
+ "IMU_gyro_z" : IMU_gyro_z}
+
+ # Create list of of only sensors that have been "activated" (set as true in the method arguments):
+ self.configured_sensors = [name for name in availableSensors if availableSensors[name] == True]
+
+ def sensor1(self): # Use default values
+ '''
+ Unknown function. Observed in bluetooth sniffing.
+ '''
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensor1'],
+ payload=[0x01])
+
+ self.getAcknowledgement("Sensor1")
+
+ def sensor2(self): # Use default values
+ '''
+ Unknown function. Observed in bluetooth sniffing.
+ '''
+ self._send(self.API_V2_characteristic,
+ devID=deviceID['sensor'],
+ commID=sensorCommands['sensor2'],
+ payload=[0x00])
+
+ self.getAcknowledgement("Sensor2")
+
+# =======================================================================
+
+class MyDelegate(btle.DefaultDelegate):
+
+ '''
+ This class handles notifications (both responses and asynchronous notifications).
+
+ Usage of this class is described in the Bluepy documentation
+
+ '''
+
+ def __init__(self, sphero_class, user_delegate):
+ self.sphero_class = sphero_class # for saving sensor values as attributes of sphero class instance
+ self.user_delegate = user_delegate # to directly notify users of callbacks
+ btle.DefaultDelegate.__init__(self)
+ self.clear_notification()
+ self.notificationPacket = []
+
+ def clear_notification(self):
+ self.notification_ack = "DEFAULT ACK"
+ self.notification_seq = -1
+
+ def bits_to_num(self, bits):
+ '''
+ This helper function decodes bytes from sensor packets into single precision floats. Encoding follows the
+ the IEEE-754 standard.
+ '''
+ num = int(bits, 2).to_bytes(len(bits) // 8, byteorder='little')
+ num = struct.unpack('f', num)[0]
+ return num
+
+ def handleNotification(self, cHandle, data):
+ '''
+ This method acts as an interrupt service routine. When a notification comes in, this
+ method is invoked, with the variable 'cHandle' being the handle of the characteristic that
+ sent the notification, and 'data' being the payload (sent one byte at a time, so the packet
+ needs to be reconstructed)
+
+ The method keeps appending bytes to the payload packet byte list until end-of-packet byte is
+ encountered. Note that this is an issue, because 0xD8 could be sent as part of the payload of,
+ say, the battery voltage notification. In future, a more sophisticated method will be required.
+ '''
+ # Allow the user to intercept and process data first..
+ if self.user_delegate != None:
+ if self.user_delegate.handleNotification(cHandle, data):
+ return
+
+ #print("Received notification with packet ", str(data))
+
+ for data_byte in data: # parse each byte separately (sometimes they arrive simultaneously)
+
+ self.notificationPacket.append(data_byte) # Add new byte to packet list
+
+ # If end of packet (need to find a better way to segment the packets):
+ if data_byte == sendPacketConstants['EndOfPacket']:
+ # Once full the packet has arrived, parse it:
+ # Packet structure is similar to the outgoing send packets (see docstring in sphero_mini._send())
+
+ # Attempt to unpack. Might fail if packet is too badly corrupted
+ try:
+ start, flags_bits, devid, commcode, seq, *notification_payload, chsum, end = self.notificationPacket
+ except ValueError:
+ print("Warning: notification packet unparseable ", self.notificationPacket)
+ self.notificationPacket = [] # Discard this packet
+ return # exit
+
+ # Compute and append checksum and add EOP byte:
+ # From Sphero docs: "The [checksum is the] modulo 256 sum of all the bytes
+ # from the device ID through the end of the data payload,
+ # bit inverted (1's complement)"
+ # For the sphero mini, the flag bits must be included too:
+ checksum_bytes = [flags_bits, devid, commcode, seq] + notification_payload
+ checksum = 0 # init
+ for num in checksum_bytes:
+ checksum = (checksum + num) & 0xFF # bitwise "and to get modulo 256 sum of appropriate bytes
+ checksum = 0xff - checksum # bitwise 'not' to invert checksum bits
+ if checksum != chsum: # check computed checksum against that recieved in the packet
+ print("Warning: notification packet checksum failed - ", str(self.notificationPacket))
+ self.notificationPacket = [] # Discard this packet
+ return # exit
+
+ # Check if response packet:
+ if flags_bits & flags['isResponse']: # it is a response
+
+ # Use device ID and command code to determine which command is being acknowledged:
+ if devid == deviceID['powerInfo'] and commcode == powerCommandIDs['wake']:
+ self.notification_ack = "Wake acknowledged" # Acknowledgement after wake command
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands['driveWithHeading']:
+ self.notification_ack = "Roll command acknowledged"
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands['stabilization']:
+ self.notification_ack = "Stabilization command acknowledged"
+
+ elif devid == deviceID['userIO'] and commcode == userIOCommandIDs['allLEDs']:
+ self.notification_ack = "LED/backlight color command acknowledged"
+
+ elif devid == deviceID['driving'] and commcode == drivingCommands["resetHeading"]:
+ self.notification_ack = "Heading reset command acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["configureCollision"]:
+ self.notification_ack = "Collision detection configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["configureSensorStream"]:
+ self.notification_ack = "Sensor stream configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensorMask"]:
+ self.notification_ack = "Mask configuration acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensor1"]:
+ self.notification_ack = "Sensor1 acknowledged"
+
+ elif devid == deviceID['sensor'] and commcode == sensorCommands["sensor2"]:
+ self.notification_ack = "Sensor2 acknowledged"
+
+ elif devid == deviceID['powerInfo'] and commcode == powerCommandIDs['batteryVoltage']:
+ V_batt = notification_payload[2] + notification_payload[1]*256 + notification_payload[0]*65536
+ V_batt /= 100 # Notification gives V_batt in 10mV increments. Divide by 100 to get to volts.
+ self.notification_ack = "Battery voltage:" + str(V_batt) + "v"
+ self.sphero_class.v_batt = V_batt
+
+ elif devid == deviceID['systemInfo'] and commcode == SystemInfoCommands['mainApplicationVersion']:
+ version = '.'.join(str(x) for x in notification_payload)
+ self.notification_ack = "Firmware version: " + version
+ self.sphero_class.firmware_version = notification_payload
+
+ else:
+ self.notification_ack = "Unknown acknowledgement" #print(self.notificationPacket)
+ print(self.notificationPacket, "===================> Unknown ack packet")
+
+ self.notification_seq = seq
+
+ else: # Not a response packet - therefore, asynchronous notification (e.g. collision detection, etc):
+
+ # Collision detection:
+ if devid == deviceID['sensor'] and commcode == sensorCommands['collisionDetectedAsync']:
+ # The first four bytes are data that is still un-parsed. the remaining unsaved bytes are always zeros
+ _, _, _, _, _, _, axis, _, Y_mag, _, X_mag, *_ = notification_payload
+ if axis == 1:
+ dir = "Left/right"
+ else:
+ dir = 'Forward/back'
+ print("Collision detected:")
+ print("\tAxis: ", dir)
+ print("\tX_mag: ", X_mag)
+ print("\tY_mag: ", Y_mag)
+
+ if self.sphero_class.collision_detection_callback is not None:
+ self.notificationPacket = [] # need to clear packet, in case new notification comes in during callback
+ self.sphero_class.collision_detection_callback()
+
+ # Sensor response:
+ elif devid == deviceID['sensor'] and commcode == sensorCommands['sensorResponse']:
+ # Convert to binary, pad bytes with leading zeros:
+ val = ''
+ for byte in notification_payload:
+ val += format(int(bin(byte)[2:], 2), '#010b')[2:]
+
+ # Break into 32-bit chunks
+ nums = []
+ while(len(val) > 0):
+ num, val = val[:32], val[32:] # Slice off first 16 bits
+ nums.append(num)
+
+ # convert from raw bits to float:
+ nums = [self.bits_to_num(num) for num in nums]
+
+ # Set sensor values as class attributes:
+ for name, value in zip(self.sphero_class.configured_sensors, nums):
+ print("Setting sensor at .", name, str(value))
+ setattr(self.sphero_class, name, value)
+
+ # Unrecognized packet structure:
+ else:
+ self.notification_ack = "Unknown asynchronous notification" #print(self.notificationPacket)
+ print(str(self.notificationPacket) + " ===================> Unknown async packet")
+
+ self.notificationPacket = [] # Start new payload after this byte
\ No newline at end of file
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/karte.jpg b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/karte.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..2dfab8ae944dcd069b9970cebe77965f549b21a0
Binary files /dev/null and b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/karte.jpg differ
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/sphero_conf.json b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/sphero_conf.json
new file mode 100644
index 0000000000000000000000000000000000000000..a490b4b798c696938452dac610e3cfac087c6279
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/sphero_conf.json
@@ -0,0 +1,3 @@
+{
+ "MAC_ADDRESS": "C6:69:72:CD:BC:6D"
+}
diff --git a/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/sphero_mini.py b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/sphero_mini.py
new file mode 100644
index 0000000000000000000000000000000000000000..2dd16aaa64b8df585becfba8c698b561e06fb62f
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/sphero_mini.py
@@ -0,0 +1,756 @@
+#!/usr/bin/env python3
+############################################################################
+#%% Imports
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+import threading
+from geometry_msgs.msg import Quaternion, Vector3
+import numpy as np
+from sphero_mini_controller.coreROS2 import SpheroMini
+import cv2
+import time
+import queue
+import matplotlib.pyplot as plt
+############################################################################
+#%% Blob Detector / Globale Variablen
+detector = cv2.SimpleBlobDetector()
+
+# Setup SimpleBlobDetector parameters.
+params = cv2.SimpleBlobDetector_Params()
+
+# Change thresholds
+#params.minThreshold = 5
+#params.maxThreshold = 500
+
+#FIlter by Color
+params.filterByColor = True
+params.blobColor = 255
+
+# Filter by Area.
+params.filterByArea = True
+params.minArea = 10
+params.maxArea = 200
+
+# Filter by Circularity
+#params.filterByCircularity = True
+#params.minCircularity = 0.5
+#params.maxCircularity = 1
+
+# Filter by Convexity
+#params.filterByConvexity = True
+#params.minConvexity = 0.7
+#params.maxConvexity = 1
+
+# Filter by Inertia
+#params.filterByInertia = True
+#params.minInertiaRatio = 0.01
+
+# Create a detector with the parameters
+detector = cv2.SimpleBlobDetector_create(params)
+############################################################################
+#%% Class Video Capture
+# Bufferless VideoCapture
+# Quelle ?
+class VideoCapture:
+ def __init__(self, name):
+ self.cap = cv2.VideoCapture(name)
+ self.q = queue.Queue()
+ t = threading.Thread(target=self._reader)
+ t.daemon = True
+ t.start()
+ # read frames as soon as they are available, keeping only most recent one
+ def _reader(self):
+ while True:
+ ret, frame = self.cap.read()
+ #cv2.imshow("Cap", frame)
+
+ if not ret:
+ break
+ if not self.q.empty():
+ try:
+ self.q.get_nowait() # discard previous (unprocessed) frame
+ except queue.Empty:
+ pass
+ self.q.put(frame)
+
+ def read(self):
+ return self.q.get()
+############################################################################
+#%% Class Sphero Node
+class MyNode(Node):
+ def __init__(self):
+ super().__init__("sphero_mini")
+
+ def connect(self):
+ MAC_ADDRESS = "C6:69:72:CD:BC:6D"
+
+ # Connect:
+ sphero = SpheroMini(MAC_ADDRESS, verbosity = 4)
+ # battery voltage
+ sphero.getBatteryVoltage()
+ print(f"Bettery voltage: {sphero.v_batt}v")
+
+ # firmware version number
+ sphero.returnMainApplicationVersion()
+ print(f"Firmware version: {'.'.join(str(x) for x in sphero.firmware_version)}")
+ return sphero
+
+ def create_quaternion(self, roll, pitch, yaw):
+ q = Quaternion()
+ cy, sy = np.cos(yaw * 0.5), np.sin(yaw * 0.5)
+ cp, sp = np.cos(pitch * 0.5), np.sin(pitch * 0.5)
+ cr, sr = np.cos(roll * 0.5), np.sin(roll * 0.5)
+
+ q.w = cr * cp * cy + sr * sp * sy
+ q.x = sr * cp * cy - cr * sp * sy
+ q.y = cr * sp * cy + sr * cp * sy
+ q.z = cr * cp * sy - sr * sp * cy
+
+ return q
+
+ def create_angular_veolocity_vector3(self, groll, gpitch, gyaw):
+ v = Vector3()
+ v.x = groll
+ v.y = gpitch
+ v.z = float(gyaw)
+
+ return v
+
+ def create_linear_acc_vector3(self, xacc, yacc, zacc):
+ v = Vector3()
+ v.x = xacc
+ v.y = yacc
+ v.z = zacc
+
+ return v
+
+ def get_sensors_data(self, sphero):
+ return {
+ "roll": sphero.IMU_roll,
+ "pitch": sphero.IMU_pitch,
+ "yaw": sphero.IMU_yaw,
+ "groll": sphero.IMU_gyro_x,
+ "gpitch": sphero.IMU_gyro_y,
+ "xacc": sphero.IMU_acc_x,
+ "yacc": sphero.IMU_acc_y,
+ "zacc": sphero.IMU_acc_z
+ }
+
+ def publish_imu(self, sensors_values,node):
+ i = Imu()
+
+ i.header.stamp = node.get_clock().now().to_msg()
+
+ i.orientation = self.create_quaternion(
+ roll=sensors_values["roll"],
+ pitch=sensors_values["pitch"],
+ yaw=sensors_values["yaw"]
+ )
+ i.angular_velocity = self.create_angular_veolocity_vector3(
+ groll=sensors_values["groll"],
+ gpitch=sensors_values["gpitch"],
+ gyaw=0 # We don't have the IMU_gyro_z
+ )
+ i.linear_acceleration = self.create_linear_acc_vector3(
+ xacc=sensors_values["xacc"],
+ yacc=sensors_values["yacc"],
+ zacc=sensors_values["zacc"]
+ )
+
+ def get_pos(self, cap, height):
+ #zeitanfang = time.time()
+
+ success = False
+
+ while(success == False):
+ try:
+ img = cap.read()
+
+ gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY )
+ keypoints = detector.detect(gray)
+
+ for keyPoint in keypoints:
+ x = keyPoint.pt[0]
+ y = keyPoint.pt[1]
+ success = True
+
+ xTrans, yTrans = self.calc_trans(x,y, height)
+ except Exception:
+ continue
+
+ #zeitende = time.time()
+ #print("Dauer Programmausführung:",(zeitende-zeitanfang))
+ #print("Get_Pos: X,Y:", xTrans,yTrans)
+
+ return xTrans,yTrans
+
+ def calc_offset(self, sphero, cap, height):
+ sphero.roll(100,0)
+ sphero.wait(1)
+ sphero.roll(0,0)
+ sphero.wait(0.5)
+
+ ref = self.get_pos(cap, height)
+ print("calc_offset: Ref Punkt x,y", ref)
+
+ sphero.wait(1)
+ sphero.roll(100,180)
+ sphero.wait(1)
+ sphero.roll(0,0)
+
+ startpunkt = self.get_pos(cap, height)
+ print("calc_offset: Startpunkt x,y", startpunkt)
+
+ ref = np.array(ref)
+ startpunkt = np.array(startpunkt)
+
+ start_ref = ref-startpunkt
+
+ phi = np.arctan2(start_ref[1],start_ref[0])
+ phi = np.degrees(phi)
+ phi = int(-phi)
+
+ print("Phi ohne alles:", phi)
+
+ phi = self.phi_in_range(phi)
+
+ print("Calc_Offset: ", phi)
+
+ return phi
+
+ def calc_av(self,startPos, sollPos, cap, height):
+ startPos = np.array(startPos)
+ sollPos = np.array(sollPos)
+
+ pktSpheroKord = sollPos - startPos
+
+ phi = np.arctan2(pktSpheroKord[1], pktSpheroKord[0])
+ phi = np.degrees(phi)
+
+ phiZiel = int(phi)
+
+ phiZiel = self.phi_in_range(phiZiel)
+
+ v = 100
+
+ #print("Calc_AV: a,v", phiZiel, v)
+
+ return phiZiel, v
+
+ def drive_to(self,sphero, targetPos, aOffset, cap, height):
+ dmin = 20
+ pos = self.get_pos(cap, height)
+ pos = np.array(pos)
+
+ diff = targetPos - pos
+
+ d = np.linalg.norm(diff, ord = 2)
+
+ ar = []
+ ar.append(0)
+
+ i = 1
+
+ while d > dmin:
+ a,v = self.calc_av(pos,targetPos, cap, height)
+
+ aR = -aOffset - a
+ ar.append((self.phi_in_range(aR)))
+
+ #Fahrbefehl
+ if(ar[i] != ar[i-1]):
+ sphero.roll(v, ar[i])
+ sphero.wait(0.05)
+ else:
+ sphero.wait(0.05)
+
+ #Aktuelle Pos
+ pos = self.get_pos(cap, height)
+ pos = np.array(pos)
+
+ #Abweichung
+ diff = targetPos - pos
+ diff = np.array(diff)
+ d = np.linalg.norm(diff, ord = 2)
+
+ i = i + 1
+
+ sphero.roll(0,0)
+ print("Ziel Erreicht")
+
+ return
+
+ def calc_trans(self,x,y, height):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+
+ def phi_in_range(self,phi):
+ while(phi < 0):
+ phi = phi + 360
+ while(phi > 360):
+ phi = phi - 360
+ return phi
+############################################################################
+#%% Class Wavefrontplanner
+class waveFrontPlanner:
+ ############################################################################
+ # WAVEFRONT ALGORITHM
+ # Adapted to Python Code By Darin Velarde
+ # Fri Jan 29 13:56:53 PST 2010
+ # from C code from John Palmisano
+ # (www.societyofrobots.com)
+ ############################################################################
+
+ def __init__(self, mapOfWorld, slow=False):
+ self.__slow = slow
+ self.__mapOfWorld = mapOfWorld
+ if str(type(mapOfWorld)).find("numpy") != -1:
+ #If its a numpy array
+ self.__height, self.__width = self.__mapOfWorld.shape
+ else:
+ self.__height, self.__width = len(self.__mapOfWorld), len(self.__mapOfWorld[0])
+
+ self.__nothing = 000
+ self.__wall = 999
+ self.__goal = 1
+ self.__path = "PATH"
+
+ self.__finalPath = []
+
+ #Robot value
+ self.__robot = 254
+ #Robot default Location
+ self.__robot_x = 0
+ self.__robot_y = 0
+
+ #default goal location
+ self.__goal_x = 8
+ self.__goal_y = 9
+
+ #temp variables
+ self.__temp_A = 0
+ self.__temp_B = 0
+ self.__counter = 0
+ self.__steps = 0 #determine how processor intensive the algorithm was
+
+ #when searching for a node with a lower value
+ self.__minimum_node = 250
+ self.__min_node_location = 250
+ self.__new_state = 1
+ self.__old_state = 1
+ self.__reset_min = 250 #above this number is a special (wall or robot)
+ ###########################################################################
+
+ def setRobotPosition(self, x, y):
+ """
+ Sets the robot's current position
+
+ """
+
+ self.__robot_x = x
+ self.__robot_y = y
+ ###########################################################################
+
+ def setGoalPosition(self, x, y):
+ """
+ Sets the goal position.
+
+ """
+
+ self.__goal_x = x
+ self.__goal_y = y
+ ###########################################################################
+
+ def robotPosition(self):
+ return (self.__robot_x, self.__robot_y)
+ ###########################################################################
+
+ def goalPosition(self):
+ return (self.__goal_x, self.__goal_y)
+ ###########################################################################
+
+ def run(self, prnt=False):
+ """
+ The entry point for the robot algorithm to use wavefront propagation.
+
+ """
+
+ path = []
+ while self.__mapOfWorld[self.__robot_x][self.__robot_y] != self.__goal:
+ if self.__steps > 20000:
+ print ("Cannot find a path.")
+ return
+ #find new location to go to
+ self.__new_state = self.propagateWavefront()
+ #update location of robot
+ if self.__new_state == 1:
+ self.__robot_x -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" % (self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 2:
+ self.__robot_y += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 3:
+ self.__robot_x += 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ if self.__new_state == 4:
+ self.__robot_y -= 1
+ if prnt:
+ print("Move to x=%d y=%d\n\n" %(self.__robot_x, self.__robot_y))
+ path.append((self.__robot_x, self.__robot_y))
+ self.__old_state = self.__new_state
+ msg = "Found the goal in %i steps:\n" % self.__steps
+ msg += "mapOfWorld size= %i %i\n\n" % (self.__height, self.__width)
+ if prnt:
+ print(msg)
+ self.printMap()
+ return path
+ ###########################################################################
+
+ def propagateWavefront(self, prnt=False):
+ self.unpropagate()
+ #Old robot location was deleted, store new robot location in mapOfWorld
+ self.__mapOfWorld[self.__robot_x][self.__robot_y] = self.__robot
+ self.__path = self.__robot
+ #start location to begin scan at goal location
+ self.__mapOfWorld[self.__goal_x][self.__goal_y] = self.__goal
+ counter = 0
+ while counter < 200: #allows for recycling until robot is found
+ x = 0
+ y = 0
+ #time.sleep(0.00001)
+ #while the mapOfWorld hasnt been fully scanned
+ while x < self.__height and y < self.__width:
+ #if this location is a wall or the goal, just ignore it
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal:
+ #a full trail to the robot has been located, finished!
+ minLoc = self.minSurroundingNodeValue(x, y)
+ if minLoc < self.__reset_min and \
+ self.__mapOfWorld[x][y] == self.__robot:
+ if prnt:
+ print("Finished Wavefront:\n")
+ self.printMap()
+ # Tell the robot to move after this return.
+ return self.__min_node_location
+ #record a value in to this node
+ elif self.__minimum_node != self.__reset_min:
+ #if this isnt here, 'nothing' will go in the location
+ self.__mapOfWorld[x][y] = self.__minimum_node + 1
+ #go to next node and/or row
+ y += 1
+ if y == self.__width and x != self.__height:
+ x += 1
+ y = 0
+ #print self.__robot_x, self.__robot_y
+ if prnt:
+ print("Sweep #: %i\n" % (counter + 1))
+ self.printMap()
+ self.__steps += 1
+ counter += 1
+ return 0
+ ###########################################################################
+
+ def unpropagate(self):
+ """
+ clears old path to determine new path
+ stay within boundary
+
+ """
+
+ for x in range(0, self.__height):
+ for y in range(0, self.__width):
+ if self.__mapOfWorld[x][y] != self.__wall and \
+ self.__mapOfWorld[x][y] != self.__goal and \
+ self.__mapOfWorld[x][y] != self.__path:
+ #if this location is a wall or goal, just ignore it
+ self.__mapOfWorld[x][y] = self.__nothing #clear that space
+ ###########################################################################
+
+ def minSurroundingNodeValue(self, x, y):
+ """
+ this method looks at a node and returns the lowest value around that
+ node.
+
+ """
+
+ #reset minimum
+ self.__minimum_node = self.__reset_min
+ #down
+ if x < self.__height -1:
+ if self.__mapOfWorld[x + 1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x + 1][y] != self.__nothing:
+ #find the lowest number node, and exclude empty nodes (0's)
+ self.__minimum_node = self.__mapOfWorld[x + 1][y]
+ self.__min_node_location = 3
+ #up
+ if x > 0:
+ if self.__mapOfWorld[x-1][y] < self.__minimum_node and \
+ self.__mapOfWorld[x-1][y] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x-1][y]
+ self.__min_node_location = 1
+ #right
+ if y < self.__width -1:
+ if self.__mapOfWorld[x][y + 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y + 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y + 1]
+ self.__min_node_location = 2
+ #left
+ if y > 0:
+ if self.__mapOfWorld[x][y - 1] < self.__minimum_node and \
+ self.__mapOfWorld[x][y - 1] != self.__nothing:
+ self.__minimum_node = self.__mapOfWorld[x][y-1]
+ self.__min_node_location = 4
+ return self.__minimum_node
+ ###########################################################################
+
+ def printMap(self):
+ """
+ Prints out the map of this instance of the class.
+
+ """
+
+ msg = ''
+ for temp_B in range(0, self.__height):
+ for temp_A in range(0, self.__width):
+ if self.__mapOfWorld[temp_B][temp_A] == self.__wall:
+ msg += "%04s" % "[#]"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__robot:
+ msg += "%04s" % "-"
+ elif self.__mapOfWorld[temp_B][temp_A] == self.__goal:
+ msg += "%04s" % "G"
+ else:
+ msg += "%04s" % str(self.__mapOfWorld[temp_B][temp_A])
+ msg += "\n\n"
+ msg += "\n\n"
+ print(msg)
+ #
+ if self.__slow == True:
+ time.sleep(0.05)
+
+ def plotMap(self, img, path,mapWorldScaled,mapWorldOrg):
+ #Programm Koordinaten
+ imgTrans = cv2.transpose(img) # X und Y-Achse im Bild tauschen
+ imgPlot, ax0 = plt.subplots()
+ ax0.set_title('Programm Koordinaten')
+ ax0.imshow(imgTrans)
+ ax0.set_xlabel('[px]')
+ ax0.set_ylabel('[px]')
+ ax0.scatter(path[:,0], path[:,1], color='r')
+ #Karten Array Scaled
+ imgPlot1, ax1 = plt.subplots()
+ ax1.set_title('Karten-Array (Vergrößerte Hindernisse e=3)')
+ ax1.imshow(mapWorldScaled, cmap = 'Greys')
+ ax1.set_xlabel('[px]')
+ ax1.set_ylabel('[px]')
+ #Karten Array Normal
+ imgPlot2, ax2 = plt.subplots()
+ ax2.set_title('Karten-Array (Originale Hindernisse)')
+ ax2.imshow(mapWorldOrg, cmap = 'Greys')
+ ax2.set_xlabel('[px]')
+ ax2.set_ylabel('[px]')
+ #Bild Koordinaten
+ imgPlot3, ax3 = plt.subplots()
+ ax3.set_title('Bild Koordinaten')
+ ax3.set_xlabel('[px]')
+ ax3.set_ylabel('[px]')
+ ax3.imshow(img)
+ ax3.scatter(path[:,1], path[:,0], color='r')
+ return
+
+
+ def getPathWelt(self,path,height,Mapobj):
+ pathWeltX, pathWeltY = Mapobj.calc_trans_welt(path[:,1], path[:,0], height)
+ pathEckenX = []
+ pathEckenY = []
+
+ for i,px in enumerate(pathWeltX):
+ if (i < len(pathWeltX)-1) & (i > 0):
+ if px != pathWeltX[i+1]:
+ if pathWeltY[i]!=pathWeltY[i-1]:
+ pathEckenX.append(px)
+ pathEckenY.append(pathWeltY[i])
+ if pathWeltY[i] != pathWeltY[i+1]:
+ if pathWeltX[i]!=pathWeltX[i-1]:
+ pathEckenX.append(px)
+ pathEckenY.append(pathWeltY[i])
+
+ pathEckenX.append(pathWeltX[-1])
+ pathEckenY.append(pathWeltY[-1])
+
+
+ uebergabePath = []
+ for i in range(0,len(pathEckenX)):
+ uebergabePath.append((pathEckenX[i],pathEckenY[i]))
+
+ print("Ecken: ", uebergabePath)
+
+ return uebergabePath
+
+############################################################################
+#%% Class Map
+class mapCreate:
+ ############################################################################
+
+ def create_map(self, scale, cap):
+ img = cap.read()
+
+ #Karte von Bild
+ #img = cv2.imread("D:/ros2_ws/src/sphero_mini_controller/sphero_mini_controller/map/map.jpg")
+ print('Original Dimensions : ',img.shape)
+
+ scale_percent = 100-scale # percent of original size
+ width = int(img.shape[1] * scale_percent / 100)
+ height = int(img.shape[0] * scale_percent / 100)
+ dim = (width, height)
+
+ # resize image
+ resized = cv2.resize(img, dim, interpolation = cv2.INTER_AREA)
+ print('Resized Dimensions : ',resized.shape)
+
+ gray = cv2.cvtColor(resized, cv2.COLOR_RGB2GRAY)
+ mask = cv2.inRange(gray, np.array([200]), np.array([255])) #Parameter hängt von Umgebung ab
+
+ plt.plot(mask)
+
+ mapOfWorld = [[0]*gray.shape[1]]*gray.shape[0]
+ mask_list= np.ndarray.tolist(mask)
+
+ #Markiere alle leeren Zellen mit 000 und alle Zellen mit Hinderniss mit 999
+ for i in range(0,mask.shape[0]):
+ mapOfWorld[i] = ['999' if j > 200 else '000' for j in mask_list[i]]
+
+ mapOfWorld = np.array(mapOfWorld, dtype = int)
+ mapOfWorldSized = mapOfWorld.copy()
+
+ e = 3
+ #Hindernisse Größer machen
+ for i in range(0,mapOfWorld.shape[0]):
+ for j in range(0,mapOfWorld.shape[1]):
+ for r in range(0,e):
+ try:
+ if (mapOfWorldSized[i][j] == 999):
+ mapOfWorld[i][j+e] = 999
+ mapOfWorld[i+e][j] = 999
+ mapOfWorld[i-e][j] = 999
+ mapOfWorld[i][j-e] = 999
+ mapOfWorld[i+e][j+e] = 999
+ mapOfWorld[i+e][j-e] = 999
+ mapOfWorld[i-e][j+e] = 999
+ mapOfWorld[i-e][j-e] = 999
+ except Exception:
+ continue
+
+ return mapOfWorld, mapOfWorldSized
+ ############################################################################
+
+ def scale_koord(self, sx, sy, gx, gy,scale):
+ scale_percent = 100-scale # percent of original size
+
+ sx = int(sx*scale_percent/100)
+ sy = int(sy*scale_percent/100)
+ gx = int(gx*scale_percent/100)
+ gy = int(gy*scale_percent/100)
+
+ return sx,sy,gx,gy
+ ############################################################################
+
+ def rescale_koord(self, path,scale):
+ scale_percent = 100-scale # percent of original size100
+
+ path = np.array(path)
+
+ for i in range(len(path)):
+ path[i] = path[i]*100/scale_percent
+
+ return path
+ ############################################################################
+
+ def calc_trans_welt(self, x, y, height):
+ yTrans = height - y
+ xTrans = x
+
+ return xTrans, yTrans
+###############################################################################
+#%% Main Funktion
+def main(args = None):
+ #Verbindung herstellen, Node erstellen
+ try:
+ rclpy.init(args=args) #Kommunikation Starten
+ node = MyNode() #Node erstellen
+ sphero = node.connect()
+ thread = threading.Thread(target=rclpy.spin, args=(node, ), daemon=True)
+ thread.start()
+
+ except Exception as e: # rclpy.ROSInterruptException
+ print("Konnte nicht verbinden")
+ raise e
+
+ cap = VideoCapture("http://root:root@10.128.41.239:80/mjpg/video.mjpg")
+ #cap = VideoCapture(4)
+ Map = mapCreate()
+
+ img = cap.read()
+ height, width = img.shape[:2]
+
+ scale = 90
+
+ sphero.setLEDColor(255,0,0)
+ sphero.wait(1)
+ sphero.setBackLEDIntensity(0)
+ sphero.stabilization(True)
+
+ aOffset = node.calc_offset(sphero, cap, height)
+
+ sphero.wait(1)
+
+ while(True):
+ mapWorldScaled, mapWorldOrg = Map.create_map(scale, cap)
+
+ #Befehle für WaveFrontPlanner/Maperstellung
+ sy,sx = node.get_pos(cap,height) #Aktuelle Roboter Pos in Welt
+ sy, sx = node.calc_trans(sy,sx,height)
+
+ gy = int(input("Bitte geben Sie die X-Zielkoordinate im Bild Koordinatensystem ein:")) #X-Koordinate im Weltkoordinaten System
+ gx = int(input("Bitte geben Sie die Y-Zielkoordinate im Bild Koordinatensystem ein:")) #Y-Koordinate im Weltkordinaten System
+
+ sx,sy,gx,gy = Map.scale_koord(sx,sy,gx,gy,scale) #Kordinaten Tauschen X,Y
+
+ start = time.time()
+ planner = waveFrontPlanner(mapWorldScaled)
+ planner.setGoalPosition(gx,gy)
+ planner.setRobotPosition(sx,sy)
+ path = planner.run(False)
+ end = time.time()
+ print("Took %f seconds to run wavefront simulation" % (end - start))
+
+ path = Map.rescale_koord(path, scale)
+
+ planner.plotMap(img, path, mapWorldScaled, mapWorldOrg)
+
+ pathWelt = planner.getPathWelt(path, height, Map)
+
+ for p in pathWelt:
+ node.drive_to(sphero, p, aOffset, cap, height)
+ sphero.wait(1)
+
+ print("Geschafft")
+
+ if cv2.waitKey(10) & 0xFF == ord('q'):
+ break
+
+ rclpy.shutdown()
+
+############################################################################
+#%% Main
+if __name__ == '__main__':
+ main()
+############################################################################
diff --git a/ros2_ws/src/sphero_mini_controller/test/test_copyright.py b/ros2_ws/src/sphero_mini_controller/test/test_copyright.py
new file mode 100644
index 0000000000000000000000000000000000000000..97a39196e84db97954341162a6d2e7f771d938c0
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/test/test_copyright.py
@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found errors'
diff --git a/ros2_ws/src/sphero_mini_controller/test/test_flake8.py b/ros2_ws/src/sphero_mini_controller/test/test_flake8.py
new file mode 100644
index 0000000000000000000000000000000000000000..27ee1078ff077cc3a0fec75b7d023101a68164d1
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/test/test_flake8.py
@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+ rc, errors = main_with_errors(argv=[])
+ assert rc == 0, \
+ 'Found %d code style errors / warnings:\n' % len(errors) + \
+ '\n'.join(errors)
diff --git a/ros2_ws/src/sphero_mini_controller/test/test_pep257.py b/ros2_ws/src/sphero_mini_controller/test/test_pep257.py
new file mode 100644
index 0000000000000000000000000000000000000000..b234a3840f4c5bd38f043638c8622b8f240e1185
--- /dev/null
+++ b/ros2_ws/src/sphero_mini_controller/test/test_pep257.py
@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+ rc = main(argv=['.', 'test'])
+ assert rc == 0, 'Found code style errors / warnings'