From 95d5fc70839b05396ca8d3d88c855725119c7b54 Mon Sep 17 00:00:00 2001 From: qev3-d Date: Tue, 7 May 2024 12:30:09 +1000 Subject: [PATCH 01/11] track updates --- .../config/pure_pursuit_cpp.yaml | 10 ++-- .../map_creation/node_cone_placement.hpp | 8 ++-- .../map_creation/src/node_cone_placement.cpp | 4 +- .../config/localisation_params.yaml | 12 ++--- .../planners/planners/node_ft_planner.py | 46 +++++++++++++------ 5 files changed, 52 insertions(+), 28 deletions(-) diff --git a/src/control/pure_pursuit_cpp/config/pure_pursuit_cpp.yaml b/src/control/pure_pursuit_cpp/config/pure_pursuit_cpp.yaml index 1d9325e1f..cb21ceaed 100644 --- a/src/control/pure_pursuit_cpp/config/pure_pursuit_cpp.yaml +++ b/src/control/pure_pursuit_cpp/config/pure_pursuit_cpp.yaml @@ -1,9 +1,9 @@ pure_pursuit_cpp_node: ros__parameters: - Kp_ang: -3.5 + Kp_ang: -4 lookahead: 2.5 - vel_max: 4.0 - vel_min: 3.0 + vel_max: 6.0 + vel_min: 4.0 discovery_lookahead: 3.0 - discovery_vel_max: 4.0 - discovery_vel_min: 3.0 + discovery_vel_max: 6.0 + discovery_vel_min: 4.0 diff --git a/src/navigation/map_creation/include/map_creation/node_cone_placement.hpp b/src/navigation/map_creation/include/map_creation/node_cone_placement.hpp index 05f026e21..8a8be0526 100644 --- a/src/navigation/map_creation/include/map_creation/node_cone_placement.hpp +++ b/src/navigation/map_creation/include/map_creation/node_cone_placement.hpp @@ -84,11 +84,13 @@ struct TrackedCone { colour = driverless_msgs::msg::Cone::YELLOW; } else if (blue_count > yellow_count && blue_count > orange_count) { colour = driverless_msgs::msg::Cone::BLUE; + } else if (orange_count > yellow_count && orange_count > blue_count) { + colour = driverless_msgs::msg::Cone::ORANGE_SMALL; } - if (orange_count > 10 || orange_count > yellow_count || orange_count > blue_count) { - colour = driverless_msgs::msg::Cone::ORANGE_BIG; - } + // if (orange_count > 10 || orange_count > yellow_count || orange_count > blue_count) { + // colour = driverless_msgs::msg::Cone::ORANGE_BIG; + // } } // transform map to car diff --git a/src/navigation/map_creation/src/node_cone_placement.cpp b/src/navigation/map_creation/src/node_cone_placement.cpp index ee83924c7..0e5172091 100644 --- a/src/navigation/map_creation/src/node_cone_placement.cpp +++ b/src/navigation/map_creation/src/node_cone_placement.cpp @@ -37,7 +37,8 @@ ConeAssociation::ConeAssociation() : Node("cone_placement_node") { void ConeAssociation::state_callback(const driverless_msgs::msg::State::SharedPtr msg) { // we haven't started driving yet - if (msg->state == driverless_msgs::msg::State::DRIVING && msg->lap_count == 0) { + // if (msg->state == driverless_msgs::msg::State::DRIVING && msg->lap_count == 0) { + if (msg->lap_count == 0) { mapping = true; } @@ -50,6 +51,7 @@ void ConeAssociation::state_callback(const driverless_msgs::msg::State::SharedPt void ConeAssociation::callback(const driverless_msgs::msg::ConeDetectionStamped::SharedPtr msg) { if (!mapping) { + RCLCPP_INFO_ONCE(get_logger(), "Not ready to map"); return; } diff --git a/src/navigation/nav_bringup/config/localisation_params.yaml b/src/navigation/nav_bringup/config/localisation_params.yaml index 6ddc0be68..563c16ffb 100644 --- a/src/navigation/nav_bringup/config/localisation_params.yaml +++ b/src/navigation/nav_bringup/config/localisation_params.yaml @@ -22,12 +22,12 @@ odom_filter_node: false, false, false, # roll_dot, pitch_dot, yaw_dot false, false, false] # x_ddot, y_ddot, z_ddot - twist0: vehicle/wheel_twist - twist0_config: [false, false, false, # x, y, z - false, false, false, # roll, pitch, yaw - true, false, false, # x_dot, y_dot, z_dot - false, false, false, # roll_dot, pitch_dot, yaw_dot - false, false, false] # x_ddot, y_ddot, z_ddot + # twist0: vehicle/wheel_twist + # twist0_config: [false, false, false, # x, y, z + # false, false, false, # roll, pitch, yaw + # true, false, false, # x_dot, y_dot, z_dot + # false, false, false, # roll_dot, pitch_dot, yaw_dot + # false, false, false] # x_ddot, y_ddot, z_ddot odom1: odometry/sbg_ekf odom1_config: [true, true, false, # x, y, z diff --git a/src/navigation/planners/planners/node_ft_planner.py b/src/navigation/planners/planners/node_ft_planner.py index fcda63b8c..c0d72edd1 100644 --- a/src/navigation/planners/planners/node_ft_planner.py +++ b/src/navigation/planners/planners/node_ft_planner.py @@ -216,8 +216,8 @@ def get_planner_cfg(self): self.declare_parameter("max_dist", 6.5) self.declare_parameter("max_dist_to_first", 6.0) self.declare_parameter("max_length", 12) - self.declare_parameter("threshold_directional_angle", 40) - self.declare_parameter("threshold_absolute_angle", 65) + self.declare_parameter("threshold_directional_angle", 30) + self.declare_parameter("threshold_absolute_angle", 55) self.declare_parameter("use_unknown_cones", True) cone_sorting_kwargs = { @@ -295,6 +295,22 @@ def planning_callback(self): return # skip if we haven't completed a lap yet + if not self.initialised: + ( + path, + ordered_blues, + ordered_yellows, + virt_blues, + virt_yellows, + _, + _, + ) = self.path_planner.calculate_path_in_global_frame( + pre_track, np.array([0.0,0.0]), 0.0, return_intermediate_results=True + ) + self.initialised = True + self.get_logger().info("Initialised planner calcs") + return + if self.current_track is None or len(self.current_track.cones) == 0: self.get_logger().warn("No track data received", throttle_duration_sec=1) return @@ -338,17 +354,21 @@ def planning_callback(self): global_cones = [unknown_cones, yellow_cones, blue_cones, orange_small_cones, orange_big_cones] - ( - path, - ordered_blues, - ordered_yellows, - virt_blues, - virt_yellows, - _, - _, - ) = self.path_planner.calculate_path_in_global_frame( - global_cones, car_position, car_direction, return_intermediate_results=True - ) + try: + ( + path, + ordered_blues, + ordered_yellows, + virt_blues, + virt_yellows, + _, + _, + ) = self.path_planner.calculate_path_in_global_frame( + global_cones, car_position, car_direction, return_intermediate_results=True + ) + except Exception as e: + self.get_logger().warn("Cant plan, error" + str(e), throttle_duration_sec=1) + return use_virt = True if use_virt: From 5561c47f713df88b96cc41acc91fe327b1f9b40b Mon Sep 17 00:00:00 2001 From: Alastair Date: Sun, 5 May 2024 11:43:42 +1000 Subject: [PATCH 02/11] beginning to integrate nav2 control. starting with RPP on the new FT planner instead of our pure pursuit --- .../controllers/node_vel_to_ackermann.py | 53 +++++ src/control/controllers/setup.py | 1 + .../nav_bringup/config/nav2_params.yaml | 8 +- .../nav_bringup/launch/nav2_bringup.launch.py | 14 +- .../launch/trackdrive.launch.py | 19 +- .../node_mission_launcher.py | 18 +- .../node_trackdrive_handler_plan.py | 190 ++++++++++++++++++ src/operations/mission_controller/setup.py | 1 + 8 files changed, 278 insertions(+), 26 deletions(-) create mode 100644 src/control/controllers/controllers/node_vel_to_ackermann.py create mode 100644 src/operations/mission_controller/mission_controller/node_trackdrive_handler_plan.py diff --git a/src/control/controllers/controllers/node_vel_to_ackermann.py b/src/control/controllers/controllers/node_vel_to_ackermann.py new file mode 100644 index 000000000..cdd4cfa61 --- /dev/null +++ b/src/control/controllers/controllers/node_vel_to_ackermann.py @@ -0,0 +1,53 @@ +from math import pi, atan + +import rclpy +from rclpy.node import Node + +from ackermann_msgs.msg import AckermannDriveStamped +from geometry_msgs.msg import Twist + + +def convert_trans_rot_vel_to_steering_angle(vel, omega, wheelbase): + if omega == 0 or vel == 0: + return 0.0 + + radius = vel / omega + return atan(wheelbase / radius) * (180 / pi) * 5 + + +class Vel2Ackermann(Node): + wheelbase = 1.5 # taken from sim config - measured on car + + def __init__(self): + super().__init__('nav2_commands') + + self.create_subscription(Twist, "/control/nav_cmd_vel", self.cmd_callback, 1) + + self.drive_pub = self.create_publisher(AckermannDriveStamped, "/control/driving_command", 1) + + self.get_logger().info("---Nav2 control interpreter initalised---") + + def cmd_callback(self, twist_msg: Twist): + vel = twist_msg.linear.x + steering = convert_trans_rot_vel_to_steering_angle( + vel, + twist_msg.angular.z, + self.wheelbase, + ) + + msg = AckermannDriveStamped() + # make time for msg id + # msg.header.stamp = + msg.header.frame_id = "base_footprint" + msg.drive.steering_angle = steering + msg.drive.speed = vel + + self.drive_pub.publish(msg) + +def main(args=None): + # begin ros node + rclpy.init(args=args) + node = Vel2Ackermann() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() diff --git a/src/control/controllers/setup.py b/src/control/controllers/setup.py index fda62ba7d..fef733d71 100644 --- a/src/control/controllers/setup.py +++ b/src/control/controllers/setup.py @@ -25,6 +25,7 @@ "point_fitting = controllers.node_point_fitting:main", "vector_spline = controllers.node_vector_spline:main", "point_fitting2 = controllers.node_point_fitting2:main", + "vel_to_ackermann = controllers.node_vel_to_ackermann:main", ], }, ) diff --git a/src/navigation/nav_bringup/config/nav2_params.yaml b/src/navigation/nav_bringup/config/nav2_params.yaml index 504d5b1f9..826f7f660 100644 --- a/src/navigation/nav_bringup/config/nav2_params.yaml +++ b/src/navigation/nav_bringup/config/nav2_params.yaml @@ -1,6 +1,6 @@ /**: # stack-wide parameters ros__parameters: - use_sim_time: True + use_sim_time: False global_frame: track local_frame: odom robot_base_frame: base_footprint @@ -179,7 +179,7 @@ controller_server: min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 - failure_tolerance: 0.3 + failure_tolerance: 0.5 progress_checker_plugin: "progress_checker" goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" controller_plugins: ["RegulatedPurePursuit"] @@ -198,8 +198,8 @@ controller_server: # Controller parameters RegulatedPurePursuit: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 8.0 # The desired maximum linear velocity to use. - lookahead_dist: 2.5 # The lookahead distance to use to find the lookahead point. + desired_linear_vel: 5.0 # The desired maximum linear velocity to use. + lookahead_dist: 4.0 # The lookahead distance to use to find the lookahead point. # min_lookahead_dist: 3.0 # The minimum lookahead distance threshold when using velocity scaled lookahead distances. # max_lookahead_dist: 5.0 # The maximum lookahead distance threshold when using velocity scaled lookahead distances. lookahead_time: 1.5 # The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. diff --git a/src/navigation/nav_bringup/launch/nav2_bringup.launch.py b/src/navigation/nav_bringup/launch/nav2_bringup.launch.py index 3fec3c800..5520718ad 100644 --- a/src/navigation/nav_bringup/launch/nav2_bringup.launch.py +++ b/src/navigation/nav_bringup/launch/nav2_bringup.launch.py @@ -45,24 +45,24 @@ def generate_launch_description(): ("/tf", "tf"), ("/tf_static", "tf_static"), ("cmd_vel", "control/nav_cmd_vel"), - ("plan", "/planning/global_path"), + ("plan", "planning/midline_path"), + ('map', 'planning/boundary_grid'), ] - # ('map', 'planning/boundary_grid')] # behaviour tree xml file location # uncomment the XML you want to test to_pose_bt_xml = os.path.join( - get_package_share_directory("qutms_nav2"), + get_package_share_directory("nav_bringup"), "behaviour_trees", # 'plan_to_pose.xml') # 'replan_to_pose.xml') - "plan_to_pose_and_follow.xml", + # "plan_to_pose_and_follow.xml", + #'replan_to_pose_and_follow.xml' + 'follow_path.xml' ) - # 'replan_to_pose_and_follow.xml') - # 'follow_path.xml') through_poses_bt_xml = os.path.join( - get_package_share_directory("qutms_nav2"), "behaviour_trees", "plan_through_poses_and_follow.xml" + get_package_share_directory("nav_bringup"), "behaviour_trees", "plan_through_poses_and_follow.xml" ) # Create our own temporary YAML files that include substitutions diff --git a/src/operations/mission_controller/launch/trackdrive.launch.py b/src/operations/mission_controller/launch/trackdrive.launch.py index 8023f81c0..2d8edc795 100644 --- a/src/operations/mission_controller/launch/trackdrive.launch.py +++ b/src/operations/mission_controller/launch/trackdrive.launch.py @@ -28,17 +28,22 @@ def generate_launch_description(): get_package_share_path("planners") / "config" / "ft_planner.yaml", ], ), - Node( - package="pure_pursuit_cpp", - executable="pure_pursuit_node", - parameters=[ - get_package_share_path("pure_pursuit_cpp") / "config" / "pure_pursuit_cpp.yaml", - ], - ), + # Node( + # package="pure_pursuit_cpp", + # executable="pure_pursuit_node", + # parameters=[ + # get_package_share_path("pure_pursuit_cpp") / "config" / "pure_pursuit_cpp.yaml", + # ], + # ), IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(get_package_share_path("nav_bringup"), "launch", "slam_toolbox.launch.py") ) ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(get_package_share_path("nav_bringup"), "launch", "nav2_bringup2.launch.py") + ) + ), ] ) diff --git a/src/operations/mission_controller/mission_controller/node_mission_launcher.py b/src/operations/mission_controller/mission_controller/node_mission_launcher.py index 622b743bb..351930e75 100644 --- a/src/operations/mission_controller/mission_controller/node_mission_launcher.py +++ b/src/operations/mission_controller/mission_controller/node_mission_launcher.py @@ -25,18 +25,20 @@ def callback(self, status: State): target_mission = INT_MISSION_TYPE[status.mission].value if target_mission == "ebs_test": command = ["stdbuf", "-o", "L", "ros2", "launch", "mission_controller", "ebs_test.launch.py"] - self.get_logger().info(f"Command: {' '.join(command)}") - self.process = Popen(command) - self.get_logger().info("Mission started: " + target_mission) - self.mission_launched = True + + # specific for trackdrive running new planner + elif target_mission == "trackdrive": + node = target_mission + "_handler_plan_node" + command = ["stdbuf", "-o", "L", "ros2", "run", "mission_controller", node] else: node = target_mission + "_handler_node" command = ["stdbuf", "-o", "L", "ros2", "run", "mission_controller", node] - self.get_logger().info(f"Command: {' '.join(command)}") - self.process = Popen(command) - self.get_logger().info("Mission started: " + target_mission) - self.mission_launched = True + + self.get_logger().info(f"Command: {' '.join(command)}") + self.process = Popen(command) + self.get_logger().info("Mission started: " + target_mission) + self.mission_launched = True elif (status.mission == State.MISSION_NONE or status.state == State.EMERGENCY) and self.mission_launched: self.get_logger().warn("Closing mission") diff --git a/src/operations/mission_controller/mission_controller/node_trackdrive_handler_plan.py b/src/operations/mission_controller/mission_controller/node_trackdrive_handler_plan.py new file mode 100644 index 000000000..e3310e72e --- /dev/null +++ b/src/operations/mission_controller/mission_controller/node_trackdrive_handler_plan.py @@ -0,0 +1,190 @@ +import time +import numpy as np +from subprocess import Popen + +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener + +import rclpy +from rclpy.action import ActionClient + +from driverless_msgs.msg import Shutdown, State +from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped +from std_msgs.msg import UInt8, Bool +from nav_msgs.msg import Path, Odometry + +from nav2_msgs.action import FollowPath + +from std_srvs.srv import Trigger + +from driverless_common.shutdown_node import ShutdownNode + +class TrackdriveHandler(ShutdownNode): + mission_started = False + odom_received = False + crossed_start = False + sent_init = False + laps = 0 + last_lap_time = 0.0 + last_x = 0.0 + goal_offet = 0.1 + path = None + goal_handle = None + + def __init__(self): + super().__init__("trackdrive_logic_node") + + self.declare_parameter("start_following", False) + self.declare_parameter("sim", False) + + self.create_subscription(State, "system/as_status", self.state_callback, 1) + self.create_subscription(Path, "planning/midline_path", self.path_callback, 1) + self.create_subscription(Odometry, "imu/odometry", self.odom_callback, 1) + + if not self.get_parameter("sim").value: + # reset odom and pose from camera + self.reset_odom_client = self.create_client(Trigger, "zed2i/zed_node/reset_odometry") + self.reset_pose_client = self.create_client(Trigger, "zed2i/zed_node/reset_pos_tracking") + + while not self.reset_odom_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info("reset_odom_client service not available, waiting again...") + while not self.reset_pose_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info("reset_pose_client service not available, waiting again...") + + self.create_timer((1 / 20), self.timer_callback) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + # publishers + self.shutdown_pub = self.create_publisher(Shutdown, "system/shutdown", 1) + self.lap_trig_pub = self.create_publisher(UInt8, "system/laps_completed", 1) + self.init_pose_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 1) + + # actions + self.nav_through_poses_client = ActionClient(self, + FollowPath, + 'follow_path') + + if self.get_parameter("start_following").value: + # start at lap 1 + self.get_logger().warn("---DEBUG MODE ENABLED---") + self.crossed_start = True + + self.get_logger().info("---Trackdrive handler node initialised---") + + def state_callback(self, msg: State): + super().state_callback(msg) + if ( + msg.state == State.DRIVING + and msg.mission == State.TRACKDRIVE + and msg.navigation_ready + and not self.mission_started + and self.odom_received + ): + self.mission_started = True + self.last_lap_time = time.time() + self.lap_trig_pub.publish(UInt8(data=0)) + + if not self.get_parameter("sim").value: + # reset odom and pose from camera + self.reset_odom_client.call_async(Trigger.Request()) + self.reset_pose_client.call_async(Trigger.Request()) + + command = ["stdbuf", "-o", "L", "ros2", "launch", "mission_controller", "trackdrive.launch.py"] + self.get_logger().info(f"Command: {' '.join(command)}") + self.process = Popen(command) + self.set_process(self.process) + self.get_logger().info("Trackdrive mission started") + + def path_callback(self, msg: Path): + # receive path and convert to numpy array + # if self.path is not None: + # return + self.get_logger().info(f"Spline Path Recieved - length: {len(msg.poses)}", once=True) + self.send_path(msg) + + def odom_callback(self, msg: Odometry): + """Ensure the SBG EKF has settled and we get odom msgs before starting the mission""" + + if not self.odom_received: + self.odom_received = True + self.get_logger().info("Odometry received") + + def timer_callback(self): + # check if car has crossed the finish line (0,0) + # get distance from 0,0 and increment laps when within a certain threshold + # and distance is increasing away from 0,0 + try: + track_to_base = self.tf_buffer.lookup_transform( + "track", "base_footprint", rclpy.time.Time(seconds=0) + ) + except TransformException as e: + self.get_logger().debug("Transform exception: " + str(e)) + return + + if not self.mission_started: + self.last_x = track_to_base.transform.translation.x + return + + if not abs(track_to_base.transform.translation.y) < 2: + self.last_x = track_to_base.transform.translation.x + return + + if (self.last_x <= self.goal_offet and track_to_base.transform.translation.x >= self.goal_offet): + if not self.crossed_start: + self.crossed_start = True + self.last_lap_time = time.time() + self.get_logger().info("Crossed start line") + # will need to go around again to reset last x + self.last_x = track_to_base.transform.translation.x + return + + if not (time.time() - self.last_lap_time > 3): # seconds at least between laps + # will need to go around again to reset last x + self.last_x = track_to_base.transform.translation.x + return + + self.laps += 1 + self.lap_trig_pub.publish(UInt8(data=self.laps)) + self.get_logger().info(f"Lap {self.laps} completed") + + # publish initial pose on first lap + if self.laps == 1 and not self.sent_init: + init_pose_msg = PoseWithCovarianceStamped() + init_pose_msg.header.stamp = track_to_base.header.stamp + init_pose_msg.header.frame_id = "track" + # convert translation to pose + init_pose_msg.pose.pose.position.x = track_to_base.transform.translation.x + init_pose_msg.pose.pose.position.y = track_to_base.transform.translation.y + init_pose_msg.pose.pose.orientation = track_to_base.transform.rotation + # cov diag to square + self.init_pose_pub.publish(init_pose_msg) + self.sent_init = True + + self.last_x = track_to_base.transform.translation.x + self.last_lap_time = time.time() + + if self.laps == 10: + self.get_logger().info("Trackdrive mission complete") + # currently only works when vehicle supervisor node is running on-car + # TODO: sort out vehicle states for eventual environment agnostic operation + shutdown_msg = Shutdown(finished_engage_ebs=True) + self.shutdown_pub.publish(shutdown_msg) + + def send_path(self, path: Path): + # Sends a `NavThroughPoses` action request + while not self.nav_through_poses_client.wait_for_server(timeout_sec=1.0): + self.get_logger().info("'NavigateThroughPoses' action server not available, waiting...") + + goal_msg = FollowPath.Goal() + goal_msg.path = path + + send_goal_future = self.nav_through_poses_client.send_goal_async(goal_msg) + +def main(args=None): + rclpy.init(args=args) + node = TrackdriveHandler() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() \ No newline at end of file diff --git a/src/operations/mission_controller/setup.py b/src/operations/mission_controller/setup.py index 025ffb0ae..aa24bca90 100644 --- a/src/operations/mission_controller/setup.py +++ b/src/operations/mission_controller/setup.py @@ -27,6 +27,7 @@ "inspection_handler_node = mission_controller.node_inspection_handler:main", "trackdrive_handler_node = mission_controller.node_trackdrive_handler:main", "trackdrive_handler_lifecycle = mission_controller.lifecycle_trackdrive_handler:main", + "trackdrive_handler_plan_node = mission_controller.node_trackdrive_handler_plan:main", ], }, ) From b5ad0ab0e4cc29b3a0de216fb90cdb4390555830 Mon Sep 17 00:00:00 2001 From: Alastair Date: Sun, 5 May 2024 15:29:27 +1000 Subject: [PATCH 03/11] running vel controller on boot --- src/control/controllers/setup.py | 2 +- src/navigation/nav_bringup/launch/nav2_bringup.launch.py | 4 ++-- .../mission_controller/launch/trackdrive.launch.py | 6 +++++- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/control/controllers/setup.py b/src/control/controllers/setup.py index fef733d71..5ded9aa57 100644 --- a/src/control/controllers/setup.py +++ b/src/control/controllers/setup.py @@ -25,7 +25,7 @@ "point_fitting = controllers.node_point_fitting:main", "vector_spline = controllers.node_vector_spline:main", "point_fitting2 = controllers.node_point_fitting2:main", - "vel_to_ackermann = controllers.node_vel_to_ackermann:main", + "vel_to_ackermann_node = controllers.node_vel_to_ackermann:main", ], }, ) diff --git a/src/navigation/nav_bringup/launch/nav2_bringup.launch.py b/src/navigation/nav_bringup/launch/nav2_bringup.launch.py index 5520718ad..d5064ae7c 100644 --- a/src/navigation/nav_bringup/launch/nav2_bringup.launch.py +++ b/src/navigation/nav_bringup/launch/nav2_bringup.launch.py @@ -46,7 +46,7 @@ def generate_launch_description(): ("/tf_static", "tf_static"), ("cmd_vel", "control/nav_cmd_vel"), ("plan", "planning/midline_path"), - ('map', 'planning/boundary_grid'), + ("map", "planning/boundary_grid"), ] # behaviour tree xml file location @@ -58,7 +58,7 @@ def generate_launch_description(): # 'replan_to_pose.xml') # "plan_to_pose_and_follow.xml", #'replan_to_pose_and_follow.xml' - 'follow_path.xml' + "follow_path.xml", ) through_poses_bt_xml = os.path.join( diff --git a/src/operations/mission_controller/launch/trackdrive.launch.py b/src/operations/mission_controller/launch/trackdrive.launch.py index 2d8edc795..7a671d453 100644 --- a/src/operations/mission_controller/launch/trackdrive.launch.py +++ b/src/operations/mission_controller/launch/trackdrive.launch.py @@ -35,6 +35,10 @@ def generate_launch_description(): # get_package_share_path("pure_pursuit_cpp") / "config" / "pure_pursuit_cpp.yaml", # ], # ), + Node( + package="controllers", + executable="vel_to_ackermann_node", + ), IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(get_package_share_path("nav_bringup"), "launch", "slam_toolbox.launch.py") @@ -42,7 +46,7 @@ def generate_launch_description(): ), IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(get_package_share_path("nav_bringup"), "launch", "nav2_bringup2.launch.py") + os.path.join(get_package_share_path("nav_bringup"), "launch", "nav2_bringup.launch.py") ) ), ] From a59ad36d6fe4b9b425a954c5aa04c82cb6cdb97b Mon Sep 17 00:00:00 2001 From: Alastair Date: Mon, 6 May 2024 11:30:01 +1000 Subject: [PATCH 04/11] proper process interrupting --- .../driverless_common/shutdown_node.py | 13 ++++--- .../controllers/node_vel_to_ackermann.py | 19 +++++----- .../roscube_machine/launch/machine.launch.py | 8 +++-- .../node_mission_launcher.py | 3 +- .../node_trackdrive_handler.py | 1 - .../node_trackdrive_handler_plan.py | 35 +++++++++---------- 6 files changed, 42 insertions(+), 37 deletions(-) diff --git a/src/common/driverless_common/driverless_common/shutdown_node.py b/src/common/driverless_common/driverless_common/shutdown_node.py index 1b33e4a71..358a0ce36 100644 --- a/src/common/driverless_common/driverless_common/shutdown_node.py +++ b/src/common/driverless_common/driverless_common/shutdown_node.py @@ -1,3 +1,6 @@ +import signal +from subprocess import Popen + from rclpy.node import Node from driverless_msgs.msg import State @@ -11,15 +14,15 @@ class ShutdownNode(Node): def __init__(self, node_name: str, **kwargs) -> None: super().__init__(node_name, **kwargs) self.reset_sub = self.create_subscription(State, "/system/as_status", self.state_callback, QOS_LATEST) - - def set_process(self, process): - self.process = process + self.process: Popen = None def state_callback(self, msg: State): if msg.state in [State.START, State.SELECT_MISSION, State.ACTIVATE_EBS, State.FINISHED, State.EMERGENCY]: if self.process is not None: - self.process.terminate() - self.get_logger().error("Terminated process") + self.process.send_signal(signal.SIGINT) + self.get_logger().error("Interrupted process") + self.process = None self.get_logger().error("Exit Node - Shutdown Triggered") self.destroy_node() + exit(1) diff --git a/src/control/controllers/controllers/node_vel_to_ackermann.py b/src/control/controllers/controllers/node_vel_to_ackermann.py index cdd4cfa61..2285cda10 100644 --- a/src/control/controllers/controllers/node_vel_to_ackermann.py +++ b/src/control/controllers/controllers/node_vel_to_ackermann.py @@ -1,4 +1,4 @@ -from math import pi, atan +from math import atan, pi import rclpy from rclpy.node import Node @@ -19,31 +19,32 @@ class Vel2Ackermann(Node): wheelbase = 1.5 # taken from sim config - measured on car def __init__(self): - super().__init__('nav2_commands') - + super().__init__("nav_cmd_translator") + self.create_subscription(Twist, "/control/nav_cmd_vel", self.cmd_callback, 1) self.drive_pub = self.create_publisher(AckermannDriveStamped, "/control/driving_command", 1) self.get_logger().info("---Nav2 control interpreter initalised---") - def cmd_callback(self, twist_msg: Twist): + def cmd_callback(self, twist_msg: Twist): vel = twist_msg.linear.x steering = convert_trans_rot_vel_to_steering_angle( - vel, - twist_msg.angular.z, + vel, + twist_msg.angular.z, self.wheelbase, ) - + msg = AckermannDriveStamped() # make time for msg id - # msg.header.stamp = + # msg.header.stamp = msg.header.frame_id = "base_footprint" msg.drive.steering_angle = steering msg.drive.speed = vel - + self.drive_pub.publish(msg) + def main(args=None): # begin ros node rclpy.init(args=args) diff --git a/src/machines/roscube_machine/launch/machine.launch.py b/src/machines/roscube_machine/launch/machine.launch.py index 3f2047c5e..74fcd190e 100644 --- a/src/machines/roscube_machine/launch/machine.launch.py +++ b/src/machines/roscube_machine/launch/machine.launch.py @@ -16,8 +16,8 @@ def generate_launch_description(): composable_node_descriptions=[ ComposableNode( package="vehicle_supervisor", - plugin="vehicle_supervisor::ASSupervisorLaunch", - name="vehicle_supervisor_launch_node", + plugin="vehicle_supervisor::ASSupervisor", + name="vehicle_supervisor_node", parameters=[ {"manual_override": False}, ], @@ -69,6 +69,10 @@ def generate_launch_description(): package="lidar_pipeline", executable="lidar_detector_node", ), + Node( + package="mission_controller", + executable="mission_control_node", + ), IncludeLaunchDescription( launch_description_source=PythonLaunchDescriptionSource( launch_file_path=str(get_package_share_path("sensors") / "launch" / "vlp32.launch.py") diff --git a/src/operations/mission_controller/mission_controller/node_mission_launcher.py b/src/operations/mission_controller/mission_controller/node_mission_launcher.py index 351930e75..7fd4f8eca 100644 --- a/src/operations/mission_controller/mission_controller/node_mission_launcher.py +++ b/src/operations/mission_controller/mission_controller/node_mission_launcher.py @@ -1,4 +1,5 @@ from subprocess import Popen +import signal import rclpy from rclpy.node import Node @@ -42,7 +43,7 @@ def callback(self, status: State): elif (status.mission == State.MISSION_NONE or status.state == State.EMERGENCY) and self.mission_launched: self.get_logger().warn("Closing mission") - self.process.terminate() + self.process.send_signal(signal.SIGINT) self.mission_launched = False diff --git a/src/operations/mission_controller/mission_controller/node_trackdrive_handler.py b/src/operations/mission_controller/mission_controller/node_trackdrive_handler.py index 443c65247..42ddeb4f1 100644 --- a/src/operations/mission_controller/mission_controller/node_trackdrive_handler.py +++ b/src/operations/mission_controller/mission_controller/node_trackdrive_handler.py @@ -82,7 +82,6 @@ def state_callback(self, msg: State): command = ["stdbuf", "-o", "L", "ros2", "launch", "mission_controller", "trackdrive.launch.py"] self.get_logger().info(f"Command: {' '.join(command)}") self.process = Popen(command) - self.set_process(self.process) self.get_logger().info("Trackdrive mission started") def odom_callback(self, msg: Odometry): diff --git a/src/operations/mission_controller/mission_controller/node_trackdrive_handler_plan.py b/src/operations/mission_controller/mission_controller/node_trackdrive_handler_plan.py index e3310e72e..350b4c647 100644 --- a/src/operations/mission_controller/mission_controller/node_trackdrive_handler_plan.py +++ b/src/operations/mission_controller/mission_controller/node_trackdrive_handler_plan.py @@ -1,7 +1,9 @@ +import os +from subprocess import PIPE, Popen import time -import numpy as np -from subprocess import Popen +from nav2_msgs.action import FollowPath +import numpy as np from tf2_ros import TransformException from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener @@ -10,16 +12,15 @@ from rclpy.action import ActionClient from driverless_msgs.msg import Shutdown, State -from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped -from std_msgs.msg import UInt8, Bool -from nav_msgs.msg import Path, Odometry - -from nav2_msgs.action import FollowPath +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped +from nav_msgs.msg import Odometry, Path +from std_msgs.msg import Bool, UInt8 from std_srvs.srv import Trigger from driverless_common.shutdown_node import ShutdownNode + class TrackdriveHandler(ShutdownNode): mission_started = False odom_received = False @@ -62,9 +63,7 @@ def __init__(self): self.init_pose_pub = self.create_publisher(PoseWithCovarianceStamped, "/initialpose", 1) # actions - self.nav_through_poses_client = ActionClient(self, - FollowPath, - 'follow_path') + self.nav_through_poses_client = ActionClient(self, FollowPath, "follow_path") if self.get_parameter("start_following").value: # start at lap 1 @@ -94,7 +93,6 @@ def state_callback(self, msg: State): command = ["stdbuf", "-o", "L", "ros2", "launch", "mission_controller", "trackdrive.launch.py"] self.get_logger().info(f"Command: {' '.join(command)}") self.process = Popen(command) - self.set_process(self.process) self.get_logger().info("Trackdrive mission started") def path_callback(self, msg: Path): @@ -116,22 +114,20 @@ def timer_callback(self): # get distance from 0,0 and increment laps when within a certain threshold # and distance is increasing away from 0,0 try: - track_to_base = self.tf_buffer.lookup_transform( - "track", "base_footprint", rclpy.time.Time(seconds=0) - ) + track_to_base = self.tf_buffer.lookup_transform("track", "base_footprint", rclpy.time.Time(seconds=0)) except TransformException as e: self.get_logger().debug("Transform exception: " + str(e)) return - + if not self.mission_started: self.last_x = track_to_base.transform.translation.x return - + if not abs(track_to_base.transform.translation.y) < 2: self.last_x = track_to_base.transform.translation.x return - if (self.last_x <= self.goal_offet and track_to_base.transform.translation.x >= self.goal_offet): + if self.last_x <= self.goal_offet and track_to_base.transform.translation.x >= self.goal_offet: if not self.crossed_start: self.crossed_start = True self.last_lap_time = time.time() @@ -182,9 +178,10 @@ def send_path(self, path: Path): send_goal_future = self.nav_through_poses_client.send_goal_async(goal_msg) + def main(args=None): rclpy.init(args=args) node = TrackdriveHandler() rclpy.spin(node) - node.destroy_node() - rclpy.shutdown() \ No newline at end of file + # node.destroy_node() + rclpy.shutdown() From d10e174d43bf5642876dc0a59ab0305440a7d561 Mon Sep 17 00:00:00 2001 From: Alastair Date: Mon, 6 May 2024 21:27:22 +1000 Subject: [PATCH 05/11] track width is less than 4 --- src/navigation/planners/planners/node_ft_planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/navigation/planners/planners/node_ft_planner.py b/src/navigation/planners/planners/node_ft_planner.py index c0d72edd1..4c4194df9 100644 --- a/src/navigation/planners/planners/node_ft_planner.py +++ b/src/navigation/planners/planners/node_ft_planner.py @@ -253,7 +253,7 @@ def get_planner_cfg(self): } # cone matching - self.declare_parameter("min_track_width", 4.5) + self.declare_parameter("min_track_width", 3.5) self.declare_parameter("max_search_range", 5) self.declare_parameter("max_search_angle", 50) self.declare_parameter("matches_should_be_monotonic", True) From ff671f1bafa99c4c16914a4745057059cdccb48b Mon Sep 17 00:00:00 2001 From: Alastair Date: Mon, 6 May 2024 21:41:59 +1000 Subject: [PATCH 06/11] markers back for foxglove and pre-commit --- .../mission_controller/node_mission_launcher.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/operations/mission_controller/mission_controller/node_mission_launcher.py b/src/operations/mission_controller/mission_controller/node_mission_launcher.py index 7fd4f8eca..2c6ea7995 100644 --- a/src/operations/mission_controller/mission_controller/node_mission_launcher.py +++ b/src/operations/mission_controller/mission_controller/node_mission_launcher.py @@ -1,5 +1,5 @@ -from subprocess import Popen import signal +from subprocess import Popen import rclpy from rclpy.node import Node From 64f3ab39e18537e624fbd814c0c9063556fe6232 Mon Sep 17 00:00:00 2001 From: qev3-d Date: Thu, 9 May 2024 17:59:10 +1000 Subject: [PATCH 07/11] few tweaks to planner and controller --- .../roscube_machine/launch/machine.launch.py | 16 +++--- .../nav_bringup/config/nav2_params.yaml | 2 +- .../planners/planners/node_ft_planner.py | 53 +++++++------------ 3 files changed, 27 insertions(+), 44 deletions(-) diff --git a/src/machines/roscube_machine/launch/machine.launch.py b/src/machines/roscube_machine/launch/machine.launch.py index 74fcd190e..e847005cb 100644 --- a/src/machines/roscube_machine/launch/machine.launch.py +++ b/src/machines/roscube_machine/launch/machine.launch.py @@ -57,10 +57,10 @@ def generate_launch_description(): return LaunchDescription( [ scs_container, - Node( - package="rosboard", - executable="rosboard_node", - ), + # Node( + # package="rosboard", + # executable="rosboard_node", + # ), Node( package="driverless_common", executable="display", @@ -69,10 +69,10 @@ def generate_launch_description(): package="lidar_pipeline", executable="lidar_detector_node", ), - Node( - package="mission_controller", - executable="mission_control_node", - ), + # Node( + # package="mission_controller", + # executable="mission_launcher_node", + # ), IncludeLaunchDescription( launch_description_source=PythonLaunchDescriptionSource( launch_file_path=str(get_package_share_path("sensors") / "launch" / "vlp32.launch.py") diff --git a/src/navigation/nav_bringup/config/nav2_params.yaml b/src/navigation/nav_bringup/config/nav2_params.yaml index 826f7f660..60e4e6693 100644 --- a/src/navigation/nav_bringup/config/nav2_params.yaml +++ b/src/navigation/nav_bringup/config/nav2_params.yaml @@ -198,7 +198,7 @@ controller_server: # Controller parameters RegulatedPurePursuit: plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" - desired_linear_vel: 5.0 # The desired maximum linear velocity to use. + desired_linear_vel: 4.0 # The desired maximum linear velocity to use. lookahead_dist: 4.0 # The lookahead distance to use to find the lookahead point. # min_lookahead_dist: 3.0 # The minimum lookahead distance threshold when using velocity scaled lookahead distances. # max_lookahead_dist: 5.0 # The maximum lookahead distance threshold when using velocity scaled lookahead distances. diff --git a/src/navigation/planners/planners/node_ft_planner.py b/src/navigation/planners/planners/node_ft_planner.py index 4c4194df9..845cb33ac 100644 --- a/src/navigation/planners/planners/node_ft_planner.py +++ b/src/navigation/planners/planners/node_ft_planner.py @@ -180,7 +180,7 @@ def __init__(self): # sub to track for all cone locations relative to car start point self.create_subscription(ConeDetectionStamped, "/slam/global_map", self.map_callback, QOS_LATEST) - self.create_timer(1 / 20, self.planning_callback) + self.create_timer(1 / 10, self.planning_callback) self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) @@ -206,6 +206,22 @@ def __init__(self): self.path_planner = PathPlanner(**self.get_planner_cfg()) + + # skip if we haven't completed a lap yet + ( + path, + ordered_blues, + ordered_yellows, + virt_blues, + virt_yellows, + _, + _, + ) = self.path_planner.calculate_path_in_global_frame( + pre_track, np.array([0.0,0.0]), 0.0, return_intermediate_results=True + ) + self.initialised = True + self.get_logger().info("Initialised planner calcs") + self.get_logger().info("---Ordered path planner node initalised---") def get_planner_cfg(self): @@ -253,7 +269,7 @@ def get_planner_cfg(self): } # cone matching - self.declare_parameter("min_track_width", 3.5) + self.declare_parameter("min_track_width", 4.0) self.declare_parameter("max_search_range", 5) self.declare_parameter("max_search_angle", 50) self.declare_parameter("matches_should_be_monotonic", True) @@ -278,39 +294,6 @@ def map_callback(self, track_msg: ConeDetectionStamped): self.current_track = track_msg def planning_callback(self): - if not self.initialised: - ( - path, - ordered_blues, - ordered_yellows, - virt_blues, - virt_yellows, - _, - _, - ) = self.path_planner.calculate_path_in_global_frame( - pre_track, np.array([0.0, 0.0]), 0.0, return_intermediate_results=True - ) - self.initialised = True - self.get_logger().info("Initialised planner calcs") - return - - # skip if we haven't completed a lap yet - if not self.initialised: - ( - path, - ordered_blues, - ordered_yellows, - virt_blues, - virt_yellows, - _, - _, - ) = self.path_planner.calculate_path_in_global_frame( - pre_track, np.array([0.0,0.0]), 0.0, return_intermediate_results=True - ) - self.initialised = True - self.get_logger().info("Initialised planner calcs") - return - if self.current_track is None or len(self.current_track.cones) == 0: self.get_logger().warn("No track data received", throttle_duration_sec=1) return From 4f2b7ae9a2962b0da725cfb6d56b32b21153bb43 Mon Sep 17 00:00:00 2001 From: Alastair Date: Sat, 11 May 2024 20:54:16 +1000 Subject: [PATCH 08/11] pre-commit fix --- src/navigation/planners/planners/node_ft_planner.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/navigation/planners/planners/node_ft_planner.py b/src/navigation/planners/planners/node_ft_planner.py index 845cb33ac..486bb4dad 100644 --- a/src/navigation/planners/planners/node_ft_planner.py +++ b/src/navigation/planners/planners/node_ft_planner.py @@ -206,7 +206,6 @@ def __init__(self): self.path_planner = PathPlanner(**self.get_planner_cfg()) - # skip if we haven't completed a lap yet ( path, @@ -217,7 +216,7 @@ def __init__(self): _, _, ) = self.path_planner.calculate_path_in_global_frame( - pre_track, np.array([0.0,0.0]), 0.0, return_intermediate_results=True + pre_track, np.array([0.0, 0.0]), 0.0, return_intermediate_results=True ) self.initialised = True self.get_logger().info("Initialised planner calcs") From 4b4dbd8c195e9df85fc0bd0f8ade46e7b97b43d6 Mon Sep 17 00:00:00 2001 From: Alastair Date: Sat, 11 May 2024 21:06:29 +1000 Subject: [PATCH 09/11] align topic name to nav2 --- src/navigation/planners/planners/node_ft_planner.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/navigation/planners/planners/node_ft_planner.py b/src/navigation/planners/planners/node_ft_planner.py index 486bb4dad..b350cc15a 100644 --- a/src/navigation/planners/planners/node_ft_planner.py +++ b/src/navigation/planners/planners/node_ft_planner.py @@ -196,8 +196,8 @@ def __init__(self): depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, ) - self.map_pub = self.create_publisher(OccupancyGrid, "/map", map_qos) - self.map_meta_pub = self.create_publisher(MapMetaData, "/map_metadata", map_qos) + self.map_pub = self.create_publisher(OccupancyGrid, "/planning/boundary_grid", map_qos) + self.map_meta_pub = self.create_publisher(MapMetaData, "/planning/boundary_grid_metadata", map_qos) self.declare_parameter("start_following", False) if self.get_parameter("start_following").value: From a1c80dab6434a61c67ba9eb71a8eb37ff985ebb1 Mon Sep 17 00:00:00 2001 From: Alastair Date: Sat, 11 May 2024 21:16:24 +1000 Subject: [PATCH 10/11] rename to nav handler --- .../mission_controller/node_mission_launcher.py | 2 +- ...rackdrive_handler_plan.py => node_trackdrive_handler_nav.py} | 0 src/operations/mission_controller/setup.py | 2 +- 3 files changed, 2 insertions(+), 2 deletions(-) rename src/operations/mission_controller/mission_controller/{node_trackdrive_handler_plan.py => node_trackdrive_handler_nav.py} (100%) diff --git a/src/operations/mission_controller/mission_controller/node_mission_launcher.py b/src/operations/mission_controller/mission_controller/node_mission_launcher.py index 2c6ea7995..af9995956 100644 --- a/src/operations/mission_controller/mission_controller/node_mission_launcher.py +++ b/src/operations/mission_controller/mission_controller/node_mission_launcher.py @@ -29,7 +29,7 @@ def callback(self, status: State): # specific for trackdrive running new planner elif target_mission == "trackdrive": - node = target_mission + "_handler_plan_node" + node = target_mission + "_handler_nav_node" command = ["stdbuf", "-o", "L", "ros2", "run", "mission_controller", node] else: diff --git a/src/operations/mission_controller/mission_controller/node_trackdrive_handler_plan.py b/src/operations/mission_controller/mission_controller/node_trackdrive_handler_nav.py similarity index 100% rename from src/operations/mission_controller/mission_controller/node_trackdrive_handler_plan.py rename to src/operations/mission_controller/mission_controller/node_trackdrive_handler_nav.py diff --git a/src/operations/mission_controller/setup.py b/src/operations/mission_controller/setup.py index aa24bca90..ccc99336e 100644 --- a/src/operations/mission_controller/setup.py +++ b/src/operations/mission_controller/setup.py @@ -27,7 +27,7 @@ "inspection_handler_node = mission_controller.node_inspection_handler:main", "trackdrive_handler_node = mission_controller.node_trackdrive_handler:main", "trackdrive_handler_lifecycle = mission_controller.lifecycle_trackdrive_handler:main", - "trackdrive_handler_plan_node = mission_controller.node_trackdrive_handler_plan:main", + "trackdrive_handler_nav_node = mission_controller.node_trackdrive_handler_nav:main", ], }, ) From f2c657705bbf2093d225ec3b7cc5b43c5b646017 Mon Sep 17 00:00:00 2001 From: Alastair Date: Sat, 11 May 2024 21:30:58 +1000 Subject: [PATCH 11/11] tidy initialisation --- src/navigation/planners/planners/node_ft_planner.py | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/src/navigation/planners/planners/node_ft_planner.py b/src/navigation/planners/planners/node_ft_planner.py index b350cc15a..abd410402 100644 --- a/src/navigation/planners/planners/node_ft_planner.py +++ b/src/navigation/planners/planners/node_ft_planner.py @@ -173,7 +173,6 @@ class FaSTTUBeBoundaryExtractor(Node): following = False current_track = None spline_const = 10 # number of points per cone - initialised = False def __init__(self): super().__init__("ft_planner_node") @@ -206,19 +205,9 @@ def __init__(self): self.path_planner = PathPlanner(**self.get_planner_cfg()) - # skip if we haven't completed a lap yet - ( - path, - ordered_blues, - ordered_yellows, - virt_blues, - virt_yellows, - _, - _, - ) = self.path_planner.calculate_path_in_global_frame( + init_plan_calcs = self.path_planner.calculate_path_in_global_frame( pre_track, np.array([0.0, 0.0]), 0.0, return_intermediate_results=True ) - self.initialised = True self.get_logger().info("Initialised planner calcs") self.get_logger().info("---Ordered path planner node initalised---")