From c3cd0bd88b3e8f04a2f3ecb7a00a6c8de6bd0443 Mon Sep 17 00:00:00 2001 From: Manos Kampourakis Date: Mon, 20 Apr 2020 15:19:24 +0300 Subject: [PATCH 1/3] Replace egn_messages with vehicle_msgs --- CMakeLists.txt | 4 +- README.md | 4 +- launch/startExploring.launch | 4 + package.xml | 2 +- src/MaRRTPathPlanNode.py | 55 +++-- src/inc/ma_rrt.py | 433 ----------------------------------- src/main.py | 0 7 files changed, 41 insertions(+), 461 deletions(-) mode change 100644 => 100755 src/MaRRTPathPlanNode.py delete mode 100644 src/inc/ma_rrt.py mode change 100644 => 100755 src/main.py diff --git a/CMakeLists.txt b/CMakeLists.txt index bf89ba0..f64ba16 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(catkin REQUIRED COMPONENTS geometry_msgs message_filters message_generation - egn_messages + vehicle_msgs ) ## System dependencies are found with CMake's conventions @@ -71,6 +71,6 @@ install(DIRECTORY install(PROGRAMS src/main.py src/MaRRTPathPlanNode.py - src/inc/ma_rrt.py + src/ma_rrt.py DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/README.md b/README.md index 0c5706e..286add4 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ A brief introduction to the main steps of the proposed algorithm is given in my ## Inputs, outputs, params #### Inputs -- rospy.Subscriber("/map", Map, ...) +- rospy.Subscriber("/map", Track, ...) - rospy.Subscriber("/odometry", Odometry, ...) #### Outputs @@ -34,6 +34,8 @@ A brief introduction to the main steps of the proposed algorithm is given in my - rospy.Publisher("/visual/waypoints", MarkerArray, ...) #### Parameters to tune (main) +- `odom_topic` = `/odometry` - The topic to get the odometry information from +- `world_frame` = `world` - The world frame to use for the visualizations - planDistance = 12 m - Maximum length of tree branch - expandDistance = 1 m - Length of tree node/step - expandAngle = 20 deg - constraining angle for next tree nodes diff --git a/launch/startExploring.launch b/launch/startExploring.launch index d6e553b..8b4b124 100644 --- a/launch/startExploring.launch +++ b/launch/startExploring.launch @@ -3,6 +3,10 @@ + + + diff --git a/package.xml b/package.xml index 891dc39..3f2825f 100644 --- a/package.xml +++ b/package.xml @@ -12,7 +12,7 @@ rospy std_msgs - egn_messages + vehicle_msgs rospy std_msgs diff --git a/src/MaRRTPathPlanNode.py b/src/MaRRTPathPlanNode.py old mode 100644 new mode 100755 index 6d2c676..2b7267c --- a/src/MaRRTPathPlanNode.py +++ b/src/MaRRTPathPlanNode.py @@ -10,10 +10,7 @@ import ma_rrt -from egn_messages.msg import Map -from egn_messages.msg import CarSensors -from egn_messages.msg import WaypointsArray -from egn_messages.msg import Waypoint +from vehicle_msgs.msg import TrackCone, Track, Command, Waypoint, WaypointsArray from visualization_msgs.msg import Marker from visualization_msgs.msg import MarkerArray @@ -45,14 +42,24 @@ def __init__(self): if rospy.has_param('~filename'): self.filename = rospy.get_param('~filename') + if rospy.has_param('~odom_topic'): + self.odometry_topic = rospy.get_param('~odom_topic') + else: + self.odometry_topic = "/odometry" + + if rospy.has_param('~world_frame'): + self.world_frame = rospy.get_param('~world_frame') + else: + self.world_frame = "world" + waypointsFrequency = rospy.get_param('~desiredWaypointsFrequency', 5) self.waypointsPublishInterval = 1.0 / waypointsFrequency self.lastPublishWaypointsTime = 0 # All Subs and pubs - rospy.Subscriber("/map", Map, self.mapCallback) - rospy.Subscriber("/odometry", Odometry, self.odometryCallback) - # rospy.Subscriber("/car_sensors", CarSensors, self.carSensorsCallback) + rospy.Subscriber("/track", Track, self.mapCallback) + rospy.Subscriber(self.odometry_topic, Odometry, self.odometryCallback) + rospy.Subscriber("/dvsim/cmd", Command, self.carSensorsCallback) # Create publishers self.waypointsPub = rospy.Publisher("/waypoints", WaypointsArray, queue_size=0) @@ -89,7 +96,7 @@ def __init__(self): self.filteredBestBranch = [] self.discardAmount = 0 - # print("MaRRTPathPlanNode Constructor has been called") + print("MaRRTPathPlanNode Constructor has been called") def __del__(self): print('MaRRTPathPlanNode: Destructor called.') @@ -107,20 +114,20 @@ def odometryCallback(self, odometry): self.carPosYaw = yaw #print "Estimated processing odometry callback: {0} ms".format((time.time() - start)*1000) - def carSensorsCallback(self, carSensors): + def carSensorsCallback(self, command): # rospy.loginfo("carSensorsCallback") # start = time.time() - self.steerAngle = carSensors.steerAngle + # The ackermann angle [rad] + self.steerAngle = math.degrees((command.theta_l + command.theta_r) / 2.0) #print "Estimated processing map callback: {0} ms".format((time.time() - start)*1000); - def mapCallback(self, map): - self.map = map + def mapCallback(self, track): + self.map = track.cones def sampleTree(self): # sampleTreeStartTime = time.time() - if self.loopClosure and len(self.savedWaypoints) > 0: # print("Publish savedWaypoints/predifined waypoints, return") self.publishWaypoints() @@ -222,7 +229,7 @@ def sampleTree(self): def mergeWaypoints(self, newWaypoints): # print "mergeWaypoints:", "len(saved):", len(self.savedWaypoints), "len(new):", len(newWaypoints) if not newWaypoints: - return; + return maxDistToSaveWaypoints = 2.0 maxWaypointAmountToSave = 2 @@ -371,7 +378,7 @@ def publishWaypoints(self, newWaypoints = None): # print "publishWaypoints(): start" waypointsArray = WaypointsArray() - waypointsArray.header.frame_id = "world" + waypointsArray.header.frame_id = self.world_frame waypointsArray.header.stamp = rospy.Time.now() waypointsArray.preliminaryLoopClosure = self.preliminaryLoopClosure @@ -417,7 +424,7 @@ def publishWaypointsVisuals(self, newWaypoints = None): markerArray = MarkerArray() savedWaypointsMarker = Marker() - savedWaypointsMarker.header.frame_id = "world" + savedWaypointsMarker.header.frame_id = self.world_frame savedWaypointsMarker.header.stamp = rospy.Time.now() savedWaypointsMarker.lifetime = rospy.Duration(1) savedWaypointsMarker.ns = "saved-publishWaypointsVisuals" @@ -441,7 +448,7 @@ def publishWaypointsVisuals(self, newWaypoints = None): if newWaypoints is not None: newWaypointsMarker = Marker() - newWaypointsMarker.header.frame_id = "world" + newWaypointsMarker.header.frame_id = self.world_frame newWaypointsMarker.header.stamp = rospy.Time.now() newWaypointsMarker.lifetime = rospy.Duration(1) newWaypointsMarker.ns = "new-publishWaypointsVisuals" @@ -544,7 +551,7 @@ def publishDelaunayEdgesVisual(self, edges): return marker = Marker() - marker.header.frame_id = "world" + marker.header.frame_id = self.world_frame marker.header.stamp = rospy.Time.now() marker.lifetime = rospy.Duration(1) marker.ns = "publishDelaunayLinesVisual" @@ -663,7 +670,7 @@ def isLeftCone(self, node, parentNode, cone): def publishBestBranchVisual(self, nodeList, leafNode): marker = Marker() - marker.header.frame_id = "world" + marker.header.frame_id = self.world_frame marker.header.stamp = rospy.Time.now() marker.lifetime = rospy.Duration(0.2) marker.ns = "publishBestBranchVisual" @@ -699,7 +706,7 @@ def publishFilteredBranchVisual(self): return marker = Marker() - marker.header.frame_id = "world" + marker.header.frame_id = self.world_frame marker.header.stamp = rospy.Time.now() marker.lifetime = rospy.Duration(0.2) marker.ns = "publisshFilteredBranchVisual" @@ -733,7 +740,7 @@ def publishTreeVisual(self, nodeList, leafNodes): # tree lines marker treeMarker = Marker() - treeMarker.header.frame_id = "world" + treeMarker.header.frame_id = self.world_frame treeMarker.header.stamp = rospy.Time.now() treeMarker.ns = "rrt" @@ -760,7 +767,7 @@ def publishTreeVisual(self, nodeList, leafNodes): # leaves nodes marker leavesMarker = Marker() - leavesMarker.header.frame_id = "world" + leavesMarker.header.frame_id = self.world_frame leavesMarker.header.stamp = rospy.Time.now() leavesMarker.lifetime = rospy.Duration(0.2) leavesMarker.ns = "rrt-leaves" @@ -803,7 +810,7 @@ def getFrontConeObstacles(self, map, frontDist): frontDistSq = frontDist ** 2 frontConeList = [] - for cone in map.cones: + for cone in map: if (headingVectorOrt[0] * (cone.y - carPosBehindPoint[1]) - headingVectorOrt[1] * (cone.x - carPosBehindPoint[0])) < 0: if ((cone.x - self.carPosX) ** 2 + (cone.y - self.carPosY) ** 2) < frontDistSq: frontConeList.append(cone) @@ -818,7 +825,7 @@ def getHeadingVector(self): def getConesInRadius(self, map, x, y, radius): coneList = [] radiusSq = radius * radius - for cone in map.cones: + for cone in map: if ((cone.x - x) ** 2 + (cone.y - y) ** 2) < radiusSq: coneList.append(cone) return coneList diff --git a/src/inc/ma_rrt.py b/src/inc/ma_rrt.py deleted file mode 100644 index 9116fc8..0000000 --- a/src/inc/ma_rrt.py +++ /dev/null @@ -1,433 +0,0 @@ -""" -Path Planning Sample Code with RRT* - -author: AtsushiSakai(@Atsushi_twi) -with edits of Maxim Yastremsky(@MaxMagazin) - -""" - -import random -import math -import copy -import numpy as np -import matplotlib.pyplot as plt -import time - -import sys, select, termios, tty - -class RRT(): - """ - Class for RRT Planning - """ - - def __init__(self, start, planDistance, obstacleList, expandDis=0.5, turnAngle=30, maxIter=1000, rrtTargets = None): - - self.start = Node(start[0], start[1], start[2]) - self.startYaw = start[2] - - self.planDistance = planDistance - self.expandDis = expandDis - self.turnAngle = math.radians(turnAngle) - - self.maxDepth = int(planDistance / expandDis) - # print(self.maxDepth) - - self.maxIter = maxIter - self.obstacleList = obstacleList - self.rrtTargets = rrtTargets - # self.end = Node(0, planDistance) - - self.aboveMaxDistance = 0 - self.belowMaxDistance = 0 - self.collisionHit = 0 - self.doubleNodeCount = 0 - - self.savedRandoms = [] - - def Planning(self, animation=False, interactive=False): - self.nodeList = [self.start] - self.leafNodes = [] - - for i in range(self.maxIter): - # rnd = self.get_random_point() - rnd = self.get_random_point_from_target_list() - - # print "===== random: {0},{1}".format(rnd[0], rnd[1]); - - nind = self.GetNearestListIndex(self.nodeList, rnd) - - nearestNode = self.nodeList[nind] - # print("nearestNode: " + str(nearestNode)) - - if (nearestNode.cost >= self.planDistance): - # self.aboveMaxDistance += 1 - continue - # self.belowMaxDistance += 1 - - newNode = self.steerConstrained(rnd, nind) - # newNode = self.steer(rnd, nind) #tests, delete - - # due to angle constraints it is possible that similar node is generated - if newNode in self.nodeList: - # self.doubleNodeCount += 1 - continue - - if self.__CollisionCheck(newNode, self.obstacleList): - # nearinds = self.find_near_nodes(newNode) - # newNode = self.choose_parent(newNode, nearinds) - self.nodeList.append(newNode) - # self.rewire(newNode, nearinds) - - if (newNode.cost >= self.planDistance): - # print("got a leaf " + str(newNode)) - self.leafNodes.append(newNode) - # else: - # self.collisionHit += 1 - - if animation: - self.DrawSample(rnd) - - if interactive: - key = self.getKey() - - if (key == '\x03'): #CTRL+C - break - - # print "rrt.Planning(): aboveMaxDistance: {0}".format(self.aboveMaxDistance); - # print "rrt.Planning(): belowMaxDistance: {0}".format(self.belowMaxDistance); - # print "rrt.Planning(): doubleNodeCount: {0}".format(self.doubleNodeCount); - # print "rrt.Planning(): collisionHit: {0}".format(self.collisionHit); - # print "rrt.Planning(): nodeList length: {0}".format(len(self.nodeList)); - return self.nodeList, self.leafNodes - - def getKey(self): - tty.setraw(sys.stdin.fileno()) - select.select([sys.stdin], [], [], 0) - key = sys.stdin.read(1) - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) - return key - - def choose_parent(self, newNode, nearinds): - if len(nearinds) == 0: - return newNode - - dlist = [] - for i in nearinds: - dx = newNode.x - self.nodeList[i].x - dy = newNode.y - self.nodeList[i].y - d = math.sqrt(dx ** 2 + dy ** 2) - theta = math.atan2(dy, dx) - if self.check_collision_extend(self.nodeList[i], theta, d): - dlist.append(self.nodeList[i].cost + d) - else: - dlist.append(float("inf")) - - mincost = min(dlist) - minind = nearinds[dlist.index(mincost)] - - if mincost == float("inf"): - print("mincost is inf") - return newNode - - newNode.cost = mincost - newNode.parent = minind - - return newNode - - def steerConstrained(self, rnd, nind): - # expand tree - nearestNode = self.nodeList[nind] - theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) - - # print "theta: {0}".format(math.degrees(theta)); - # print "nearestNode.yaw: {0}".format(math.degrees(nearestNode.yaw)); - - # dynamic constraints - angleChange = self.pi_2_pi(theta - nearestNode.yaw) - - # print "angleChange: {0}".format(math.degrees(angleChange)); - - angle30degree = math.radians(30) - - if angleChange > angle30degree: - angleChange = self.turnAngle - elif angleChange >= -angle30degree: - angleChange = 0 - else: - angleChange = -self.turnAngle - # print "angleChange2: {0}".format(math.degrees(angleChange)); - - newNode = copy.deepcopy(nearestNode) - newNode.yaw += angleChange - newNode.x += self.expandDis * math.cos(newNode.yaw) - newNode.y += self.expandDis * math.sin(newNode.yaw) - - newNode.cost += self.expandDis - newNode.parent = nind - - # print "newNode: {0}".format(newNode) - return newNode - - def pi_2_pi(self, angle): - return (angle + math.pi) % (2*math.pi) - math.pi - - def steer(self, rnd, nind): - # expand tree - nearestNode = self.nodeList[nind] - theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) - newNode = copy.deepcopy(nearestNode) - newNode.x += self.expandDis * math.cos(theta) - newNode.y += self.expandDis * math.sin(theta) - - newNode.cost += self.expandDis - newNode.parent = nind - return newNode - - def get_random_point(self): - - randX = random.uniform(0, self.planDistance) - randY = random.uniform(-self.planDistance, self.planDistance) - rnd = [randX, randY] - - car_rot_mat = np.array([[math.cos(self.startYaw), -math.sin(self.startYaw)], [math.sin(self.startYaw), math.cos(self.startYaw)]]) - rotatedRnd = np.dot(car_rot_mat, rnd) - - rotatedRnd = [rotatedRnd[0] + self.start.x, rotatedRnd[1] + self.start.y] - return rotatedRnd - - def get_random_point_from_target_list(self): - - maxTargetAroundDist = 3 - - if not self.rrtTargets: - return self.get_random_point() - - targetId = np.random.randint(len(self.rrtTargets)) - x, y, oSize = self.rrtTargets[targetId] - - # square idea - # randX = random.uniform(-maxTargetAroundDist, maxTargetAroundDist) - # randY = random.uniform(-maxTargetAroundDist, maxTargetAroundDist) - # finalRnd = [x + randX, y + randY] - - # circle idea - randAngle = random.uniform(0, 2 * math.pi) - randDist = random.uniform(oSize, maxTargetAroundDist) - finalRnd = [x + randDist * math.cos(randAngle), y + randDist * math.sin(randAngle)] - - return finalRnd - - def get_best_last_index(self): - - disglist = [self.calc_dist_to_goal( - node.x, node.y) for node in self.nodeList] - goalinds = [disglist.index(i) for i in disglist if i <= self.expandDis] - # print(goalinds) - - if len(goalinds) == 0: - return None - - mincost = min([self.nodeList[i].cost for i in goalinds]) - for i in goalinds: - if self.nodeList[i].cost == mincost: - return i - - return None - - def gen_final_course(self, goalind): - path = [[self.end.x, self.end.y]] - while self.nodeList[goalind].parent is not None: - node = self.nodeList[goalind] - path.append([node.x, node.y]) - goalind = node.parent - path.append([self.start.x, self.start.y]) - return path - - def calc_dist_to_goal(self, x, y): - return np.linalg.norm([x - self.end.x, y - self.end.y]) - - def find_near_nodes(self, newNode): - nnode = len(self.nodeList) - # r = 50.0 * math.sqrt((math.log(nnode) / nnode)) - r = self.expandDis * 3.0 - dlist = [(node.x - newNode.x) ** 2 + - (node.y - newNode.y) ** 2 for node in self.nodeList] - nearinds = [dlist.index(i) for i in dlist if i <= r ** 2] - # print "find_near_nodes, size: {0}".format(len(nearinds)) - return nearinds - - def rewire(self, newNode, nearinds): - nnode = len(self.nodeList) - for i in nearinds: - nearNode = self.nodeList[i] - - dx = newNode.x - nearNode.x - dy = newNode.y - nearNode.y - d = math.sqrt(dx ** 2 + dy ** 2) - - scost = newNode.cost + d - - if nearNode.cost > scost: - theta = math.atan2(dy, dx) - if self.check_collision_extend(nearNode, theta, d): - nearNode.parent = nnode - 1 - nearNode.cost = scost - - def check_collision_extend(self, nearNode, theta, d): - - tmpNode = copy.deepcopy(nearNode) - - for i in range(int(d / self.expandDis)): - tmpNode.x += self.expandDis * math.cos(theta) - tmpNode.y += self.expandDis * math.sin(theta) - if not self.__CollisionCheck(tmpNode, self.obstacleList): - return False - - return True - - def DrawSample(self, rnd=None): - - plt.clf() - if rnd is not None: - plt.plot(rnd[0], rnd[1], "^k") - - for node in self.nodeList: - if node.parent is not None: - plt.plot([node.x, self.nodeList[node.parent].x], [ - node.y, self.nodeList[node.parent].y], "-g") - - # draw obstacles - axes = plt.gca() - for (ox, oy, size) in self.obstacleList: - circle = plt.Circle((ox,oy), radius=size) - axes.add_patch(circle) - - plt.plot(self.start.x, self.start.y, "xr") - - axes = plt.gca() - xmin, xmax, ymin, ymax = -5, 25, -20, 20 - # plt.axis([-5, 45, -20, 20]) - axes.set_xlim([xmin,xmax]) - axes.set_ylim([ymin,ymax]) - # plt.axis("equal") - plt.grid(True) - plt.pause(0.001) - - def DrawGraph(self): - # plt.clf() - - # draw obstacles - ax = plt.gca() - for (ox, oy, size) in self.obstacleList: - circle = plt.Circle((ox,oy), radius=size) - ax.add_patch(circle) - - for node in self.nodeList: - if node.parent is not None: - plt.plot([node.x, self.nodeList[node.parent].x], [ - node.y, self.nodeList[node.parent].y], "-g") - - # plt.plot(self.start.x, self.start.y, "xr") - # plt.plot(self.end.x, self.end.y, "xr") - plt.axis([-5, 45, -20, 20]) - plt.axis("equal") - plt.grid(True) - plt.pause(0.01) - - def GetNearestListIndex(self, nodeList, rnd): - dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodeList] - minind = dlist.index(min(dlist)) - return minind - - def __CollisionCheck(self, node, obstacleList): - for (ox, oy, size) in obstacleList: - dx = ox - node.x - dy = oy - node.y - d = dx * dx + dy * dy - if d <= size ** 2: - return False # collision - return True # safe - -class Node(): - """ - RRT Node - """ - - def __init__(self, x, y, yaw): - self.x = x - self.y = y - self.yaw = yaw - self.cost = 0.0 - self.parent = None - - def __str__(self): - return str(round(self.x, 2)) + "," + str(round(self.y,2)) + "," + str(math.degrees(self.yaw)) + "," + str(self.cost) - - def __eq__(self, other): - return self.x == other.x and self.y == other.y and self.yaw == other.yaw and self.cost == other.cost - - def __repr__(self): - return str(self) - - -def main(): - print("Start rrt planning!") - - # ====Search Path with RRT==== - radius = 1 - obstacleList = [ - (1, -3, radius), - (1, 3, radius), - # (3, -3, radius), - # (3, 3, radius), - (6, -3, radius), - (6, 3, radius), - # (9, -2.8, radius), - # (9, 3.2, radius), - (12.5, -2.5, radius), - (12, 4, radius), - # (16, -2, radius), - # (15, 5, radius), - (20, -1, radius), - (18.5, 6.5, radius), - (24, 1, radius), - (22, 8, radius) - ] # [x,y,size(radius)] - - start = [0.0, 0.0, math.radians(0.0)] - planDistance = 10 - iterationNumber = 500 - rrtConeTargets = [] - - for o in obstacleList: - coneDist = math.sqrt((start[0] - o[0]) ** 2 + (start[1] - o[1]) ** 2) - - if coneDist > 10 and coneDist < 15: - rrtConeTargets.append((o[0], o[1], radius)) - - startTime = time.time() - - # Set Initial parameters - rrt = RRT(start, planDistance, obstacleList=obstacleList, expandDis=1, maxIter=iterationNumber, rrtTargets = rrtConeTargets) - rrt.Planning(True, True) - # rrt.Planning() - - print "rrt.Planning(): {0} ms".format((time.time() - startTime) * 1000); - print "nodesNumber/iteration: {0}/{1}".format(len(rrt.nodeList), iterationNumber) - - show_animation = True - - if show_animation: - rrt.DrawGraph() - # plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') - plt.grid(True) - plt.pause(0.01) # Need for Mac - plt.show() - - -if __name__ == '__main__': - settings = termios.tcgetattr(sys.stdin) - - main() - - termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) diff --git a/src/main.py b/src/main.py old mode 100644 new mode 100755 From 6b20b67ddc12ba192ad42c18956a11c3e8520644 Mon Sep 17 00:00:00 2001 From: Manos Kampourakis Date: Mon, 20 Apr 2020 15:29:09 +0300 Subject: [PATCH 2/3] Add vehicle_msgs --- README.md | 3 +++ vehicle_msgs/CMakeLists.txt | 28 ++++++++++++++++++++++++++++ vehicle_msgs/msg/Command.msg | 6 ++++++ vehicle_msgs/msg/Track.msg | 1 + vehicle_msgs/msg/TrackCone.msg | 4 ++++ vehicle_msgs/msg/Waypoint.msg | 4 ++++ vehicle_msgs/msg/WaypointsArray.msg | 6 ++++++ vehicle_msgs/package.xml | 14 ++++++++++++++ 8 files changed, 66 insertions(+) create mode 100644 vehicle_msgs/CMakeLists.txt create mode 100644 vehicle_msgs/msg/Command.msg create mode 100644 vehicle_msgs/msg/Track.msg create mode 100644 vehicle_msgs/msg/TrackCone.msg create mode 100644 vehicle_msgs/msg/Waypoint.msg create mode 100644 vehicle_msgs/msg/WaypointsArray.msg create mode 100644 vehicle_msgs/package.xml diff --git a/README.md b/README.md index 286add4..7080e45 100644 --- a/README.md +++ b/README.md @@ -17,6 +17,9 @@ A brief introduction to the main steps of the proposed algorithm is given in my - Exploration of [FSG'17 trackdrive circuit](https://www.youtube.com/watch?v=jJAjrCig3yE) in Gazebo. - *Your video (feel free to pull-request a link with it here).* +#### Vehicle Messages +Some custom messages are included inside the `ma_rrt_path_plan` package folder. Because the Git repository was setup before the creation of the messages (via a PR from [ekampourakis](https://github.com/ekampourakis/)), the `vehicle_msgs` package folder needs to be moved to the parent folder of the `ma_rrt_path_plan` in order to act as a seperate package and possibly included in the Makefile too. + ## Inputs, outputs, params #### Inputs diff --git a/vehicle_msgs/CMakeLists.txt b/vehicle_msgs/CMakeLists.txt new file mode 100644 index 0000000..dca96bf --- /dev/null +++ b/vehicle_msgs/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 2.8.12) +project(vehicle_msgs) + +find_package(catkin REQUIRED COMPONENTS + cmake_modules + geometry_msgs + message_generation + std_msgs +) + +add_message_files( + FILES + Command.msg + TrackCone.msg + Track.msg + Waypoint.msg + WaypointsArray.msg +) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) + +catkin_package( + CATKIN_DEPENDS message_runtime std_msgs +) diff --git a/vehicle_msgs/msg/Command.msg b/vehicle_msgs/msg/Command.msg new file mode 100644 index 0000000..a21b0fc --- /dev/null +++ b/vehicle_msgs/msg/Command.msg @@ -0,0 +1,6 @@ +float64 theta_l +float64 theta_r +float64 torque_fl +float64 torque_fr +float64 torque_rl +float64 torque_rr \ No newline at end of file diff --git a/vehicle_msgs/msg/Track.msg b/vehicle_msgs/msg/Track.msg new file mode 100644 index 0000000..6461de8 --- /dev/null +++ b/vehicle_msgs/msg/Track.msg @@ -0,0 +1 @@ +vehicle_msgs/TrackCone[] cones \ No newline at end of file diff --git a/vehicle_msgs/msg/TrackCone.msg b/vehicle_msgs/msg/TrackCone.msg new file mode 100644 index 0000000..9b1ca64 --- /dev/null +++ b/vehicle_msgs/msg/TrackCone.msg @@ -0,0 +1,4 @@ +float64 x +float64 y + +string type \ No newline at end of file diff --git a/vehicle_msgs/msg/Waypoint.msg b/vehicle_msgs/msg/Waypoint.msg new file mode 100644 index 0000000..f05cebc --- /dev/null +++ b/vehicle_msgs/msg/Waypoint.msg @@ -0,0 +1,4 @@ +float64 x +float64 y + +float64 id \ No newline at end of file diff --git a/vehicle_msgs/msg/WaypointsArray.msg b/vehicle_msgs/msg/WaypointsArray.msg new file mode 100644 index 0000000..c7e0dc4 --- /dev/null +++ b/vehicle_msgs/msg/WaypointsArray.msg @@ -0,0 +1,6 @@ +Header header + +vehicle_msgs/Waypoint[] waypoints + +bool preliminaryLoopClosure +bool loopClosure \ No newline at end of file diff --git a/vehicle_msgs/package.xml b/vehicle_msgs/package.xml new file mode 100644 index 0000000..4f09625 --- /dev/null +++ b/vehicle_msgs/package.xml @@ -0,0 +1,14 @@ + + vehicle_msgs + 0.0.1 + Messages for controlling the vehicle + Emmanouil Kampourakis + TODO + + catkin + + message_generation + message_runtime + + std_msgs + \ No newline at end of file From 8d93218b15b3c6ea446277dacbfc7f683866e065 Mon Sep 17 00:00:00 2001 From: Kampourakis Emmanouil <34644857+ekampourakis@users.noreply.github.com> Date: Sun, 10 May 2020 14:08:53 +0300 Subject: [PATCH 3/3] Upload wrongly deleted file --- src/ma_rrt.py | 433 ++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 433 insertions(+) create mode 100644 src/ma_rrt.py diff --git a/src/ma_rrt.py b/src/ma_rrt.py new file mode 100644 index 0000000..9116fc8 --- /dev/null +++ b/src/ma_rrt.py @@ -0,0 +1,433 @@ +""" +Path Planning Sample Code with RRT* + +author: AtsushiSakai(@Atsushi_twi) +with edits of Maxim Yastremsky(@MaxMagazin) + +""" + +import random +import math +import copy +import numpy as np +import matplotlib.pyplot as plt +import time + +import sys, select, termios, tty + +class RRT(): + """ + Class for RRT Planning + """ + + def __init__(self, start, planDistance, obstacleList, expandDis=0.5, turnAngle=30, maxIter=1000, rrtTargets = None): + + self.start = Node(start[0], start[1], start[2]) + self.startYaw = start[2] + + self.planDistance = planDistance + self.expandDis = expandDis + self.turnAngle = math.radians(turnAngle) + + self.maxDepth = int(planDistance / expandDis) + # print(self.maxDepth) + + self.maxIter = maxIter + self.obstacleList = obstacleList + self.rrtTargets = rrtTargets + # self.end = Node(0, planDistance) + + self.aboveMaxDistance = 0 + self.belowMaxDistance = 0 + self.collisionHit = 0 + self.doubleNodeCount = 0 + + self.savedRandoms = [] + + def Planning(self, animation=False, interactive=False): + self.nodeList = [self.start] + self.leafNodes = [] + + for i in range(self.maxIter): + # rnd = self.get_random_point() + rnd = self.get_random_point_from_target_list() + + # print "===== random: {0},{1}".format(rnd[0], rnd[1]); + + nind = self.GetNearestListIndex(self.nodeList, rnd) + + nearestNode = self.nodeList[nind] + # print("nearestNode: " + str(nearestNode)) + + if (nearestNode.cost >= self.planDistance): + # self.aboveMaxDistance += 1 + continue + # self.belowMaxDistance += 1 + + newNode = self.steerConstrained(rnd, nind) + # newNode = self.steer(rnd, nind) #tests, delete + + # due to angle constraints it is possible that similar node is generated + if newNode in self.nodeList: + # self.doubleNodeCount += 1 + continue + + if self.__CollisionCheck(newNode, self.obstacleList): + # nearinds = self.find_near_nodes(newNode) + # newNode = self.choose_parent(newNode, nearinds) + self.nodeList.append(newNode) + # self.rewire(newNode, nearinds) + + if (newNode.cost >= self.planDistance): + # print("got a leaf " + str(newNode)) + self.leafNodes.append(newNode) + # else: + # self.collisionHit += 1 + + if animation: + self.DrawSample(rnd) + + if interactive: + key = self.getKey() + + if (key == '\x03'): #CTRL+C + break + + # print "rrt.Planning(): aboveMaxDistance: {0}".format(self.aboveMaxDistance); + # print "rrt.Planning(): belowMaxDistance: {0}".format(self.belowMaxDistance); + # print "rrt.Planning(): doubleNodeCount: {0}".format(self.doubleNodeCount); + # print "rrt.Planning(): collisionHit: {0}".format(self.collisionHit); + # print "rrt.Planning(): nodeList length: {0}".format(len(self.nodeList)); + return self.nodeList, self.leafNodes + + def getKey(self): + tty.setraw(sys.stdin.fileno()) + select.select([sys.stdin], [], [], 0) + key = sys.stdin.read(1) + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) + return key + + def choose_parent(self, newNode, nearinds): + if len(nearinds) == 0: + return newNode + + dlist = [] + for i in nearinds: + dx = newNode.x - self.nodeList[i].x + dy = newNode.y - self.nodeList[i].y + d = math.sqrt(dx ** 2 + dy ** 2) + theta = math.atan2(dy, dx) + if self.check_collision_extend(self.nodeList[i], theta, d): + dlist.append(self.nodeList[i].cost + d) + else: + dlist.append(float("inf")) + + mincost = min(dlist) + minind = nearinds[dlist.index(mincost)] + + if mincost == float("inf"): + print("mincost is inf") + return newNode + + newNode.cost = mincost + newNode.parent = minind + + return newNode + + def steerConstrained(self, rnd, nind): + # expand tree + nearestNode = self.nodeList[nind] + theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) + + # print "theta: {0}".format(math.degrees(theta)); + # print "nearestNode.yaw: {0}".format(math.degrees(nearestNode.yaw)); + + # dynamic constraints + angleChange = self.pi_2_pi(theta - nearestNode.yaw) + + # print "angleChange: {0}".format(math.degrees(angleChange)); + + angle30degree = math.radians(30) + + if angleChange > angle30degree: + angleChange = self.turnAngle + elif angleChange >= -angle30degree: + angleChange = 0 + else: + angleChange = -self.turnAngle + # print "angleChange2: {0}".format(math.degrees(angleChange)); + + newNode = copy.deepcopy(nearestNode) + newNode.yaw += angleChange + newNode.x += self.expandDis * math.cos(newNode.yaw) + newNode.y += self.expandDis * math.sin(newNode.yaw) + + newNode.cost += self.expandDis + newNode.parent = nind + + # print "newNode: {0}".format(newNode) + return newNode + + def pi_2_pi(self, angle): + return (angle + math.pi) % (2*math.pi) - math.pi + + def steer(self, rnd, nind): + # expand tree + nearestNode = self.nodeList[nind] + theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x) + newNode = copy.deepcopy(nearestNode) + newNode.x += self.expandDis * math.cos(theta) + newNode.y += self.expandDis * math.sin(theta) + + newNode.cost += self.expandDis + newNode.parent = nind + return newNode + + def get_random_point(self): + + randX = random.uniform(0, self.planDistance) + randY = random.uniform(-self.planDistance, self.planDistance) + rnd = [randX, randY] + + car_rot_mat = np.array([[math.cos(self.startYaw), -math.sin(self.startYaw)], [math.sin(self.startYaw), math.cos(self.startYaw)]]) + rotatedRnd = np.dot(car_rot_mat, rnd) + + rotatedRnd = [rotatedRnd[0] + self.start.x, rotatedRnd[1] + self.start.y] + return rotatedRnd + + def get_random_point_from_target_list(self): + + maxTargetAroundDist = 3 + + if not self.rrtTargets: + return self.get_random_point() + + targetId = np.random.randint(len(self.rrtTargets)) + x, y, oSize = self.rrtTargets[targetId] + + # square idea + # randX = random.uniform(-maxTargetAroundDist, maxTargetAroundDist) + # randY = random.uniform(-maxTargetAroundDist, maxTargetAroundDist) + # finalRnd = [x + randX, y + randY] + + # circle idea + randAngle = random.uniform(0, 2 * math.pi) + randDist = random.uniform(oSize, maxTargetAroundDist) + finalRnd = [x + randDist * math.cos(randAngle), y + randDist * math.sin(randAngle)] + + return finalRnd + + def get_best_last_index(self): + + disglist = [self.calc_dist_to_goal( + node.x, node.y) for node in self.nodeList] + goalinds = [disglist.index(i) for i in disglist if i <= self.expandDis] + # print(goalinds) + + if len(goalinds) == 0: + return None + + mincost = min([self.nodeList[i].cost for i in goalinds]) + for i in goalinds: + if self.nodeList[i].cost == mincost: + return i + + return None + + def gen_final_course(self, goalind): + path = [[self.end.x, self.end.y]] + while self.nodeList[goalind].parent is not None: + node = self.nodeList[goalind] + path.append([node.x, node.y]) + goalind = node.parent + path.append([self.start.x, self.start.y]) + return path + + def calc_dist_to_goal(self, x, y): + return np.linalg.norm([x - self.end.x, y - self.end.y]) + + def find_near_nodes(self, newNode): + nnode = len(self.nodeList) + # r = 50.0 * math.sqrt((math.log(nnode) / nnode)) + r = self.expandDis * 3.0 + dlist = [(node.x - newNode.x) ** 2 + + (node.y - newNode.y) ** 2 for node in self.nodeList] + nearinds = [dlist.index(i) for i in dlist if i <= r ** 2] + # print "find_near_nodes, size: {0}".format(len(nearinds)) + return nearinds + + def rewire(self, newNode, nearinds): + nnode = len(self.nodeList) + for i in nearinds: + nearNode = self.nodeList[i] + + dx = newNode.x - nearNode.x + dy = newNode.y - nearNode.y + d = math.sqrt(dx ** 2 + dy ** 2) + + scost = newNode.cost + d + + if nearNode.cost > scost: + theta = math.atan2(dy, dx) + if self.check_collision_extend(nearNode, theta, d): + nearNode.parent = nnode - 1 + nearNode.cost = scost + + def check_collision_extend(self, nearNode, theta, d): + + tmpNode = copy.deepcopy(nearNode) + + for i in range(int(d / self.expandDis)): + tmpNode.x += self.expandDis * math.cos(theta) + tmpNode.y += self.expandDis * math.sin(theta) + if not self.__CollisionCheck(tmpNode, self.obstacleList): + return False + + return True + + def DrawSample(self, rnd=None): + + plt.clf() + if rnd is not None: + plt.plot(rnd[0], rnd[1], "^k") + + for node in self.nodeList: + if node.parent is not None: + plt.plot([node.x, self.nodeList[node.parent].x], [ + node.y, self.nodeList[node.parent].y], "-g") + + # draw obstacles + axes = plt.gca() + for (ox, oy, size) in self.obstacleList: + circle = plt.Circle((ox,oy), radius=size) + axes.add_patch(circle) + + plt.plot(self.start.x, self.start.y, "xr") + + axes = plt.gca() + xmin, xmax, ymin, ymax = -5, 25, -20, 20 + # plt.axis([-5, 45, -20, 20]) + axes.set_xlim([xmin,xmax]) + axes.set_ylim([ymin,ymax]) + # plt.axis("equal") + plt.grid(True) + plt.pause(0.001) + + def DrawGraph(self): + # plt.clf() + + # draw obstacles + ax = plt.gca() + for (ox, oy, size) in self.obstacleList: + circle = plt.Circle((ox,oy), radius=size) + ax.add_patch(circle) + + for node in self.nodeList: + if node.parent is not None: + plt.plot([node.x, self.nodeList[node.parent].x], [ + node.y, self.nodeList[node.parent].y], "-g") + + # plt.plot(self.start.x, self.start.y, "xr") + # plt.plot(self.end.x, self.end.y, "xr") + plt.axis([-5, 45, -20, 20]) + plt.axis("equal") + plt.grid(True) + plt.pause(0.01) + + def GetNearestListIndex(self, nodeList, rnd): + dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodeList] + minind = dlist.index(min(dlist)) + return minind + + def __CollisionCheck(self, node, obstacleList): + for (ox, oy, size) in obstacleList: + dx = ox - node.x + dy = oy - node.y + d = dx * dx + dy * dy + if d <= size ** 2: + return False # collision + return True # safe + +class Node(): + """ + RRT Node + """ + + def __init__(self, x, y, yaw): + self.x = x + self.y = y + self.yaw = yaw + self.cost = 0.0 + self.parent = None + + def __str__(self): + return str(round(self.x, 2)) + "," + str(round(self.y,2)) + "," + str(math.degrees(self.yaw)) + "," + str(self.cost) + + def __eq__(self, other): + return self.x == other.x and self.y == other.y and self.yaw == other.yaw and self.cost == other.cost + + def __repr__(self): + return str(self) + + +def main(): + print("Start rrt planning!") + + # ====Search Path with RRT==== + radius = 1 + obstacleList = [ + (1, -3, radius), + (1, 3, radius), + # (3, -3, radius), + # (3, 3, radius), + (6, -3, radius), + (6, 3, radius), + # (9, -2.8, radius), + # (9, 3.2, radius), + (12.5, -2.5, radius), + (12, 4, radius), + # (16, -2, radius), + # (15, 5, radius), + (20, -1, radius), + (18.5, 6.5, radius), + (24, 1, radius), + (22, 8, radius) + ] # [x,y,size(radius)] + + start = [0.0, 0.0, math.radians(0.0)] + planDistance = 10 + iterationNumber = 500 + rrtConeTargets = [] + + for o in obstacleList: + coneDist = math.sqrt((start[0] - o[0]) ** 2 + (start[1] - o[1]) ** 2) + + if coneDist > 10 and coneDist < 15: + rrtConeTargets.append((o[0], o[1], radius)) + + startTime = time.time() + + # Set Initial parameters + rrt = RRT(start, planDistance, obstacleList=obstacleList, expandDis=1, maxIter=iterationNumber, rrtTargets = rrtConeTargets) + rrt.Planning(True, True) + # rrt.Planning() + + print "rrt.Planning(): {0} ms".format((time.time() - startTime) * 1000); + print "nodesNumber/iteration: {0}/{1}".format(len(rrt.nodeList), iterationNumber) + + show_animation = True + + if show_animation: + rrt.DrawGraph() + # plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') + plt.grid(True) + plt.pause(0.01) # Need for Mac + plt.show() + + +if __name__ == '__main__': + settings = termios.tcgetattr(sys.stdin) + + main() + + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)