diff --git a/src/action/acceleration/acceleration/__init__.py b/src/action/acceleration/acceleration/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/action/acceleration/acceleration/controller.py b/src/action/acceleration/acceleration/controller.py new file mode 100644 index 00000000..bf2d4f73 --- /dev/null +++ b/src/action/acceleration/acceleration/controller.py @@ -0,0 +1,73 @@ +#!/usr/bin/env python3 +# Python imports +import rclpy +from rclpy.node import Node +from moa_msgs.msg import ConeMap +from ackermann_msgs.msg import AckermannDrive, AckermannDriveStamped +from std_msgs.msg import Header + + +class Acceleration_algorithm(Node): + def __init__(self): + super().__init__("Acceleration") + self.get_logger().info("Acceleration Started") + + # subscribe to Cone detection result + self.create_subscription(ConeMap, "cone_detection", self.main_hearback, 5) + + self.drive_pub = self.create_publisher(AckermannDrive, "/drive", 5) + self.drive_vis_pub = self.create_publisher(AckermannDrive, "/drive_vis", 5) + self.cmd_vel_pub = self.create_publisher(AckermannDriveStamped, "cmd_vel", 5) + + + def main_hearback(self, msg: ConeMap): + if len(msg.cones) == 0: + #No cones, make throttle to 0 + self.current_speed = 0.0 + self.publish_ackermann() + else: + #There is cones, adjust throttle to target speed + self.current_speed = 20/3.6 #In m/s + self.publish_ackermann() + + + + def publish_ackermann(self): + + args1 = {"steering_angle": 0.0, + "steering_angle_velocity": 0.0, + "speed": float(self.current_speed), + "acceleration": 0.0, + "jerk": 0.0} + msg1 = AckermannDrive(**args1) + + msg_cmd_vel = self.convert_to_stamped(msg1) + self.cmd_vel_pub.publish(msg_cmd_vel) + self.drive_pub.publish(msg1) + self.drive_vis_pub.publish(msg1) + + def convert_to_stamped(self, ackermann_msgs): + stamped_msg = AckermannDriveStamped() + stamped_msg.header = Header() + stamped_msg.header.stamp = self.get_clock().now().to_msg() # Set the current timestamp + stamped_msg.header.frame_id = '0' # Set the appropriate frame ID + stamped_msg.drive = ackermann_msgs + return stamped_msg + + +def main(args=None): + rclpy.init(args=args) + + accelrator = Acceleration_algorithm() + + rclpy.spin(accelrator) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + accelrator.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/action/acceleration/package.xml b/src/action/acceleration/package.xml new file mode 100644 index 00000000..27f00612 --- /dev/null +++ b/src/action/acceleration/package.xml @@ -0,0 +1,18 @@ + + + + acceleration + 0.0.0 + TODO: Package description + Yiyang Chen + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/action/acceleration/resource/acceleration b/src/action/acceleration/resource/acceleration new file mode 100644 index 00000000..e69de29b diff --git a/src/action/acceleration/setup.cfg b/src/action/acceleration/setup.cfg new file mode 100644 index 00000000..3427a7d3 --- /dev/null +++ b/src/action/acceleration/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/acceleration +[install] +install_scripts=$base/lib/acceleration diff --git a/src/action/acceleration/setup.py b/src/action/acceleration/setup.py new file mode 100644 index 00000000..c33358f6 --- /dev/null +++ b/src/action/acceleration/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'acceleration' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Yiyang Chen', + maintainer_email='yiyang030903@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'controller = acceleration.controller:main', + ], + }, +) diff --git a/src/action/acceleration/test/test_copyright.py b/src/action/acceleration/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/src/action/acceleration/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/src/action/acceleration/test/test_flake8.py b/src/action/acceleration/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/action/acceleration/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/src/action/acceleration/test/test_pep257.py b/src/action/acceleration/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/action/acceleration/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' diff --git a/src/moa/moa_bringup/launch/sim_launch.py b/src/moa/moa_bringup/launch/sim_launch.py index 8ef27272..d0042c83 100644 --- a/src/moa/moa_bringup/launch/sim_launch.py +++ b/src/moa/moa_bringup/launch/sim_launch.py @@ -4,11 +4,18 @@ def generate_launch_description(): return launch.LaunchDescription([ # cone map - # launch_ros.actions.Node( - # package='cone_mapping', - # executable='dbscan', - # name='dbscan', - # ), + launch_ros.actions.Node( + package='aruco_detection', + executable='aruco_detection', + name='aruco_detection' + ), + + # cone mapping + launch_ros.actions.Node( + package='cone_mapping', + executable='dbscan', + name='dbscan', + ), # path optimization launch_ros.actions.Node( @@ -19,8 +26,8 @@ def generate_launch_description(): # path viz launch_ros.actions.Node( - package='path_planning_visualization', - executable='visualize2', + package='path_planning', + executable='shortest_path_viz', name='path_viz', ), @@ -30,4 +37,11 @@ def generate_launch_description(): executable='visualizer', name='track_viz', ), + + # launch_ros.actions.Node( + # package='foxglove_bridge', + # executable='foxglove_bridge', + # name='foxglove_bridge', + # parameters=[{'port':8765}] + # ), ]) \ No newline at end of file diff --git a/src/moa/moa_controllers/moa_controllers/trajectory_follower_p_controller.py b/src/moa/moa_controllers/moa_controllers/trajectory_follower_p_controller.py index ea4c43bf..23f261f2 100644 --- a/src/moa/moa_controllers/moa_controllers/trajectory_follower_p_controller.py +++ b/src/moa/moa_controllers/moa_controllers/trajectory_follower_p_controller.py @@ -24,7 +24,7 @@ def __init__(self): # ('debug', False) # ] # ) - self._distance_to_front = 3.0 + self._distance_to_front = 0.9 qos_profile = QoSProfile( reliability=QoSReliabilityPolicy.BEST_EFFORT, @@ -84,7 +84,7 @@ def callback(self, msg:ConeMap): # publish msgs args = {"steering_angle": float(steering_angle_deg), "steering_angle_velocity": 0.0, - "speed": 7.0, + "speed": 3.0, "acceleration": 0.0, "jerk": 0.0} diff --git a/src/planning/path_planning/path_planning/shortest_path/CoreModels.py b/src/planning/path_planning/path_planning/shortest_path/CoreModels.py index f9ebc957..e9e8def0 100644 --- a/src/planning/path_planning/path_planning/shortest_path/CoreModels.py +++ b/src/planning/path_planning/path_planning/shortest_path/CoreModels.py @@ -11,7 +11,7 @@ def __init__(self, node, entryVector, velocity=0.0, cost=np.inf, previousNode=Fa self._previousNode = previousNode self._nextState = nextState self._max = Imax - self._min = Imin + # self._min = Imin class Node(): def __init__(self, bid, xy, innerdistance, outerdistance) -> None: diff --git a/src/planning/path_planning/path_planning/shortest_path/PathHelpers.py b/src/planning/path_planning/path_planning/shortest_path/PathHelpers.py index 4f3b0c3d..2b0e8005 100644 --- a/src/planning/path_planning/path_planning/shortest_path/PathHelpers.py +++ b/src/planning/path_planning/path_planning/shortest_path/PathHelpers.py @@ -35,3 +35,13 @@ def noughtTo60(nought_to_60): Converts nought to 60 to linear acceleration """ return 60/nought_to_60 * 0.44704 + +def getTraverseTime(distance_between_nodes, current_state, next_node_state): + dist = distance_between_nodes + c_vel = current_state._velocity # current state velocity + n_vel = next_node_state._velocity # next state velocity + n_cost = next_node_state._cost # next state cost + dist_bound = max(next_node_state._node._innerDistance, next_node_state._node._outerDistance) + + objective = ((2*dist)/(c_vel+n_vel)) + n_cost + np.sqrt(dist_bound) + return objective diff --git a/src/planning/path_planning/path_planning/shortest_path/Race lines/$track_name optimal.png b/src/planning/path_planning/path_planning/shortest_path/Race lines/$track_name optimal.png index dd222d6e..499223f1 100644 Binary files a/src/planning/path_planning/path_planning/shortest_path/Race lines/$track_name optimal.png and b/src/planning/path_planning/path_planning/shortest_path/Race lines/$track_name optimal.png differ diff --git a/src/planning/path_planning/path_planning/shortest_path/Track images/imported track.png b/src/planning/path_planning/path_planning/shortest_path/Track images/imported track.png index a285869b..f6c7e473 100644 Binary files a/src/planning/path_planning/path_planning/shortest_path/Track images/imported track.png and b/src/planning/path_planning/path_planning/shortest_path/Track images/imported track.png differ diff --git a/src/planning/path_planning/path_planning/shortest_path/Track images/nodes.png b/src/planning/path_planning/path_planning/shortest_path/Track images/nodes.png index cd6d1454..1202b64f 100644 Binary files a/src/planning/path_planning/path_planning/shortest_path/Track images/nodes.png and b/src/planning/path_planning/path_planning/shortest_path/Track images/nodes.png differ diff --git a/src/planning/path_planning/path_planning/shortest_path/TrackHelpers.py b/src/planning/path_planning/path_planning/shortest_path/TrackHelpers.py index 9894cc0d..d71ea5ea 100644 --- a/src/planning/path_planning/path_planning/shortest_path/TrackHelpers.py +++ b/src/planning/path_planning/path_planning/shortest_path/TrackHelpers.py @@ -151,10 +151,13 @@ def Plot(nodes:bool, vector_list:np.array, label:str,col=None): def plotOptimal(vector_list:np.array, velocities:np.array, label:str): all_x = [P[0] for P in vector_list] all_y = [P[1] for P in vector_list] - col = cm.jet((velocities-np.min(velocities))/(np.max(velocities)-np.min(velocities))) - ax = plt.gca() + # col = cm.jet((velocities-np.min(velocities))/(np.max(velocities)-np.min(velocities))) + cmap = plt.cm.get_cmap("autumn_r") + ax = plt.subplot() for i in range(len(all_x)-1): - ax.plot([all_x[i], all_x[i+1]], [all_y[i], all_y[i+1]], c=col[i], label=label) - im = ax.scatter(all_x, all_y, c=velocities, s=0, cmap=cm.jet) + ax.plot([all_x[i], all_x[i+1]], [all_y[i], all_y[i+1]], c=cmap(velocities[i]/np.max(velocities)), label=label) + im = ax.scatter(all_x, all_y, c=velocities, s=0, cmap=cmap) + # im = cm.ScalarMappable(cmap=cmap) + # im = im.set_array(velocities) return im diff --git a/src/planning/path_planning/path_planning/shortest_path/TrackMethods.py b/src/planning/path_planning/path_planning/shortest_path/TrackMethods.py index 8aff6093..312ea0dd 100644 --- a/src/planning/path_planning/path_planning/shortest_path/TrackMethods.py +++ b/src/planning/path_planning/path_planning/shortest_path/TrackMethods.py @@ -91,7 +91,8 @@ def importTrack(track_info:pd.DataFrame=None, trackname:str=None, plot:bool=Fals "outer": outer, "inner": inner, "cline": center_points.tolist(), - "p_vector": p_vector + "p_vector": p_vector, + "lap_distance": lap_distance, }) if plot: @@ -285,6 +286,20 @@ def getBrackets(df:pd.DataFrame, n_nodes, plot:bool=False): return brackets +def getSectionofTrack(df, brackets, d_start, d_end): + df_range = np.where(df.lap_distance[df.lap_distance <= d_end] >= d_start)[0] + + df = pd.DataFrame({ + "outer": df.outer[df_range], + "inner": df.inner[df_range], + "cline": df.cline[df_range], + "p_vector": df.p_vector[df_range], + "lap_distance": df.lap_distance[df_range], + }) + brackets = brackets[df_range] + + return df, brackets + # def belman_ford_path(df, velocity_range, brackets, start_node, track_name=None, plot:bool=False): # # Initialise first set of paths from first bracket # for node in brackets[-1]._nodeList: @@ -356,22 +371,28 @@ def getBrackets(df:pd.DataFrame, n_nodes, plot:bool=False): # return start_node -def optimal_path(track_name:str, df:pd.DataFrame, start_node:Node, brackets:np.array, n_vel, μ, mass, α, α_d, max_steer_angle, tire_width, wheelbase, max_velocity, plot): +def optimal_path(track_name:str, car_position, df:pd.DataFrame, start_node:Node, brackets:np.array, n_vel, CAR:dict, plot:bool): """keeps ALL state from each pair of node state combination""" + attributes = ["mass", "μ", "α", "α_d", "max steer angle", "max velocity", "tire width", "wheelbase"] + mass, μ, α, α_d, max_steer_angle, max_velocity, tire_width, wheelbase = [CAR[key] for key in attributes] + traction_force = PathHelpers.getMaxTractionForce(μ, mass) velocity_range = np.linspace(0, max_velocity, n_vel) - min_steer_rad= wheelbase/np.sin(np.deg2rad(max_steer_angle)) + 0.5* tire_width + min_steer_rad = wheelbase/np.sin(np.deg2rad(max_steer_angle)) + 0.5*tire_width for node in brackets[-1]._nodeList: for velocity in velocity_range: for previous_node in brackets[-2]._nodeList: entry_vector = TrackHelpers.getVector(previous_node._xy, node._xy, True) - state = State(node, entry_vector, velocity, 0.0, previous_node, False) + state = State(node, entry_vector, velocity, 0.0, previous_node) node._stateList.append(state) + + start_node._stateList[0]._previousNode = Node(0,start_node._xy - 2*wheelbase*start_node._stateList[0]._entryVector, 0, 0) for i in range(len(brackets)-1,0,-1): tmp = rclpyNode("tmp") tmp.get_logger().info(f"Bracket: {i}\n") + print(f"Bracket: {i}\n") if i == 2: previous_node_list = [start_node]; @@ -383,13 +404,13 @@ def optimal_path(track_name:str, df:pd.DataFrame, start_node:Node, brackets:np.a current_node_list = brackets[i-1]._nodeList previous_node_list = brackets[i-2]._nodeList; - for current_node in current_node_list: + def process_current_node(current_node): + # for current_node in current_node_list: # current_node._stateList=[] # Initialising statelist for the current node for previous_node in previous_node_list: entry_vector = TrackHelpers.getVector(previous_node._xy, current_node._xy, True) current_node._stateList.append(State(current_node, entry_vector, 0.0, np.inf, previous_node, False, True)) - current_node._stateList.append(State(current_node, entry_vector, 0.0, np.inf, previous_node, False, False, True)) for velocity in velocity_range: state = State(current_node, entry_vector, velocity, np.inf, previous_node) current_node._stateList.append(state) @@ -397,50 +418,60 @@ def optimal_path(track_name:str, df:pd.DataFrame, start_node:Node, brackets:np.a for next_node in brackets[i]._nodeList: distance_between_nodes = TrackHelpers.getDistance(current_node._xy, next_node._xy) for current_state in current_node._stateList: - prev_xy = current_state._xy-np.array(current_state._entryVector)*distance_between_nodes if i == 1 else current_state._previousNode._xy - traction_velocity, radius = PathHelpers.getTractionVelocity3p(prev_xy, current_node._xy,next_node._xy,traction_force, mass) + # prev_xy = current_state._previousNode._xy if i != 1 else current_state._xy-np.array(current_state._entryVector)*distance_between_nodes + traction_velocity, radius = PathHelpers.getTractionVelocity3p(current_state._previousNode._xy, current_node._xy, next_node._xy, traction_force, mass) + # print(traction_velocity, radius) if radius < min_steer_rad: continue # skip current current state for next_node_state in next_node._stateList: if next_node_state._previousNode == current_node: - min_va, max_va = PathHelpers.minmaxAccelerationVelocity(next_node_state._velocity, distance_between_nodes,α,α_d) + min_va, max_va = PathHelpers.minmaxAccelerationVelocity(next_node_state._velocity, distance_between_nodes, α, α_d) if traction_velocity < min_va: continue # skip next state - if current_state._min: - # accelerating, optimal minimum speed state - ideal_velocity = min_va - traverse_time = ((2*distance_between_nodes)/(ideal_velocity+next_node_state._velocity)) + next_node_state._cost - if traverse_time < current_state._cost: - current_state._velocity = ideal_velocity - current_state._cost = traverse_time - current_state._nextState = next_node_state - elif current_state._max: + # if current_state._min: + # # accelerating, optimal minimum speed state + # ideal_velocity = min_va + # traverse_time = ((2*distance_between_nodes)/(ideal_velocity+next_node_state._velocity)) + next_node_state._cost + # if traverse_time < current_state._cost: + # current_state._velocity = ideal_velocity + # current_state._cost = traverse_time + # current_state._nextState = next_node_state + if current_state._max: # braking, optimum maximum speed state ideal_velocity = min(traction_velocity, max_va, max_velocity) - traverse_time = ((2*distance_between_nodes)/(ideal_velocity+next_node_state._velocity)) + next_node_state._cost + tmp = current_state._velocity + current_state._velocity = ideal_velocity + traverse_time = PathHelpers.getTraverseTime(distance_between_nodes, current_state, next_node_state) if traverse_time < current_state._cost: current_state._velocity = ideal_velocity current_state._cost = traverse_time current_state._nextState = next_node_state + else: + current_state._velocity = tmp # non ideal state - if min_va <= current_state._velocity <= min(traction_velocity, max_va, max_velocity): - traverse_time = ((2*distance_between_nodes)/(current_state._velocity+next_node_state._velocity)) + next_node_state._cost + elif min_va <= current_state._velocity < min(traction_velocity, max_va, max_velocity): + traverse_time = PathHelpers.getTraverseTime(distance_between_nodes, current_state, next_node_state) if traverse_time < current_state._cost: + # print('called') current_state._cost = traverse_time current_state._nextState = next_node_state - + # for state in current_node._stateList # if state._cost == Inf; deleteState!(state); end - + # executor = concurrent.futures.ProcessPoolExecutor(8) + # futures = [executor.submit(process_current_node, current_node) for current_node in current_node_list] + # concurrent.futures.wait(futures) + _ = list(map(process_current_node, current_node_list)) + # _ = [process_current_node(n) for n in current_node_list] # get optimal/best path by iterating through EVERY SINGLE state LOL print("getting best path") best_xy, best_velocities, cost = getBestStates(start_node) - print(best_xy, "\n", best_velocities,"\n") + print(best_xy, "\n\n", best_velocities,"\n") if plot: # plotting @@ -450,11 +481,25 @@ def optimal_path(track_name:str, df:pd.DataFrame, start_node:Node, brackets:np.a TrackHelpers.Plot(False, df.inner, "inner boundary") # outer boundary TrackHelpers.Plot(False, df.outer, "outer boundary") + # actual car position + plt.plot(car_position[0], car_position[1], "or", label="actual car position") # optimal path im = TrackHelpers.plotOptimal(best_xy, best_velocities, "optimal race line") - plt.colorbar(im) + plt.colorbar(im, label='velocity (m/s)') + plt.title("Optimal Trajectory Colored by Velocity") + + # nodes + for i,B in enumerate(brackets): + all_nodes = B._nodeList + if i == len(brackets)-1: + col = "green" + else: + col = "black" + TrackHelpers.Plot(True, all_nodes, "nodes", col) + p.savefig(f"{os.path.dirname(__file__)}/Race lines/{track_name}.png", dpi=600) + plt.legend(["left boundary", "right boundary", "actual car position", "optimal race line"]) plt.show() return start_node, brackets, cost/60 @@ -465,7 +510,6 @@ def getBestStates(start_node:Node): velocities = [] for state in start_node._stateList: current_state = state - tmp_cost = current_state._cost while current_state: best_xy.append(current_state._xy) velocities.append(current_state._velocity) diff --git a/src/planning/path_planning/path_planning/shortest_path/main.py b/src/planning/path_planning/path_planning/shortest_path/main.py index bf0a9b59..de8707cf 100644 --- a/src/planning/path_planning/path_planning/shortest_path/main.py +++ b/src/planning/path_planning/path_planning/shortest_path/main.py @@ -1,9 +1,25 @@ # main file +# imports +# import CoreModels +# import PathHelpers +from path_planning.shortest_path.CoreModels import Node, State +import path_planning.shortest_path.TrackMethods as TrackMethods +import path_planning.shortest_path.TrackHelpers as TrackHelpers +import path_planning.shortest_path.PathHelpers as PathHelpers + +import numpy as np + def main(): - # imports - # import CoreModels - # import PathHelpers - import planning.path_planning.path_planning.shortest_path.TrackMethods as TrackMethods + CAR = { + "mass": 84.5, # kg + "μ": 0.3, # static friction coefficient - dimensionless + "α": PathHelpers.noughtTo60(3.0), + "α_d": 15.0, # max decel in m/s^2 + "max steer angle": 25.0, # degrees + "max velocity": 10.0, # m/s + "tire width": 0.11, # in m + "wheelbase": 1.5, # wheelbase length (in m) + } # choose track track_name = "Silverstone" @@ -17,21 +33,55 @@ def main(): # create brackets print("CREATING BRACKETS") - brackets = TrackMethods.getBrackets(df, 10) - - velocity_range = [0.01, 8, 16, 24, 32, 40 ]# velocities in meters per second + brackets = TrackMethods.getBrackets(df, 8, False) + d_start = 300 + d_end = 400 + section_of_track = True + if section_of_track: + df, brackets = TrackMethods.getSectionofTrack(df, np.array(brackets), d_start, d_end) print("COMPUTING OPTIMAL PATH") - start_node = brackets[0]._nodeList[5] - start_node._cost = 0 - print("starting inner distance: ",start_node._innerDistance) - print("starting outer distance: ", start_node._outerDistance) # mass = 50.0 # μ = 0.7 - start_node = TrackMethods.belman_ford_path(df, velocity_range, brackets, start_node, track_name) + # car_position = brackets[8]._nodeList[4]._xy + [0, -5] + # current_position, brackets = getStartingPosition(car_position, brackets) + # start_node = get_start_node(starting_point=current_position) + start_node = brackets[0]._nodeList[4] + print(-1/df.p_vector.tolist()[0]) + start_node._stateList.append(State(start_node, -1/df.p_vector.tolist()[0], 0.0, np.Inf)) + print("starting inner distance: ",start_node._innerDistance) + print("starting outer distance: ", start_node._outerDistance) + # start_node = TrackMethods.belman_ford_path(df, velocity_range, brackets, start_node, plot=self._plot) + n_vel = 10 + start_node, brackets, optimal_cost = TrackMethods.optimal_path( + "$track_name optimal", + df, + start_node, + brackets, + n_vel, + CAR, + True, + ) # start_node = TrackMethods.optimise_path(track_name, df, brackets, start_node, mass, μ) print("\nOPTIMAL PATH COMPUTED") +def getStartingPosition(car_position, brackets): + best_dist = np.Inf + best_bracket_idx = 0 + for i, B in enumerate(brackets): + dists = [TrackHelpers.getDistance(car_position, node._xy) for node in B._nodeList] + if min(dists) < best_dist: + best_dist = min(dists) + best_bracket_idx = i + starting_point = brackets[i]._nodeList[np.argmin(dists)]._xy + # delete brackets before the starting position + brackets = brackets[best_bracket_idx:] + + return starting_point, brackets + +def get_start_node(starting_point): + return Node(1, np.array(starting_point), None, None) + if __name__ == "__main__": main() \ No newline at end of file diff --git a/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py b/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py index 14487d61..9110aa2c 100644 --- a/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py +++ b/src/planning/path_planning/path_planning/trajectory_optimisation_CS.py @@ -75,20 +75,20 @@ def set_boundaries(self, msg: ConeMap): elif cones[i].colour == 2: rightboundary.append([x,y]) - # interpolate - funcL = interpolate.interp1d([P[0] for P in leftboundary], [P[1] for P in leftboundary], kind='slinear') - funcR = interpolate.interp1d([P[0] for P in rightboundary], [P[1] for P in rightboundary], kind='slinear') - - xlrange = np.linspace(min([P[0] for P in leftboundary]), max([P[0] for P in leftboundary])) - xrrange = np.linspace(min([P[0] for P in rightboundary]), max([P[0] for P in rightboundary])) - self.get_logger().info(f"length of xlrange = {len(xlrange)}") - leftboundary = [] - rightboundary = [] - for P in xlrange: - leftboundary.append([P,funcL(P)]) - - for P in xrrange: - rightboundary.append([P,funcR(P)]) + # # interpolate + # funcL = interpolate.interp1d([P[0] for P in leftboundary], [P[1] for P in leftboundary], kind='slinear') + # funcR = interpolate.interp1d([P[0] for P in rightboundary], [P[1] for P in rightboundary], kind='slinear') + + # xlrange = np.linspace([P[0] for P in leftboundary][0], [P[0] for P in leftboundary][-1], 50) + # xrrange = np.linspace([P[0] for P in rightboundary][0], [P[0] for P in rightboundary][-1], 50) + # self.get_logger().info(f"length of xlrange = {len(xlrange)}") + # leftboundary = [] + # rightboundary = [] + # for P in xlrange: + # leftboundary.append([P,funcL(P)]) + + # for P in xrrange: + # rightboundary.append([P,funcR(P)]) # get correspnoding right boundary based on left boundary x1, y1, x2, y2 = leftboundary[0][0], leftboundary[0][1], rightboundary[0][0], rightboundary[0][1] @@ -507,10 +507,10 @@ def get_center_line(self): coods.append(self.get_avg_point(x1,y1,x2,y2)) # # perform extrapolation to extend line - # func = interpolate.interp1d([P[0] for P in coods], [P[1] for P in coods], kind='cubic', fill_value='extrapolate') + # func = interpolate.interp1d([P[0] for P in coods], [P[1] for P in coods], kind='quadratic') # into_future_points = 0 - # into_future_distance = 0 + # into_future_distance = 0.1 # # track direction # if coods[-1][0] > coods[-2][0]: diff --git a/src/planning/path_planning/path_planning/trajectory_shortest_path.py b/src/planning/path_planning/path_planning/trajectory_shortest_path.py index 881ea9fd..301cd0c7 100644 --- a/src/planning/path_planning/path_planning/trajectory_shortest_path.py +++ b/src/planning/path_planning/path_planning/trajectory_shortest_path.py @@ -9,6 +9,7 @@ # from shapely import LineString, MultiPoint # from shapely import Point as shapelyPoint import os +import matplotlib.pyplot as plt import rclpy from rclpy.node import Node as NODE @@ -29,46 +30,70 @@ def __init__(self): namespace='', parameters=[ ('plot', True), - ('save_track', True), + ('save_track', False), ] ) # attributes self._plot = self.get_parameter("plot").get_parameter_value().bool_value self._save_track = self.get_parameter("save_track").get_parameter_value().bool_value + self._sim = False + # car properties self._CAR = { - "mass": 200.0, # kg - "μ": 0.9, # static friction coefficient - dimensionless - "α": PathHelpers.noughtTo60(2.1), - "α_d": 39.0, - "max steer angle": 16.0, # degrees - "max velocity": 80.0, # m/s - "tire width": 18/39.37, # in m (18 inches here) - "wheelbase": 3.6, # wheelbase length (m? - LIAM TO CONFIRM) + "mass": 84.5, # kg + "μ": 0.6, # static friction coefficient - dimensionless + "α": PathHelpers.noughtTo60(3.0), + "α_d": 15.0, # max decel in m/s^2 + "max steer angle": 25.0, # degrees + "max velocity": 10.0, # m/s + "tire width": 0.11, # in m + "wheelbase": 1.5, # wheelbase length (in m) } # subscribers - self.create_subscription(ConeMap, "cone_map", self.set_boundaries, 10) + self.create_subscription(ConeMap, "cone_map", self.callback, 10) # publishers self.steering_angle = self.create_publisher(Float32, "/test/cmd_steering", 10) self.best_trajectory_publisher = self.create_publisher(PoseArray, "moa/selected_trajectory", 10) - def set_boundaries(self, msg:ConeMap): - innerboundary, outerboundary, car_position, oc, ic = self.get_boundaries(msg.cones) + def callback(self, msg:ConeMap): + innerboundary, outerboundary, car_position, w, oc, ic = self.get_boundaries(msg.cones) + self.get_logger().info(f"car orientation = {w}") # saving track if self._save_track: self.save_track(innerboundary, outerboundary) - # innerboundary.reverse() - # outerboundary.reverse() + + if self._sim: + # FOR SOME REASON THE CONE LIST IN THE SIM COMES OUT AS OPPOSITE WITH CONES FURTHEST AWAY AT FIRST INDEX + # DOEST HAPPEN WITH FAKE CONE DATA + innerboundary.reverse() + outerboundary.reverse() # smaller (incl. negative) x, y come before innerboundary = np.array(innerboundary) outerboundary = np.array(outerboundary) # self.get_logger().info(f"bound: {innerboundary}") + # you want pairs of cones for path planning to work - HAPPENDS IN SIM & FAKE CONE + if len(innerboundary) > len(outerboundary): + innerboundary = innerboundary[:len(outerboundary)] + elif len(outerboundary) > len(innerboundary): + outerboundary = outerboundary[:len(innerboundary)] + + if self._plot: + xl = [P[0] for P in innerboundary] + yl = [P[1] for P in innerboundary] + xr = [P[0] for P in outerboundary] + yr = [P[1] for P in outerboundary] + plt.plot(xl, yl, "ob", label="left boundary") + plt.plot(xr, yr, "og", label="right boundary") + plt.plot(car_position[0],car_position[1], "or", label="car position") + plt.legend() + plt.show() + # if ic > oc: innerboundary = innerboundary[:(oc-ic)] # if oc > ic: outerboundary = outerboundary[:(ic-oc)] @@ -86,47 +111,45 @@ def set_boundaries(self, msg:ConeMap): print("IMPORTING TRACK") # df = TrackMethods.importTrack(track_info=track_info, plot=self._plot) - df = self.create_track_dataframe(innerboundary, outerboundary) + df = self.create_track_dataframe(innerboundary, outerboundary) # both boundaries should be equal length # create brackets print("CREATING BRACKETS") - brackets = TrackMethods.getBrackets(df, 8, plot=self._plot) - - # velocity_range = [0.01, 8, 16, 24, 32, 40]# velocities in meters per second + brackets = TrackMethods.getBrackets(df, 5, plot=self._plot) # compute optimal path print("COMPUTING OPTIMAL PATH") - start_node = self.get_start_node(starting_point=car_position) # transformed car position - start_node._stateList.append(State(start_node, [np.cos(0), np.sin(0)], 0.0)) + # car_position = np.array(car_position) + (np.random.rand(2)*2) + start_node = brackets[int(np.random.randint(0,5))]._nodeList[int(np.random.randint(0,6))] + car_position = start_node._xy + + current_position, brackets = self.getStartingPosition(car_position, brackets) + start_node = self.get_start_node(starting_point=current_position) # transformed car position + angle = np.pi / 2 + start_node._stateList.append(State(start_node, np.array([np.cos(angle), np.sin(angle)]), 0.0, np.Inf)) print("starting inner distance: ", start_node._innerDistance) print("starting outer distance: ", start_node._outerDistance) # start_node = TrackMethods.belman_ford_path(df, velocity_range, brackets, start_node, plot=self._plot) - n_vel = 5 + n_vel = 10 start_node, brackets, optimal_cost = TrackMethods.optimal_path( "$track_name optimal", + car_position, df, start_node, brackets, n_vel, - self._CAR["μ"], - self._CAR["mass"], - self._CAR["α"], - self._CAR["α_d"], - self._CAR["max steer angle"], - self._CAR["tire width"], - self._CAR["wheelbase"], - self._CAR["max velocity"], + self._CAR, self._plot ) print("\nOPTIMAL PATH COMPUTED") # get steering angle based on current and next point - # p1 = start_node._xy # relative to global + p1 = start_node._xy # relative to global # self.get_logger().info(f"{p1}") # self.get_logger().info(f"{start_node._nextNode._xy}") # # p2 = self.get_transformed_point(msg, start_node._nextNode._xy) # relative to local - # p2 = start_node._nextNode._xy + p2 = start_node._stateList[0]._nextState._xy # self.get_logger().info(f"{p2}") # # p2 = start_node._nextNode._xy + start_node._xy # steering_angle = TrackHelpers.getAngleRotation(np.array(p1), np.array(p2)) @@ -135,18 +158,34 @@ def set_boundaries(self, msg:ConeMap): # publish msgs # self.steering_angle.publish(Float32(data=steering_angle)) - - # points = [p1, p2] - # msg = PoseArray() - # for P in points: - # args = {"position": Point(x=P[0], y=P[1], z=0.0)} - # msg.poses.append(Pose(**args)) - # self.best_trajectory_publisher.publish(msg) + p3 = start_node._stateList[0]._nextState._nextState._xy + points = [p1, p2, p3] + msg = PoseArray() + for P in points: + args = {"position": Point(x=P[0], y=P[1], z=0.0)} + msg.poses.append(Pose(**args)) + self.best_trajectory_publisher.publish(msg) # self.get_logger().info(f"steering angle published: {steering_angle}") return + def getStartingPosition(self, car_position, brackets): + best_dist = np.Inf + best_bracket_idx = 0 + for i, B in enumerate(brackets): + dists = [TrackHelpers.getDistance(car_position, node._xy) for node in B._nodeList] + if min(dists) < best_dist: + best_dist = min(dists) + best_bracket_idx = i + starting_point = brackets[i]._nodeList[np.argmin(dists)]._xy + # delete brackets before the starting position + # brackets = brackets[best_bracket_idx:best_bracket_idx+5] + brackets = brackets[best_bracket_idx:] + + return starting_point, brackets + + def get_boundaries(self, cones): # loop through each cone innerboundary = [] @@ -158,6 +197,7 @@ def get_boundaries(self, cones): y = cones[i].pose.pose.position.y if i == 0: car_position = [x,y] + w = cones[i].pose.pose.orientation.w else: # blue - left if cones[i].colour == 0: @@ -167,7 +207,7 @@ def get_boundaries(self, cones): outerboundary.append([x,y]) oc += 1 - return innerboundary, outerboundary, car_position, oc, ic + return innerboundary, outerboundary, car_position, w, oc, ic def save_track(self, innerboundary, outerboundary): with open(f'/{os.path.dirname(__file__)}/bound_coods', 'w') as fh: @@ -191,10 +231,10 @@ def save_track(self, innerboundary, outerboundary): def create_track_dataframe(self, innerboundary:np.array, outerboundary:np.array): - up_to = min(len(innerboundary), len(outerboundary)) + # up_to = min(len(innerboundary), len(outerboundary)) return pd.DataFrame({ - "inner": list(innerboundary[:up_to]), - "outer": list(outerboundary[:up_to]), + "inner": list(innerboundary), + "outer": list(outerboundary), }) diff --git a/src/planning/path_planning/setup.py b/src/planning/path_planning/setup.py index 44bc96b3..986b8571 100644 --- a/src/planning/path_planning/setup.py +++ b/src/planning/path_planning/setup.py @@ -29,7 +29,8 @@ 'trajectory_optimisation = path_planning.trajectory_optimisation_CS:main', 'center_line = path_planning.simple_centerline_planner:main', 'shortest_path = path_planning.trajectory_shortest_path:main', - 'shortest_path_viz = path_planning.shortest_path_viz:main' + 'shortest_path_viz = path_planning.shortest_path_viz:main', + 'trajectory_generation = path_planning.trajectory_generation:main' ], }, )