diff --git a/data/navigator_default.rviz b/data/navigator_default.rviz index 6f017558..9325104c 100644 --- a/data/navigator_default.rviz +++ b/data/navigator_default.rviz @@ -4,12 +4,15 @@ Panels: Name: Displays Property Tree Widget: Expanded: + - /Global Options1 - /TF1/Frames1 - /Routes Paths1 - - /Cameras1 + - /Routes Paths1/Path1/Topic1 - /Grids1 + - /Nav21/Map1 + - /Pure Pursuit Controller1 Splitter Ratio: 0.5 - Tree Height: 960 + Tree Height: 603 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -55,34 +58,20 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /robot_description - Enabled: true + Enabled: false Links: All Links Enabled: true Expand Joint Details: false Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - hero: - Alpha: 1 - Show Axes: false - Show Trail: false - model_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true Mass Properties: Inertia: false Mass: false Name: RobotModel TF Prefix: "" Update Interval: 0 - Value: true + Value: false Visual Enabled: true - Class: rviz_default_plugins/TF Enabled: false @@ -181,7 +170,7 @@ Visualization Manager: Durability Policy: Volatile Filter size: 10 History Policy: Keep Last - Reliability Policy: Reliable + Reliability Policy: Best Effort Value: /planning/path Value: true Enabled: true @@ -237,9 +226,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0.9825243353843689 + Max Intensity: 0.9850215315818787 Min Color: 0; 73; 112 - Min Intensity: 0.7125484347343445 + Min Intensity: 0.716873288154602 Name: Filtered LiDAR Position Transformer: XYZ Selectable: true @@ -336,7 +325,7 @@ Visualization Manager: Displays: - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map - Color Scheme: raw + Color Scheme: map Draw Behind: true Enabled: false Name: Drivable Area @@ -537,6 +526,143 @@ Visualization Manager: Reliability Policy: Reliable Value: /radar/radar_viz Value: false + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Planning Goal + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/goal_marker + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Planning Look Ahead + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/look_ahead_marker + Value: true + - Class: rviz_common/Group + Displays: + - Alpha: 0.699999988079071 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nav2/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nav2/map_updates + Use Timestamp: false + Value: true + - Alpha: 1 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: true + Name: Polygon + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nav2/global_costmap/published_footprint + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /nav2/plan + Value: true + Enabled: true + Name: Nav2 + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 0; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 1 + Name: Pure Pursuit Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ppc/path + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Pure Pursuit Lookahead + Namespaces: + lookahead: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/barrier_marker + Value: true + Enabled: true + Name: Pure Pursuit Controller Enabled: true Global Options: Background Color: 222; 222; 222 @@ -583,7 +709,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 40.92072296142578 + Distance: 20.80792236328125 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -598,10 +724,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.649796724319458 + Pitch: 0.9497964382171631 Target Frame: Value: Orbit (rviz_default_plugins) - Yaw: 2.9573850631713867 + Yaw: 2.19738507270813 Saved: ~ Window Geometry: Birds Eye: @@ -616,16 +742,16 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1373 + Height: 1016 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 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 + QMainWindow State: 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 Selection: collapsed: false Tool Properties: collapsed: false Views: collapsed: true - Width: 2176 - X: 74 + Width: 1310 + X: 610 Y: 27 diff --git a/launches/launch.vehicle.py b/launches/launch.vehicle.py index 27555848..2d03b610 100644 --- a/launches/launch.vehicle.py +++ b/launches/launch.vehicle.py @@ -29,44 +29,48 @@ def generate_launch_description(): perception_launch_entities = perception_launch_description.describe_sub_entities() - return LaunchDescription( - [ - map_manager_carla, - RegisterEventHandler( - OnProcessStart( - target_action=map_manager_carla, - on_start=[ - LogInfo( - msg="Map Manager Started... launching the rest of Navigator..." - ), - # CONTROL - # carla_controller, - # INTERFACE - # leaderboard_liaison, - # web_bridge, - ##joy_linux, - ##joy_translation, - # LOCALIZATION - # gnss_averager, - # mcl, - # MAPPING - # MISC - # recorder, - # rqt, - # camera_streamer, - # PERCEPTION - *perception_launch_entities, - # PLANNING - routing_monitor, - grid_summation, - junction_manager, - rtp, - # SAFETY - ##airbags, - ##guardian, - rviz, - ], - ) - ), - ] - ) + return LaunchDescription([ + map_manager_carla, + RegisterEventHandler( + OnProcessStart( + target_action=map_manager_carla, + on_start=[ + LogInfo(msg='Map Manager Started... launching the rest of Navigator...'), + # CONTROL + #carla_controller, + + # INTERFACE + # leaderboard_liaison, + # web_bridge, + ##joy_linux, + ##joy_translation, + + # LOCALIZATION + # gnss_averager, + # mcl, + + # MAPPING + # MISC + # recorder, + # rqt, + # camera_streamer, + + # PERCEPTION + *perception_launch_entities, + + # PLANNING + routing_monitor, + grid_summation, + junction_manager, + rtp, + pure_pursuit_controller, + + # SAFETY + ##airbags, + ##guardian, + rviz, + ] + ) + ) + ]) + diff --git a/launches/launch_node_definitions.py b/launches/launch_node_definitions.py index bb33ca26..28f946bf 100644 --- a/launches/launch_node_definitions.py +++ b/launches/launch_node_definitions.py @@ -4,6 +4,11 @@ NAVIGATOR_DIR = "/navigator/" +pure_pursuit_controller = Node( + package='pure_pursuit_controller', + executable='pure_pursuit_controller' +) + airbags = Node( package='airbags', executable='airbag_node' diff --git a/src/control/pure_pursuit_controller/package.xml b/src/control/pure_pursuit_controller/package.xml new file mode 100755 index 00000000..06fbf7d9 --- /dev/null +++ b/src/control/pure_pursuit_controller/package.xml @@ -0,0 +1,18 @@ + + + + pure_pursuit_controller + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/__init__.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py new file mode 100755 index 00000000..9c38e0b1 --- /dev/null +++ b/src/control/pure_pursuit_controller/pure_pursuit_controller/pure_pursuit_controller_node.py @@ -0,0 +1,360 @@ +import math + +import rclpy +import numpy as np + +from numpy import typing as npt + +from rclpy.node import Node + +from std_msgs.msg import ColorRGBA, Header +from rosgraph_msgs.msg import Clock +from nav_msgs.msg import Path, Odometry +from geometry_msgs.msg import Pose, PoseStamped, Point, Vector3 +from visualization_msgs.msg import Marker + +from navigator_msgs.msg import VehicleControl, VehicleSpeed + + +class Constants: + # Look ahead distance in meters + LD: float = 3.0 + # Look forward gain in meters (gain in look ahead distance per m/s of speed) + kf: float = 0.1 + # Wheel base (distance between front and rear wheels) in meter + WHEEL_BASE: float = 3.5 + # Max throttle and acceleration + MAX_THROTTLE = 2 + # Max speed in m/s + MAX_SPEED: float = 1.5 + # Stopping distance in meters + # This is the distance from the end of the path at which the vehicle will start to brake. + STOPPING_DIST: float = 10 + # Max break possible + MAX_BREAK: float = 1.0 + + +class VehicleState: + """Vehicle state class that holds the current pose and velocity of the vehicle.""" + + def __init__(self): + self.pose: Pose | None = None + self.velocity: float | None = None + + def loaded(self) -> bool: + """Check if the vehicle state is loaded with pose and velocity. + + Returns: + bool: True if pose and velocity are not None, False otherwise. + """ + return self.pose is not None and self.velocity is not None + + def calc_throttle_brake(self) -> tuple[float, float] | None: + """Calculate throttle and brake based on the current velocity. + + Returns: + tuple[float, float] | None: Throttle and brake values (None if vehicle state is not fulled loaded). + """ + if not self.loaded(): + return + + # Our target speed is the max speed + pid_error = Constants.MAX_SPEED - self.velocity + if pid_error > 0: + throttle = min(pid_error * 0.5, Constants.MAX_THROTTLE) + brake = 0.0 + elif pid_error <= 0: + brake = pid_error * Constants.MAX_THROTTLE * -1.0 + throttle = 0.0 + + return throttle, brake + + def calc_steer(self, target: tuple[float, float]) -> float | None: + """Calculate the steering angle based on the target point. + + Args: + target (tuple[float, float]): Target point (x, y) to steer towards. + + Returns: + float | None: Steering angle in the range [-1, 1] (None if vehicle state is not fulled loaded). + """ + if not self.loaded(): + return + + alpha = math.atan2(target[1], target[0]) + delta = math.atan2( + 2.0 * Constants.WHEEL_BASE * math.sin(alpha), self.lookahead_distance() + ) + + # Normalize the steering angle from [-pi, pi] to [-1, 1] + return -delta / (math.pi / 2) + + def lookahead_distance(self) -> float: + """Calculate the lookahead distance based on the current velocity. + + Returns: + float: Lookahead distance in meters. + """ + return Constants.kf * self.velocity + Constants.LD + + +class PursuitPath: + """Pursuit path class that holds the path and calculates the target point for the vehicle. + + Args: + path (list[tuple[float, float]], optional): List of points (x, y) that represent the path. Defaults to []. + """ + + def __init__(self, path: list[tuple[float, float]] = []): + self.path: npt.NDArray[np.float64] = np.asarray(path) + self.target_index: int | None = None + + def distance(self) -> float: + if len(self.path) == 0: + return 0.0 + + x, y = self.path[-1] + return math.hypot(x, y) + + def set_path(self, path: list[tuple[float, float]]) -> None: + """Set the path. + + Args: + path (list[tuple[float, float]]): List of points (x, y) that represent the path. + """ + self.path = np.asarray(path) + self.target_index = None + + def _calc_nearest_index(self) -> int: + """Calculate the nearest index of the path to the vehicle's current position. + + Returns: + int: Nearest index of the path to the vehicle's current position. + """ + min_dist = float("inf") + min_index = 0 + for i, (x, y) in enumerate(self.path): + dist = math.hypot(x, y) + if dist < min_dist: + min_dist = dist + min_index = i + + return min_index + + def get_current_path(self) -> npt.NDArray[np.float64]: + """Get the current path up to and including the target point.""" + target_index = self.target_index + 1 if self.target_index is not None else 0 + return np.copy(self.path[:target_index]) + + def calc_target_point(self, vs: VehicleState) -> tuple[float, float] | None: + """Calculate the target point based on the vehicle state. + + Args: + vs (VehicleState): Vehicle state object that holds the current pose and velocity of the vehicle. + + Returns: + tuple[float, float] | None: Target point (x, y) to steer towards (None if path is empty or vehicle state is not fulled loaded). + """ + if len(self.path) == 0 or not vs.loaded(): + return + + # If calculating the target point for the first time, find the nearest index to the vehicle's current position. + if self.target_index is None: + self.target_index = self._calc_nearest_index() + + # When the path is shorter then the lookahead, we will pick the last point. + if self.distance() < vs.lookahead_distance(): + x, y = self.path[-1] + return (x, y) + + # Find the first point that is further than the lookahead distance. + # If looking at the same path, start from the last target index. + for i, (x, y) in enumerate(self.path[self.target_index :]): + dist = math.hypot(x, y) + if dist > vs.lookahead_distance(): + self.target_index = i + return (x, y) + + +class PursePursuitController(Node): + """Pure pursuit controller node that publishes vehicle control commands based on the pure pursuit algorithm.""" + + def __init__(self): + super().__init__("pure_pursuit_controler") + + self.vehicle_state = VehicleState() + self.path = PursuitPath() + self.target_waypoint = None + self.clock = Clock().clock + + self.route_subscriber = self.create_subscription( + Path, "/planning/path", self.route_callback, 1 + ) + self.odometry_subscriber = self.create_subscription( + Odometry, "/gnss/odometry", self.odometry_callback, 1 + ) + self.speed_subscriber = self.create_subscription( + VehicleSpeed, "/speed", self.speed_callback, 1 + ) + self.clock_subscriber = self.create_subscription( + Clock, "/clock", self.clock_callback, 1 + ) + + self.command_publisher = self.create_publisher( + VehicleControl, "/vehicle/control", 1 + ) + self.barrier_marker_pub = self.create_publisher( + Marker, "/planning/barrier_marker", 1 + ) + self.lookahead_path_publisher = self.create_publisher(Path, "/ppc/path", 1) + + self.control_timer = self.create_timer(0.1, self.control_callback) + self.visualize_path_timer = self.create_timer(0.1, self.visualize_path_callback) + self.visualize_waypoint_timer = self.create_timer( + 0.1, self.visualize_waypoint_callback + ) + + def clock_callback(self, msg: Clock): + self.clock = msg.clock + + def odometry_callback(self, msg: Odometry): + self.vehicle_state.pose = msg.pose.pose + + def speed_callback(self, msg: VehicleSpeed): + self.vehicle_state.velocity = msg.speed + + def route_callback(self, msg: Path): + """Callback function for the path subscriber. + + Recalculates the path and target waypoint based on the received path message. + """ + path = [ + (pose_stamped.pose.position.x, pose_stamped.pose.position.y) + for pose_stamped in msg.poses + ] + self.path.set_path(path) + + def stop_vehicle(self, steer: float, break_value: float): + """Stop the vehicle by applying the break.""" + control_msg = VehicleControl() + control_msg.brake = break_value + control_msg.steer = steer + self.command_publisher.publish(control_msg) + + def control_callback(self): + """Calculate and publish the vehicle control commands based on the pure pursuit algorithm.""" + # Calculate the waypoint just outside the lookahead distance. + self.target_waypoint = self.path.calc_target_point(self.vehicle_state) + self.get_logger().info(f"Target Waypoint: {self.target_waypoint}") + + # If no target waypoint, do nothing. + if not self.target_waypoint: + return + + path_dist = self.path.distance() + if path_dist < Constants.STOPPING_DIST: + # Scale the break value linearly based on the distance to the end of the path. + # Break scale is 1.0 at the end of the path and 0.0 at the stopping distance. + break_scale = 1.0 - (path_dist / Constants.STOPPING_DIST) + break_value = min( + Constants.MAX_BREAK, + break_scale * Constants.MAX_BREAK, + ) + steer = self.vehicle_state.calc_steer(self.target_waypoint) + self.stop_vehicle(steer, break_value) + return + + # Calculate throttle, brake, and steer + throttle_brake = self.vehicle_state.calc_throttle_brake() + steer = self.vehicle_state.calc_steer(self.target_waypoint) + + self.get_logger().info(f"Steer: {steer} Throttle/Brake: {throttle_brake}") + + # If vehicle state is not loaded, do nothing. + if throttle_brake is None or steer is None: + return + + # Set throttle, brake, and steer and publish. + throttle, brake = throttle_brake + control_msg = VehicleControl() + control_msg.throttle = throttle + control_msg.brake = brake + control_msg.steer = steer + self.get_logger().info( + f"Steer: {control_msg.steer} Throttle: {throttle} Brake: {brake}" + ) + self.command_publisher.publish(control_msg) + + def visualize_waypoint_callback(self): + """Visualize the target waypoint in RVIZ.""" + if self.target_waypoint is None: + return + + relative_waypoint = 0.4 * np.asarray(self.target_waypoint) + + lookahead_dist = self.vehicle_state.lookahead_distance() + radius_marker = Marker( + header=Header(frame_id="base_link", stamp=self.clock), + ns="lookahead", + id=0, + type=Marker.CYLINDER, + action=Marker.ADD, + scale=Vector3(x=lookahead_dist * 2, y=lookahead_dist * 2, z=0.2), + color=ColorRGBA(a=0.3, g=1.0, b=1.0), + ) + self.barrier_marker_pub.publish(radius_marker) + + arrow_marker = Marker( + header=Header(frame_id="base_link", stamp=self.clock), + ns="lookahead", + id=1, + type=Marker.ARROW, + action=Marker.ADD, + scale=Vector3(x=0.5, y=0.8, z=0.3), + color=ColorRGBA(a=0.7, g=1.0, b=0.6), + points=[Point(), Point(x=relative_waypoint[0], y=relative_waypoint[1])], + ) + + self.barrier_marker_pub.publish(arrow_marker) + + def visualize_path_callback(self): + """Visualize the path in RVIZ.""" + current_path = self.path.get_current_path() + + if len(current_path) == 0: + return + + path_msg = Path() + path_msg.header.frame_id = "base_link" + path_msg.header.stamp = self.clock + + # Each RVIZ grid cell is 0.4m. + path = np.copy(np.asarray(current_path)) * 0.4 + + path_msg.poses = [ + PoseStamped( + header=path_msg.header, + pose=Pose(position=Point(x=x, y=y)), + ) + for x, y in path + ] + + self.lookahead_path_publisher.publish(path_msg) + + +def main(args=None): + rclpy.init(args=args) + + minimal_publisher = PursePursuitController() + + rclpy.spin(minimal_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + minimal_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/control/pure_pursuit_controller/resource/pure_pursuit_controller b/src/control/pure_pursuit_controller/resource/pure_pursuit_controller new file mode 100755 index 00000000..e69de29b diff --git a/src/control/pure_pursuit_controller/setup.cfg b/src/control/pure_pursuit_controller/setup.cfg new file mode 100755 index 00000000..f56125a2 --- /dev/null +++ b/src/control/pure_pursuit_controller/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/pure_pursuit_controller +[install] +install_scripts=$base/lib/pure_pursuit_controller diff --git a/src/control/pure_pursuit_controller/setup.py b/src/control/pure_pursuit_controller/setup.py new file mode 100755 index 00000000..78e03d9e --- /dev/null +++ b/src/control/pure_pursuit_controller/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = "pure_pursuit_controller" + +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="root", + maintainer_email="root@todo.todo", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "pure_pursuit_controller = pure_pursuit_controller.pure_pursuit_controller_node:main" + ], + }, +)