├── .gitignore
├── docs
    ├── demo.gif
    ├── diagram.png
    └── diagram.drawio
├── config
    ├── AreaToPath.cfg
    └── LinePlanner.cfg
├── CMakeLists.txt
├── package.xml
├── LICENSE
├── launch
    └── line_planner.launch
├── src
    ├── utils.py
    ├── markers.py
    ├── area_to_path_node.py
    └── line_planner_node.py
└── ReadMe.md


/.gitignore:
--------------------------------------------------------------------------------
1 |  .vscode/*


--------------------------------------------------------------------------------
/docs/demo.gif:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MoffKalast/line_planner/HEAD/docs/demo.gif


--------------------------------------------------------------------------------
/docs/diagram.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/MoffKalast/line_planner/HEAD/docs/diagram.png


--------------------------------------------------------------------------------
/config/AreaToPath.cfg:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | 
 3 | PACKAGE = "line_planner"
 4 | 
 5 | from dynamic_reconfigure.parameter_generator_catkin import *
 6 | 
 7 | gen = ParameterGenerator()
 8 | 
 9 | gen.add("step_size", double_t, 0, "Step size for grid-based patterns", 2.0, 0.1, 100.0)
10 | 
11 | exit(gen.generate(PACKAGE, "line_planner", "AreaToPath"))


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.0.2)
 2 | project(line_planner)
 3 | 
 4 | find_package(catkin REQUIRED COMPONENTS
 5 |     dynamic_reconfigure
 6 |     rospy
 7 |     move_base_msgs
 8 |     std_msgs
 9 |     message_generation
10 | )
11 | 
12 | generate_dynamic_reconfigure_options(
13 |     config/LinePlanner.cfg
14 |     config/AreaToPath.cfg
15 | )
16 | 
17 | catkin_package(
18 |     CATKIN_DEPENDS dynamic_reconfigure rospy geometry_msgs move_base_msgs std_msgs
19 | )
20 | 
21 | include_directories(
22 |     ${catkin_INCLUDE_DIRS}
23 | )
24 | 
25 | install(PROGRAMS
26 |     src/line_planner_node.py
27 |     src/area_to_path_node.py
28 |     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
29 | )


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <package format="2">
 2 |     <name>line_planner</name>
 3 |     <version>0.0.1</version>
 4 |     <description>A simple planner that attempts to keep the robot close to a line between the starting and target goal.</description>
 5 |     <maintainer email="vid.rijavec@gmail.com">MoffKalast</maintainer>
 6 |     <license>MIT</license>
 7 | 
 8 |     <depend>rospy</depend>
 9 |     <depend>dynamic_reconfigure</depend>
10 | 
11 |     <buildtool_depend>catkin</buildtool_depend>
12 |     <build_depend>actionlib_msgs</build_depend>
13 |     <build_depend>geometry_msgs</build_depend>
14 |     <build_depend>move_base_msgs</build_depend>
15 |     <build_depend>std_msgs</build_depend>
16 | 
17 |     <build_export_depend>actionlib_msgs</build_export_depend>
18 |     <build_export_depend>move_base_msgs</build_export_depend>
19 |     <build_export_depend>std_msgs</build_export_depend>
20 | 
21 |     <exec_depend>geometry_msgs</exec_depend>
22 |     <exec_depend>actionlib_msgs</exec_depend>
23 |     <exec_depend>move_base_msgs</exec_depend>
24 |     <exec_depend>std_msgs</exec_depend>
25 | 
26 | </package>
27 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2023 MoffKalast
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/config/LinePlanner.cfg:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | 
 3 | PACKAGE = "line_planner"
 4 | 
 5 | from dynamic_reconfigure.parameter_generator_catkin import *
 6 | 
 7 | gen = ParameterGenerator()
 8 | 
 9 | gen.add("publish_debug_markers", bool_t, 0, "Publish debug markers", True)
10 | gen.add("ignore_altitude", bool_t, 0, "Ignore Z values", False)
11 | 
12 | gen.add("max_linear_speed", double_t, 0, "Maximum linear speed", 0.45, 0, 5)
13 | gen.add("max_turning_speed", double_t, 0, "Maximum turning speed", 0.9, 0, 5)
14 | gen.add("max_vertical_speed", double_t, 0, "Maximum vertical speed", 0.5, 0, 5)
15 | 
16 | gen.add("max_line_divergence", double_t, 0, "Maximum line divergence", 1.0, 0, 50)
17 | 
18 | gen.add("min_project_dist", double_t, 0, "Minimum projection distance", 0.15, 0.01, 20)
19 | gen.add("max_project_dist", double_t, 0, "Maximum projection distance", 1.2, 0, 100)
20 | 
21 | gen.add("xy_distance_threshold", double_t, 0, "Horizontal distance threshold", 0.6, 0, 10)
22 | gen.add("z_distance_threshold", double_t, 0, "Vertical distance threshold", 0.6, 0, 10)
23 | 
24 | gen.add("P", double_t, 0, "Proportional gain for PID controller", 3.0, 0, 100)
25 | gen.add("I", double_t, 0, "Integral gain for PID controller", 0.001, 0, 10)
26 | gen.add("D", double_t, 0, "Derivative gain for PID controller", 65.0, 0, 100)
27 | 
28 | gen.add("side_offset_mult", double_t, 0, "Side projection multiplier", 0.5, 0, 1.0)
29 | 
30 | gen.add("rate", int_t, 0, "Update rate", 30, 10, 60)
31 | 
32 | exit(gen.generate(PACKAGE, "line_planner", "LinePlanner"))


--------------------------------------------------------------------------------
/launch/line_planner.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |     <arg name="robot_frame" default="base_link" />
 4 |     <arg name="planning_frame" default="map" />
 5 | 
 6 |     <node name="line_planner" pkg="line_planner" type="line_planner_node.py" output="screen">
 7 | 
 8 |         <param name="robot_frame" value="$(arg robot_frame)"/>
 9 |         <param name="planning_frame" value="$(arg planning_frame)"/>
10 | 
11 |         <param name="publish_debug_markers" value="true"/>
12 |         <param name="ignore_altitude" value="false"/>
13 | 
14 |         <!--Speeds will be clamped to these values -->
15 |         <param name="max_linear_speed" value="1.0"/>
16 |         <param name="max_turning_speed" value="2.0"/>
17 |         <param name="max_vertical_speed" value="1.0"/>
18 | 
19 |         <!-- If base_link is this far away from the line, the projected distance will be min and scale to max when it's on the line.-->
20 |         <param name="max_line_divergence" value="1.5"/>
21 | 
22 |         <!-- If any obstacles are defined then line divergence + robot width are used for finding a suitably wide path.-->
23 |         <param name="robot_width" value="3.0"/>
24 | 
25 |         <!-- Reduce min distance and increase max distance for smoother, but less accurate following.-->
26 |         <param name="min_project_dist" value="0.3"/>
27 |         <param name="max_project_dist" value="5.0"/>
28 | 
29 |         <!-- Distance at which the goal is considered reached.-->
30 |         <param name="xy_distance_threshold" value="1.0"/>
31 |         <param name="z_distance_threshold" value="0.5"/>
32 | 
33 |         <!-- PID params for rotational heading control.-->
34 |         <param name="P" value="2.0"/>
35 |         <param name="I" value="0.002"/>
36 |         <param name="D" value="65.0"/>
37 | 
38 |         <!-- If the robot frame is away from the line, the goal will be mirrored into the opposite direction and multiplied with this value.-->
39 |         <param name="side_offset_mult" value="0.8"/>
40 | 
41 |         <!-- Update rate, should be about the same as localization rate.-->
42 |         <param name="rate" value="30"/>
43 |     </node>
44 |     
45 |     <node name="area_to_path_node" pkg="line_planner" type="area_to_path_node.py" output="screen">
46 |         <param name="step_size" value="2.0"/>
47 |     </node>
48 | 
49 | </launch>
50 | 


--------------------------------------------------------------------------------
/src/utils.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python3
  2 | 
  3 | import math
  4 | import numpy as np
  5 | 
  6 | from geometry_msgs.msg import Pose, Point
  7 | 
  8 | def project_position(start, end, current, mindist, maxdist, line_divergence, side_offset):
  9 | 	
 10 | 	def unit_vector(vector):
 11 | 		return vector / np.linalg.norm(vector + 1e-10)  # A small constant to prevent division by zero
 12 | 	
 13 | 	def pose_to_np(p):
 14 | 		return np.array([p.position.x, p.position.y])
 15 | 
 16 | 	def np_to_point(np_array):
 17 | 		return Point(np_array[0], np_array[1], 0)
 18 | 
 19 | 	start_pos = pose_to_np(start)
 20 | 	end_pos = pose_to_np(end)
 21 | 	current_pos = pose_to_np(current)
 22 | 
 23 | 	# Calculate the vector representing the line segment
 24 | 	line = end_pos - start_pos
 25 | 	unit_line = unit_vector(line)
 26 | 	
 27 | 	# Calculate the projection of the current point onto the line
 28 | 	current_to_start = current_pos - start_pos
 29 | 	projection_length = np.dot(current_to_start, unit_line)
 30 | 	projection = start_pos + unit_line * projection_length
 31 | 
 32 | 	# Clamp the projection to the start point if necessary
 33 | 	if np.dot(projection - start_pos, start_pos - end_pos) > 0:
 34 | 		projection = start_pos
 35 | 
 36 | 	# Calculate the distance and new position based on input parameters
 37 | 	deltadist = current_pos - projection 
 38 | 	value = 0 if line_divergence == 0 else (line_divergence - np.sqrt(deltadist.dot(deltadist))) / line_divergence
 39 | 	distance = mindist + (maxdist - mindist) * clamp(value, 0.0, 1.0)
 40 | 	new_pos = projection + unit_vector(end_pos - start_pos) * distance
 41 | 
 42 | 	# Clamp the new position to the end point if necessary
 43 | 	if np.dot(new_pos - end_pos, end_pos - start_pos) > 0:
 44 | 		new_pos = end_pos
 45 | 
 46 | 	# Add the side vector for more aggressive tracking
 47 | 	additional_vector = -side_offset * (current_pos - projection)
 48 | 
 49 | 	return np_to_point(new_pos + additional_vector)
 50 | 
 51 | def clamp(num, min, max):
 52 | 	return min if num < min else max if num > max else num
 53 | 
 54 | def transform_to_pose(t):
 55 | 	pose = Pose()
 56 | 	pose.position.x = t.transform.translation.x
 57 | 	pose.position.y = t.transform.translation.y
 58 | 	pose.position.z = t.transform.translation.z
 59 | 	pose.orientation.x = t.transform.rotation.x
 60 | 	pose.orientation.y = t.transform.rotation.y
 61 | 	pose.orientation.z = t.transform.rotation.z
 62 | 	pose.orientation.w = t.transform.rotation.w
 63 | 
 64 | 	return pose
 65 | 
 66 | def get_dir(from_vec, to_vec):
 67 | 	target_direction = [
 68 | 		to_vec.x - from_vec.x,
 69 | 		to_vec.y - from_vec.y,
 70 | 	]
 71 | 	target_distance = math.sqrt(target_direction[0] ** 2 + target_direction[1] ** 2)
 72 | 
 73 | 	return [target_direction[0] / target_distance, target_direction[1] / target_distance]
 74 | 
 75 | class PID:
 76 | 	def __init__(self, kp, ki, kd, target=0, windup_guard=20.0):
 77 | 		self.kp = kp
 78 | 		self.ki = ki
 79 | 		self.kd = kd
 80 | 		self.target = target
 81 | 
 82 | 		self.windup_guard = windup_guard
 83 | 
 84 | 		self.prev_error = 0.0
 85 | 		self.integral = 0.0
 86 | 		self.derivative = 0.0
 87 | 
 88 | 	def reset(self):
 89 | 		self.prev_error = 0.0
 90 | 		self.integral = 0.0
 91 | 		self.derivative = 0.0
 92 | 
 93 | 	def compute(self, current_value):
 94 | 		error = self.target - current_value
 95 | 
 96 | 		self.integral += error
 97 | 		self.integral = max(min(self.integral, self.windup_guard), -self.windup_guard)
 98 | 
 99 | 		self.derivative = error - self.prev_error
100 | 
101 | 		output = self.kp * error + self.ki * self.integral + self.kd * self.derivative
102 | 
103 | 		self.prev_error = error
104 | 
105 | 		return output


--------------------------------------------------------------------------------
/src/markers.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | 
 3 | import rospy
 4 | import math
 5 | from visualization_msgs.msg import Marker, MarkerArray
 6 | from std_msgs.msg import ColorRGBA
 7 | from geometry_msgs.msg import Point, Pose
 8 | 
 9 | class DebugMarkers:
10 | 
11 |     def __init__(self, planning_frame):
12 |         self.planning_frame = planning_frame
13 |         self.marker_pub = rospy.Publisher("line_planner/markers", MarkerArray, queue_size=1)
14 | 
15 |         self.queued_marker_array = None
16 |         self.publish_timer = rospy.Timer(rospy.Duration(0.2), self._timer_callback)  # 200 ms throttle
17 | 
18 |     def _timer_callback(self, event):
19 |         if self.queued_marker_array is not None:
20 |             self.marker_pub.publish(self.queued_marker_array)
21 |             self.queued_marker_array = None
22 | 
23 |     def delete_debug_markers(self):
24 |         marker = Marker()
25 |         marker.action = Marker.DELETEALL
26 |         markerArray = MarkerArray()
27 |         markerArray.markers.append(marker)
28 |         self.queued_marker_array = markerArray
29 | 
30 |     def get_side_vec(self, start_goal, end_goal, max_divergence):
31 |         start = start_goal.position
32 |         end = end_goal.position
33 | 
34 |         delta_x = end.x - start.x
35 |         delta_y = end.y - start.y
36 | 
37 |         # Normalize the direction vector
38 |         magnitude = math.hypot(delta_x, delta_y) + 0.0001
39 |         delta_x = (delta_x / magnitude) * max_divergence
40 |         delta_y = (delta_y / magnitude) * max_divergence
41 | 
42 |         # Calculate perpendicular vectors
43 |         start_left = Point(start.x + delta_y, start.y - delta_x, start.z)
44 |         start_right = Point(start.x - delta_y, start.y + delta_x, start.z)
45 | 
46 |         end_left = Point(end.x + delta_y, end.y - delta_x, end.z)
47 |         end_right = Point(end.x - delta_y, end.y + delta_x, end.z)
48 | 
49 |         return start_left, start_right, end_left, end_right
50 | 
51 |     def draw_debug_markers(self, target_position, start_goal, end_goal, min_goal_dist, max_divergence):
52 | 
53 |         def sphere_marker(position, marker_id, r, g, b, size):
54 |             marker = Marker()
55 |             marker.header.frame_id = self.planning_frame
56 |             marker.type = Marker.SPHERE
57 |             marker.pose.position = position
58 |             marker.pose.orientation.w = 1.0
59 |             marker.scale.x = size
60 |             marker.scale.y = size
61 |             marker.scale.z = size
62 |             marker.color = ColorRGBA(r, g, b, 0.5)
63 |             marker.id = marker_id
64 |             return marker
65 | 
66 |         def line_marker(p_from, p_to, marker_id, r, g, b):
67 |             marker = Marker()
68 |             marker.header.frame_id = self.planning_frame
69 |             marker.type = Marker.LINE_STRIP
70 |             marker.pose.orientation.w = 1.0
71 |             marker.points = [p_from, p_to]
72 |             marker.colors = [ColorRGBA(r, g, b, 1.0), ColorRGBA(r, g, b, 1.0)]
73 |             marker.scale.x = 0.03
74 |             marker.id = marker_id
75 |             return marker
76 | 
77 |         start_left, start_right, end_left, end_right = self.get_side_vec(start_goal, end_goal, max_divergence)
78 | 
79 |         markerArray = MarkerArray()
80 |         markerArray.markers.append(line_marker(start_left, end_left, 0, 0.1, 0.6, 0.1))
81 |         markerArray.markers.append(line_marker(start_right, end_right, 1, 0.6, 0.2, 0.2))
82 |         markerArray.markers.append(line_marker(start_goal.position, end_goal.position, 2, 0.870, 0.870, 0.870))
83 |         markerArray.markers.append(sphere_marker(start_goal.position, 3, 1.0, 0.0, 0.0, 0.2))
84 |         markerArray.markers.append(sphere_marker(end_goal.position, 4, 0.0, 0.0, 1.0, min_goal_dist * 2))
85 |         markerArray.markers.append(sphere_marker(target_position, 5, 0.0, 1.0, 0.0, 0.2))
86 |         self.queued_marker_array = markerArray
87 | 


--------------------------------------------------------------------------------
/ReadMe.md:
--------------------------------------------------------------------------------
  1 |  # Simple Projecting Line Planner
  2 | 
  3 | [![License: MIT](https://img.shields.io/badge/License-MIT-yellow.svg)](https://opensource.org/licenses/MIT) [![Build Status](https://build.ros.org/buildStatus/icon?job=Ndev__line_planner__ubuntu_focal_amd64&build=3)](https://build.ros.org/job/Ndev__line_planner__ubuntu_focal_amd64/3/)
  4 | 
  5 | A local planner that takes two goals (last and next), and follows a projected goal that keeps it close to the line segment between the two goals. Designed to resist strong side forces, such as from wind or water current.
  6 | 
  7 | ![demo image](docs/demo.gif)
  8 | 
  9 | ## Params
 10 | 
 11 | ```xml
 12 | <node name="line_planner" pkg="line_planner" type="line_planner_node.py" output="screen">
 13 | 	<param name="robot_frame" value="base_link"/>
 14 | 	<param name="planning_frame" value="map"/>
 15 | 
 16 | 	<param name="publish_debug_markers" value="true"/>
 17 | 	<param name="ignore_altitude" value="false"/> <!-- Won't bother with z values -->
 18 | 
 19 | 	<param name="max_linear_speed" value="3.0"/>
 20 | 	<param name="max_turning_speed" value="2.0"/>
 21 | 	<param name="max_vertical_speed" value="10.0"/>
 22 | 
 23 | 	<!-- If base_link is this far away from the line, the projected distance will be min and scale to max when it's on the line.-->
 24 | 	<param name="max_line_divergence" value="1.5"/>
 25 | 
 26 | 	<!-- If any obstacles are defined then line divergence + robot width are used for finding a suitably wide path.-->
 27 | 	<param name="robot_width" value="3.0"/>
 28 | 
 29 | 	<param name="min_project_dist" value="0.3"/>
 30 | 	<param name="max_project_dist" value="5.0"/>
 31 | 
 32 | 	<!-- Distance at which the goal is considered reached.-->
 33 | 	<param name="xy_distance_threshold" value="1.0"/>
 34 | 	<param name="z_distance_threshold" value="0.5"/>
 35 | 
 36 | 	<!-- PID params for heading control.-->
 37 | 	<param name="P" value="2.0"/>
 38 | 	<param name="I" value="0.002"/>
 39 | 	<param name="D" value="65.0"/>
 40 | 
 41 | 	<!-- If the robot frame is away from the line, the goal will be mirrored into the opposite direction and multiplied with this value.-->
 42 | 	<param name="side_offset_mult" value="0.8"/>
 43 | 
 44 | 	<!-- Update rate, should be about the same as localization rate.-->
 45 | 	<param name="rate" value="30"/>
 46 | </node>
 47 | ```
 48 | 
 49 | Here's a diagram showing the possible states of the planner, and which distances each parameter affects:
 50 | 
 51 | ![diagram](docs/diagram.png)
 52 | 
 53 | 
 54 | ## Subscribed Topics
 55 | 
 56 | - `/move_base_simple/goal` (PoseStamped), takes the current position as the starting point and moves towards the goal
 57 | 
 58 | - `/move_base_simple/clear` (Empty), stops all movement immediately
 59 | 
 60 | - `/move_base_simple/waypoints` (Path), takes each two consecutive points and navigates along the line between them
 61 | 
 62 | ## Published Topics
 63 | 
 64 | - `/cmd_vel` (Twist), publishes velocity for vehicle motion
 65 | 
 66 | - `line_planner/active` (Bool), publishes navigation status
 67 | 
 68 | - `line_planner/plan` (Path), publishes a nav plan, also the entire route if given
 69 | 
 70 | - `line_planner/markers` (MarkerArray), publishes debug markers shown above
 71 | 
 72 | - `line_planner/vertical_target` (Float32), publishes the current altitude target
 73 | 
 74 |  ## Dynamic Reconfigure Params
 75 | 
 76 | - `publish_debug_markers` (bool_t), if set to True, the node will publish markers for debugging purposes.
 77 | 
 78 | - `ignore_altitude` (bool_t), if true, Z values will be disregarded.
 79 | 
 80 | - `max_linear_speed` (double_t), the maximum linear speed of the robot.
 81 | 
 82 | - `max_turning_speed` (double_t), the maximum speed at which the robot can turn.
 83 | 
 84 | - `max_vertical_speed` (double_t), the maximum vertical speed of the robot.
 85 | 
 86 | - `max_line_divergence` (double_t), the maximum distance that the robot can diverge from the line between the goals.
 87 | 
 88 | - `min_project_dist` (double_t), the minimum projection distance for the goal.
 89 | 
 90 | - `max_project_dist` (double_t), the maximum projection distance for the goal.
 91 | 
 92 | - `goal_distance_threshold` (double_t), the distance at which a goal is considered reached.
 93 | 
 94 | - `P` (double_t), the proportional gain for the PID controller that controls the heading of the robot.
 95 | 
 96 | - `I` (double_t), the integral gain for the PID controller.
 97 | 
 98 | - `D` (double_t), the derivative gain for the PID controller.
 99 | 
100 | - `side_offset_mult` (double_t), multiplier for the side projection of the robot's position.
101 | 
102 | - `rate` (int_t), the rate at which the robot updates its position and velocity.
103 | 
104 | Based on the code you've provided, here's an addition to the documentation that describes the functionality of your newly added node:
105 | 
106 | ---
107 | 
108 | ## Bounding Box to Path Helper
109 | 
110 | This package also contains a small helper demo node that takes a rectangle defined as a PolygonStamped and turns it into various patterns which then get published as a Path that the line planer can execute. Right now it provides three different patterns: lawnmower, expanding square, and victor sierra. 
111 | 
112 | The lawnmower pattern creates a path in a back-and-forth, or "mowing the lawn" manner. The expanding square pattern creates a square outward spiral path. The victor sierra pattern is a coast guard sector search. 
113 | 
114 | it also accepts a home polygon, which it uses to add a final waypoint that points back home after following the path, so the robot doesn't get stuck out of wifi range.
115 | 
116 | ## Params
117 | 
118 | ```xml
119 | <node name="area_to_path_node" pkg="line_planner" type="area_to_path_node.py" output="screen">
120 | 	<param name="step_size" value="2.0"/> <!-- determines the distance between paralel lines -->
121 | </node>
122 | ```
123 | 
124 | ## Subscribed Topics
125 | 
126 | - `/area_to_path/lawnmower` (PolygonStamped), takes a 4 vertex square and creates a lawnmower path inside it.
127 | 
128 | - `/area_to_path/expanding_square` (PolygonStamped), takes a 4 vertex square and creates an expanding square (spiral) path inside it.
129 | 
130 | - `/area_to_path/victor_sierra` (PolygonStamped), takes a 4 vertex square and creates a victor sierra (star) path inside it.
131 | 
132 | - `/area_to_path/home` (PolygonStamped), takes a 4 vertex square and sets the center point as the home position.
133 | 
134 | ## Published Topics
135 | 
136 | - `/move_base_simple/waypoints` (Path), publishes the created path.
137 | 


--------------------------------------------------------------------------------
/src/area_to_path_node.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python3
  2 | 
  3 | import math
  4 | import rospy
  5 | import itertools
  6 | 
  7 | from nav_msgs.msg import Path
  8 | from geometry_msgs.msg import PoseStamped, PolygonStamped, Point
  9 | 
 10 | from dynamic_reconfigure.server import Server as DynamicReconfigureServer
 11 | from line_planner.cfg import AreaToPathConfig
 12 | 
 13 | class BoundingBox3DListener:
 14 | 	def __init__(self):
 15 | 		rospy.init_node("area_to_path_node")
 16 | 
 17 | 		self.path_pub = rospy.Publisher("/move_base_simple/waypoints", Path, queue_size=10)
 18 | 
 19 | 		self.lawnmower_sub = rospy.Subscriber("/area_to_path/lawnmower", PolygonStamped, self.lawnmower_callback)
 20 | 		self.square_sub = rospy.Subscriber("/area_to_path/expanding_square", PolygonStamped, self.square_callback)
 21 | 		self.sierra_sub = rospy.Subscriber("/area_to_path/victor_sierra", PolygonStamped, self.sierra_callback)
 22 | 		self.home_sub = rospy.Subscriber("/area_to_path/home", PolygonStamped, self.home_callback)
 23 | 
 24 | 		self.STEP_SIZE = rospy.get_param("~step_size", 2.0)  # The step size (X meters) in the path
 25 | 
 26 | 		self.home_point = None
 27 | 		self.home_header = None
 28 | 
 29 | 		self.reconfigure_server = DynamicReconfigureServer(AreaToPathConfig, self.dynamic_reconfigure_callback)
 30 | 
 31 | 	def dynamic_reconfigure_callback(self, config, level):
 32 | 		self.STEP_SIZE = config.step_size
 33 | 		return config
 34 | 
 35 | 	def send_path(self, header, poses):
 36 | 		if len(poses) == 0:
 37 | 			rospy.logwarn("Send a larger polygon or reduce step size.")
 38 | 			return
 39 | 
 40 | 		path = Path()
 41 | 		path.header = header
 42 | 		path.poses = poses
 43 | 		self.path_pub.publish(path)
 44 | 
 45 | 	def home_callback(self, msg):
 46 | 		p = msg.polygon.points
 47 | 
 48 | 		if len(p) != 4:
 49 | 			rospy.logwarn("Invalid Polygon, path creation requires a 4 vertex square.")
 50 | 			return
 51 | 
 52 | 		self.home_point = Point(
 53 | 			x=(p[0].x + p[2].x) / 2,
 54 | 			y=(p[0].y + p[2].y) / 2
 55 | 		)
 56 | 
 57 | 		self.home_header = msg.header
 58 | 
 59 | 		rospy.loginfo("Home point set.")
 60 | 
 61 | 	def norm(self, point):
 62 | 		p = Point()
 63 | 
 64 | 		psum = math.sqrt(point.x **2 + point.y **2 + point.z **2)
 65 | 		if psum == 0:
 66 | 			return p
 67 | 
 68 | 		p.x = point.x / psum
 69 | 		p.y = point.y / psum
 70 | 		p.z = point.z / psum
 71 | 
 72 | 		return p
 73 | 
 74 | 	def get_pose(self, header, x, y):
 75 | 		pose = PoseStamped()
 76 | 		pose.header = header
 77 | 		pose.pose.position.x = x
 78 | 		pose.pose.position.y = y
 79 | 		pose.pose.position.z = 0
 80 | 		pose.pose.orientation.w = 1.0
 81 | 		return pose
 82 | 	
 83 | 	def sierra_callback(self, msg):
 84 | 		p = msg.polygon.points
 85 | 
 86 | 		if len(p) != 4:
 87 | 			rospy.logwarn("Invalid Polygon, path creation requires a 4 vertex square.")
 88 | 			return
 89 | 
 90 | 		poses = []
 91 | 
 92 | 		# Calculate vectors between points
 93 | 		vec_side_1 = Point(x=p[1].x - p[0].x, y=p[1].y - p[0].y)
 94 | 		vec_side_2 = Point(x=p[3].x - p[0].x, y=p[3].y - p[0].y)
 95 | 
 96 | 		# Compute the lengths of the two sides and the average (which will be our radius)
 97 | 		length_side_1 = math.sqrt(vec_side_1.x**2 + vec_side_1.y**2)
 98 | 		length_side_2 = math.sqrt(vec_side_2.x**2 + vec_side_2.y**2)
 99 | 		radius = (length_side_1 + length_side_2) / 4
100 | 
101 | 		# Calculate the center point of the search area
102 | 		center = Point(
103 | 			x=(p[0].x + p[2].x) / 2,
104 | 			y=(p[0].y + p[2].y) / 2
105 | 		)
106 | 
107 | 		poses.append(self.get_pose(msg.header, center.x, center.y))
108 | 
109 | 		angle = math.radians(0)
110 | 		poses.append(self.get_pose(msg.header, 
111 | 			center.x + radius * math.cos(angle), 
112 | 			center.y + radius * math.sin(angle)
113 | 		))
114 | 
115 | 		angle = math.radians(60)
116 | 		poses.append(self.get_pose(msg.header, 
117 | 			center.x + radius * math.cos(angle), 
118 | 			center.y + radius * math.sin(angle)
119 | 		))
120 | 
121 | 		angle = math.radians(240)
122 | 		poses.append(self.get_pose(msg.header, 
123 | 			center.x + radius * math.cos(angle), 
124 | 			center.y + radius * math.sin(angle)
125 | 		))
126 | 
127 | 		angle = math.radians(300)
128 | 		poses.append(self.get_pose(msg.header, 
129 | 			center.x + radius * math.cos(angle), 
130 | 			center.y + radius * math.sin(angle)
131 | 		))
132 | 
133 | 		angle = math.radians(120)
134 | 		poses.append(self.get_pose(msg.header, 
135 | 			center.x + radius * math.cos(angle), 
136 | 			center.y + radius * math.sin(angle)
137 | 		))
138 | 
139 | 		angle = math.radians(180)
140 | 		poses.append(self.get_pose(msg.header, 
141 | 			center.x + radius * math.cos(angle), 
142 | 			center.y + radius * math.sin(angle)
143 | 		))
144 | 
145 | 		poses.append(self.get_pose(msg.header, center.x, center.y))
146 | 
147 | 		if self.home_point != None:
148 | 			poses.append(self.get_pose(self.home_header, self.home_point.x, self.home_point.y))
149 | 
150 | 		self.send_path(msg.header, poses)
151 | 
152 | 	def square_callback(self, msg):
153 | 		p = msg.polygon.points
154 | 
155 | 		if len(p) != 4:
156 | 			rospy.logwarn("Invalid Polygon, path creation requires a 4 vertex square.")
157 | 			return
158 | 
159 | 		poses = []
160 | 
161 | 		# Calculate vectors between points
162 | 		vec_side_1 = Point(x=p[1].x - p[0].x, y=p[1].y - p[0].y)
163 | 		vec_side_2 = Point(x=p[3].x - p[0].x, y=p[3].y - p[0].y)
164 | 
165 | 		# Compute the lengths of the two sides
166 | 		length_side_1 = math.sqrt(vec_side_1.x**2 + vec_side_1.y**2)
167 | 		length_side_2 = math.sqrt(vec_side_2.x**2 + vec_side_2.y**2)
168 | 
169 | 		# Determine the center of the square
170 | 		center = Point(x=p[0].x + vec_side_1.x/2 + vec_side_2.x/2, y=p[0].y + vec_side_1.y/2 + vec_side_2.y/2)
171 | 
172 | 		# The spiral is drawn as a series of line segments, starting from the center of the square
173 | 		pos = [center.x, center.y]
174 | 		dir = [0, -1]  # Initial direction: up
175 | 
176 | 		# Continue drawing the spiral until we reach the edge of the square
177 | 		for step in itertools.count(start=1, step=1):
178 | 			if self.STEP_SIZE * step > max(length_side_1, length_side_2):  # If we would go beyond the edge of the square, end the spiral
179 | 				break
180 | 
181 | 			for _ in range(2):  # Each "layer" of the spiral consists of two steps
182 | 				for _ in range(step):
183 | 					if abs(pos[0] - center.x) <= length_side_1 / 2 and abs(pos[1] - center.y) <= length_side_2 / 2:
184 | 						pos[0] += dir[0] * self.STEP_SIZE
185 | 						pos[1] += dir[1] * self.STEP_SIZE
186 | 
187 | 				
188 | 				poses.append(self.get_pose(msg.header, pos[0], pos[1]))
189 | 				dir = [-dir[1], dir[0]]  # Rotate direction 90 degrees to the right
190 | 
191 | 		if self.home_point != None:
192 | 			poses.append(self.get_pose(self.home_header, self.home_point.x, self.home_point.y))
193 | 
194 | 		self.send_path(msg.header, poses)
195 | 
196 | 
197 | 	def lawnmower_callback(self, msg):
198 | 		p = msg.polygon.points
199 | 
200 | 		if len(p) != 4:
201 | 			rospy.logwarn("Invalid Polygon, path creation requires a 4 vertex square.")
202 | 			return
203 | 
204 | 		poses = []
205 | 
206 | 		# Calculate vectors between points
207 | 		vec_side_1 = Point(x=p[1].x - p[0].x, y=p[1].y - p[0].y)
208 | 		vec_side_2 = Point(x=p[3].x - p[0].x, y=p[3].y - p[0].y)
209 | 
210 | 		# Compute the lengths of the two sides
211 | 		length_side_1 = math.sqrt(vec_side_1.x**2 + vec_side_1.y**2)
212 | 		length_side_2 = math.sqrt(vec_side_2.x**2 + vec_side_2.y**2)
213 | 
214 | 		# Decide which side to move along based on their lengths
215 | 		if length_side_1 > length_side_2:
216 | 			vec_step = self.norm(vec_side_2)
217 | 			vec_side = vec_side_1
218 | 			steps = math.ceil(length_side_2 / self.STEP_SIZE)
219 | 		else:
220 | 			vec_step = self.norm(vec_side_1)
221 | 			vec_side = vec_side_2
222 | 			steps = math.ceil(length_side_1 / self.STEP_SIZE)
223 | 
224 | 		vec_step.x *= self.STEP_SIZE
225 | 		vec_step.y *= self.STEP_SIZE
226 | 
227 | 		for i in range(steps):
228 | 			if i % 2 == 0:  # For every other line, the direction should be reversed
229 | 				# Start of the line
230 | 				poses.append(self.get_pose(
231 | 					msg.header,
232 | 					p[0].x + i * vec_step.x,
233 | 					p[0].y + i * vec_step.y
234 | 				))
235 | 
236 | 				# End of the line
237 | 				poses.append(self.get_pose(
238 | 					msg.header,
239 | 					p[0].x + i * vec_step.x + vec_side.x,
240 | 					p[0].y + i * vec_step.y + vec_side.y,
241 | 				))
242 | 			else:
243 | 				# Start of the line
244 | 				poses.append(self.get_pose(
245 | 					msg.header,
246 | 					p[0].x + i * vec_step.x + vec_side.x,
247 | 					p[0].y + i * vec_step.y + vec_side.y,
248 | 				))
249 | 
250 | 				# End of the line
251 | 				poses.append(self.get_pose(
252 | 					msg.header,
253 | 					p[0].x + i * vec_step.x,
254 | 					p[0].y + i * vec_step.y,
255 | 				))
256 | 
257 | 		if self.home_point != None:
258 | 			poses.append(self.get_pose(self.home_header, self.home_point.x, self.home_point.y))
259 | 
260 | 		self.send_path(msg.header, poses)
261 | 
262 | 
263 | box = BoundingBox3DListener()
264 | rospy.spin()
265 | 
266 | 
267 | 


--------------------------------------------------------------------------------
/src/line_planner_node.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python3
  2 | from xmlrpc.client import Boolean
  3 | import rospy
  4 | import math
  5 | import tf
  6 | import tf2_ros
  7 | 
  8 | from utils import *
  9 | from markers import DebugMarkers
 10 | 
 11 | from geometry_msgs.msg import Twist, Point
 12 | from visualization_msgs.msg import Marker, MarkerArray
 13 | from geometry_msgs.msg import PoseStamped
 14 | from std_msgs.msg import Empty, ColorRGBA, Bool, Float32
 15 | 
 16 | from tf.transformations import euler_from_quaternion
 17 | from tf2_geometry_msgs import do_transform_pose
 18 | 
 19 | from dynamic_reconfigure.server import Server as DynamicReconfigureServer
 20 | 
 21 | from line_planner.cfg import LinePlannerConfig
 22 | from nav_msgs.msg import Path
 23 | 
 24 | ROBOT_FRAME = "base_link"
 25 | PLANNING_FRAME = "map"
 26 | 
 27 | class GoalServer:
 28 | 
 29 | 	def __init__(self, tf2_buffer, update_plan):
 30 | 		self.start_goal = None
 31 | 		self.end_goal = None
 32 | 		self.tf2_buffer = tf2_buffer
 33 | 		self.update_plan = update_plan
 34 | 
 35 | 		self.simple_goal_sub = rospy.Subscriber("/move_base_simple/waypoints", Path, self.route_callback)
 36 | 		self.simple_goal_sub = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.goal_callback)
 37 | 		self.clear_goals_sub = rospy.Subscriber("/move_base_simple/clear", Empty, self.reset)
 38 | 
 39 | 		self.vertical_pub = rospy.Publisher("line_planner/vertical_target", Float32, queue_size=1)
 40 | 
 41 | 		self.start_goal = None
 42 | 		self.end_goal = None
 43 | 		self.route = []
 44 | 		self.route_index = 0
 45 | 
 46 | 	def reset(self,msg):
 47 | 		self.start_goal = None
 48 | 		self.end_goal = None
 49 | 		self.route = []
 50 | 		self.route_index = 0
 51 | 		self.update_plan()
 52 | 
 53 | 	def goal_callback(self, goal):
 54 | 		self.route = []
 55 | 		self.route_index = 0
 56 | 		self.process_goal(goal)
 57 | 		self.update_plan()
 58 | 
 59 | 	def route_callback(self, msg):
 60 | 		rospy.loginfo("New route received.")
 61 | 
 62 | 		if len(msg.poses) == 0:
 63 | 			rospy.loginfo("Empty route, stopping.")
 64 | 			self.reset(None)
 65 | 			return
 66 | 
 67 | 		self.route = msg.poses
 68 | 		self.route_index = 0
 69 | 		self.process_goal(self.route[0])
 70 | 		self.update_plan()
 71 | 
 72 | 	def get_goals(self):
 73 | 		return self.start_goal, self.end_goal
 74 | 	
 75 | 	def goal_reached(self):
 76 | 		if len(self.route) > 0:
 77 | 			rospy.loginfo("Goal #%i reached.",self.route_index)
 78 | 			if self.route_index < len(self.route)-1:
 79 | 				self.route_index +=1
 80 | 				self.process_goal(self.route[self.route_index])
 81 | 			else:
 82 | 				rospy.loginfo("-> Route finished.")
 83 | 				self.start_goal = None
 84 | 				self.end_goal = None
 85 | 		else:
 86 | 			rospy.loginfo("Simple goal reached.")
 87 | 			self.start_goal = None
 88 | 			self.end_goal = None
 89 | 
 90 | 		self.update_plan()
 91 | 
 92 | 
 93 | 	def set_goal_pair(self, endgoal):
 94 | 		if len(self.route) == 0 or self.route_index == 0:
 95 | 			try:
 96 | 				self.start_goal = transform_to_pose(self.tf2_buffer.lookup_transform(PLANNING_FRAME, ROBOT_FRAME, rospy.Time(0)))
 97 | 				self.end_goal = endgoal
 98 | 			except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
 99 | 				rospy.logwarn("TF2 exception: %s", e)
100 | 		else:
101 | 			self.start_goal = self.end_goal
102 | 			self.end_goal = endgoal
103 | 
104 | 		vert_msg = Float32()
105 | 		vert_msg.data = self.end_goal.position.z
106 | 		self.vertical_pub.publish(vert_msg)
107 | 
108 | 		self.update_plan()
109 | 
110 | 	def process_goal(self, goal):
111 | 		if PLANNING_FRAME == goal.header.frame_id:
112 | 			rospy.loginfo("------------------")
113 | 			rospy.loginfo("Received goal in planning ("+PLANNING_FRAME+") frame.")
114 | 			rospy.loginfo("Position: X: %f, Y: %f, Z: %f", goal.pose.position.x, goal.pose.position.y, goal.pose.position.z)
115 | 			rospy.loginfo("Orientation: X: %f, Y: %f, Z: %f, W: %f", goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w)
116 | 			self.set_goal_pair(goal.pose)
117 | 		else:
118 | 			try:
119 | 				transform = self.tf2_buffer.lookup_transform(PLANNING_FRAME, goal.header.frame_id, rospy.Time(0))
120 | 				goal_transformed = do_transform_pose(goal, transform)
121 | 				self.set_goal_pair(goal_transformed.pose)
122 | 				rospy.loginfo("------------------")
123 | 				rospy.loginfo("Received goal in "+goal.header.frame_id+" frame, transformed to "+PLANNING_FRAME+".")
124 | 				rospy.loginfo("Position: X: %f, Y: %f, Z: %f", goal_transformed.pose.position.x, goal_transformed.pose.position.y, goal_transformed.pose.position.z)
125 | 				rospy.loginfo("Orientation: X: %f, Y: %f, Z: %f, W: %f", goal_transformed.pose.orientation.x, goal_transformed.pose.orientation.y, goal_transformed.pose.orientation.z, goal_transformed.pose.orientation.w)
126 | 
127 | 			except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
128 | 				rospy.logwarn("TF2 exception: %s", e)
129 | 				return
130 | 
131 | class LineFollowingController:
132 | 	def __init__(self):
133 | 		rospy.init_node("line_following_controller")
134 | 		global ROBOT_FRAME, PLANNING_FRAME
135 | 
136 | 		ROBOT_FRAME = rospy.get_param('~robot_frame', 'base_link')
137 | 		PLANNING_FRAME = rospy.get_param('~planning_frame', 'map')
138 | 		
139 | 		self.MIN_GOAL_XY_DIST = rospy.get_param('~xy_distance_threshold', 0.5)
140 | 		self.MIN_GOAL_Z_DIST = rospy.get_param('~z_distance_threshold', 0.5)
141 | 
142 | 		self.MAX_LINEAR_SPD = rospy.get_param('~max_linear_speed', 0.45)
143 | 		self.MAX_VERTICAL_SPD = rospy.get_param('~max_vertical_speed', 0.5)
144 | 		self.MAX_ANGULAR_SPD = rospy.get_param('~max_turning_speed', 0.9)
145 | 
146 | 		self.LINE_DIVERGENCE = rospy.get_param('~max_line_divergence', 1.0)
147 | 		self.MIN_PROJECT_DIST = rospy.get_param('~min_project_dist', 0.15)
148 | 		self.MAX_PROJECT_DIST = rospy.get_param('~max_project_dist', 1.2)
149 | 
150 | 		self.SIDE_OFFSET_MULT = rospy.get_param('~side_offset_mult', 0.5)
151 | 
152 | 		self.IGNORE_ALTITUDE = rospy.get_param('~ignore_altitude', False)
153 | 
154 | 		self.DEBUG_MARKERS = rospy.get_param('~publish_debug_markers', True)
155 | 		self.markers = DebugMarkers(PLANNING_FRAME)
156 | 
157 | 		self.tf_listener = tf.TransformListener()
158 | 
159 | 		self.tf2_buffer = tf2_ros.Buffer()
160 | 		self.tf2_listener = tf2_ros.TransformListener(self.tf2_buffer)
161 | 
162 | 		self.cmd_vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
163 | 
164 | 		self.status_pub = rospy.Publisher("line_planner/active", Bool, queue_size=1, latch=True)
165 | 		self.plan_pub = rospy.Publisher("line_planner/plan", Path, queue_size=1, latch=True)
166 | 		self.marker_pub = rospy.Publisher("line_planner/markers", MarkerArray, queue_size=1)
167 | 
168 | 		self.pid = PID(
169 | 			rospy.get_param('P', 3.0),
170 | 			rospy.get_param('I', 0.001), 
171 | 			rospy.get_param('D', 65.0)
172 | 		)
173 | 
174 | 		self.pid_vert = PID(
175 | 			rospy.get_param('P', 3.0),
176 | 			rospy.get_param('I', 0.001), 
177 | 			rospy.get_param('D', 65.0)
178 | 		)
179 | 
180 | 		self.goal_server = GoalServer(self.tf2_buffer, self.update_plan)
181 | 		self.active = False
182 | 
183 | 		self.reconfigure_server = DynamicReconfigureServer(LinePlannerConfig, self.dynamic_reconfigure_callback)
184 | 
185 | 		self.marker_publish_skip = 0
186 | 		self.status_pub.publish(False)
187 | 
188 | 		rospy.loginfo("Line planner started.")
189 | 		rospy.loginfo("Robot frame: "+ROBOT_FRAME)
190 | 		rospy.loginfo("Planning frame: "+PLANNING_FRAME)
191 | 
192 | 	def dynamic_reconfigure_callback(self, config, level):
193 | 		self.pid.kp = config.P
194 | 		self.pid.ki = config.I
195 | 		self.pid.kd = config.D
196 | 
197 | 		self.pid_vert.kp = config.P
198 | 		self.pid_vert.ki = config.I
199 | 		self.pid_vert.kd = config.D
200 | 
201 | 		self.MIN_GOAL_XY_DIST = config.xy_distance_threshold
202 | 		self.MIN_GOAL_Z_DIST = config.z_distance_threshold
203 | 
204 | 		self.MAX_LINEAR_SPD = config.max_linear_speed
205 | 		self.MAX_ANGULAR_SPD = config.max_turning_speed
206 | 		self.MAX_VERTICAL_SPD = config.max_vertical_speed
207 | 
208 | 		self.LINE_DIVERGENCE = config.max_line_divergence
209 | 		self.MIN_PROJECT_DIST = config.min_project_dist
210 | 		self.MAX_PROJECT_DIST = config.max_project_dist
211 | 
212 | 		self.SIDE_OFFSET_MULT = config.side_offset_mult
213 | 
214 | 		self.DEBUG_MARKERS = config.publish_debug_markers
215 | 		self.IGNORE_ALTITUDE = config.ignore_altitude
216 | 
217 | 		return config
218 | 
219 | 	def get_angle_error(self, current_pose, target_position):
220 | 		_, _, current_yaw = euler_from_quaternion([
221 | 			current_pose.orientation.x,
222 | 			current_pose.orientation.y,
223 | 			current_pose.orientation.z,
224 | 			current_pose.orientation.w,
225 | 		])
226 | 
227 | 		delta_x = target_position.x - current_pose.position.x
228 | 		delta_y = target_position.y - current_pose.position.y
229 | 		target_yaw = math.atan2(delta_y, delta_x)
230 | 
231 | 		angle_error = target_yaw - current_yaw
232 | 		angle_error = -math.atan2(math.sin(angle_error), math.cos(angle_error))
233 | 
234 | 		return angle_error
235 | 
236 | 	def get_distance(self, pose, goal):
237 | 		deltax = goal.position.x - pose.position.x
238 | 		deltay = goal.position.y - pose.position.y
239 | 		return math.sqrt(deltax** 2 + deltay ** 2)
240 | 
241 | 	def get_linear_velocity(self, distance, angle_error):
242 | 		ANGLE_40_RAD = math.radians(40)
243 | 		ANGLE_60_RAD = math.radians(60)
244 | 		ANGLE_120_RAD = math.radians(120)
245 | 		ANGLE_140_RAD = math.radians(140)
246 | 
247 | 		abs_angle_error = math.fabs(angle_error)
248 | 		vel_multiplier = 0.0
249 | 
250 | 		if abs_angle_error <= ANGLE_40_RAD:
251 | 			# Full speed ahead
252 | 			vel_multiplier = 1.0
253 | 		elif abs_angle_error <= ANGLE_60_RAD:
254 | 			# Linearly decrease speed from 100% at 30 deg to 0% at 60 deg.
255 | 			scale = (abs_angle_error - ANGLE_40_RAD) / (ANGLE_60_RAD - ANGLE_40_RAD)
256 | 			vel_multiplier = 1.0 - scale
257 | 		elif abs_angle_error <= ANGLE_120_RAD:
258 | 			# Turn in place between 60 and 120 deg
259 | 			vel_multiplier = 0.0
260 | 		elif abs_angle_error <= ANGLE_140_RAD:
261 | 			# Reverse from 0% at 120 deg to -100% at 180 deg for J turn behaviour.
262 | 			scale = (abs_angle_error - ANGLE_120_RAD) / (ANGLE_140_RAD - ANGLE_120_RAD)
263 | 			vel_multiplier = -scale
264 | 
265 | 		linear_vel = self.MAX_LINEAR_SPD * vel_multiplier
266 | 
267 | 		# Slow down while waiting for depth to match
268 | 		if distance < self.MIN_GOAL_XY_DIST:
269 | 			linear_vel *= 0.1
270 | 
271 | 		return max(-self.MAX_LINEAR_SPD, min(linear_vel, self.MAX_LINEAR_SPD))
272 | 
273 | 
274 | 	def update(self):
275 | 		start_goal, end_goal = self.goal_server.get_goals()
276 | 
277 | 		if start_goal == None or end_goal == None:
278 | 			if self.active:
279 | 				self.reset()
280 | 			return
281 | 		
282 | 		try:
283 | 			self.active = True
284 | 			self.status_pub.publish(True)
285 | 			pose = transform_to_pose(self.tf2_buffer.lookup_transform(PLANNING_FRAME, ROBOT_FRAME, rospy.Time(0)))
286 | 
287 | 			target_position = project_position(
288 | 				start_goal,
289 | 				end_goal,
290 | 				pose,
291 | 				self.MIN_PROJECT_DIST,
292 | 				self.MAX_PROJECT_DIST,
293 | 				self.LINE_DIVERGENCE,
294 | 				self.SIDE_OFFSET_MULT
295 | 			)
296 | 			
297 | 			angle_error = self.get_angle_error(pose, target_position)
298 | 			angular_velocity = clamp(self.pid.compute(angle_error), -self.MAX_ANGULAR_SPD, self.MAX_ANGULAR_SPD)
299 | 			target_distance = self.get_distance(end_goal, pose)
300 | 			linear_velocity = self.get_linear_velocity(target_distance, angle_error)
301 | 
302 | 			vertical_error = pose.position.z - end_goal.position.z
303 | 			vertical_velocity = clamp(self.pid_vert.compute(vertical_error), -self.MAX_VERTICAL_SPD, self.MAX_VERTICAL_SPD)
304 | 				
305 | 			if target_distance > self.MIN_GOAL_XY_DIST:
306 | 				pass
307 | 			elif math.fabs(vertical_error) > self.MIN_GOAL_Z_DIST and not self.IGNORE_ALTITUDE:
308 | 				angular_velocity = 0
309 | 			else:
310 | 				linear_velocity = 0
311 | 				angular_velocity = 0
312 | 				vertical_velocity = 0
313 | 				self.goal_server.goal_reached()
314 | 
315 | 			self.send_twist(linear_velocity, angular_velocity, vertical_velocity)
316 | 
317 | 			if self.DEBUG_MARKERS:
318 | 				self.markers.draw_debug_markers(target_position, start_goal, end_goal, self.MIN_GOAL_XY_DIST, self.LINE_DIVERGENCE)
319 | 
320 | 		except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
321 | 			rospy.logwarn("TF Exception")
322 | 
323 | 	def update_plan(self):
324 | 
325 | 		msg = Path()
326 | 		msg.header.frame_id = PLANNING_FRAME
327 | 
328 | 		if len(self.goal_server.route) > 1:
329 | 			msg.poses = self.goal_server.route[self.goal_server.route_index:]
330 | 		else:
331 | 			start_goal, end_goal = self.goal_server.get_goals()
332 | 
333 | 			if start_goal == None or end_goal == None:
334 | 				self.plan_pub.publish(Path())
335 | 				return
336 | 
337 | 			start_stamped = PoseStamped()
338 | 			start_stamped.header.frame_id = PLANNING_FRAME
339 | 			start_stamped.pose = start_goal
340 | 			msg.poses.append(start_stamped)
341 | 
342 | 			end_stamped = PoseStamped()
343 | 			end_stamped.header.frame_id = PLANNING_FRAME
344 | 			end_stamped.pose = end_goal		
345 | 
346 | 			msg.poses = [start_stamped, end_stamped]
347 | 
348 | 		self.plan_pub.publish(msg)
349 | 
350 | 	def send_twist(self, linear, angular, vert):
351 | 
352 | 		#sanity check, just in case
353 | 		if math.isnan(linear):
354 | 			linear = 0
355 | 
356 | 		if math.isnan(angular):
357 | 			angular = 0
358 | 
359 | 		if self.IGNORE_ALTITUDE or math.isnan(vert):
360 | 			vert = 0
361 | 
362 | 		twist = Twist()
363 | 		twist.linear.x = linear
364 | 		twist.linear.z = vert
365 | 		twist.angular.z = angular
366 | 		self.cmd_vel_pub.publish(twist)
367 | 
368 | 	def reset(self):
369 | 		self.send_twist(0, 0, 0)
370 | 
371 | 		if self.DEBUG_MARKERS:
372 | 			self.markers.delete_debug_markers()
373 | 
374 | 		self.active = False
375 | 		self.status_pub.publish(False)
376 | 		self.pid.reset()
377 | 		self.pid_vert.reset()
378 | 
379 | ctrl = LineFollowingController()
380 | rate = rospy.Rate(rospy.get_param('rate', 30))
381 | rospy.on_shutdown(ctrl.reset)
382 | 
383 | while not rospy.is_shutdown():
384 | 	ctrl.update()
385 | 	rate.sleep()
386 | 


--------------------------------------------------------------------------------
/docs/diagram.drawio:
--------------------------------------------------------------------------------
  1 | <mxfile host="app.diagrams.net" modified="2023-05-13T21:18:30.168Z" agent="Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/113.0.0.0 Safari/537.36" etag="PT1LFCb5s4VlPJGZHvMB" version="21.3.0" type="device">
  2 |   <diagram name="Page-1" id="T1XgxFX7Yl2GAL7NN9PY">
  3 |     <mxGraphModel dx="819" dy="392" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="827" pageHeight="1169" math="0" shadow="0">
  4 |       <root>
  5 |         <mxCell id="0" />
  6 |         <mxCell id="1" parent="0" />
  7 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-46" value="&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; text-align: start; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;font style=&quot;&quot; face=&quot;ui-monospace, SFMono-Regular, SF Mono, Menlo, Consolas, Liberation Mono, monospace&quot;&gt;&lt;font style=&quot;--darkreader-inline-color: #d8d4cf;&quot; data-darkreader-inline-color=&quot;&quot;&gt;max_project_dist&lt;/font&gt;&lt;font style=&quot;--darkreader-inline-color: #c8c4bd;&quot; data-darkreader-inline-color=&quot;&quot;&gt;&lt;br&gt;&lt;/font&gt;&lt;/font&gt;&lt;/pre&gt;&lt;/pre&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=9;fontColor=#000000;" vertex="1" parent="1">
  8 |           <mxGeometry x="290" y="453" width="110" height="10" as="geometry" />
  9 |         </mxCell>
 10 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-23" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=none;strokeColor=#6c8ebf;gradientColor=#7ea6e0;" vertex="1" parent="1">
 11 |           <mxGeometry x="60" y="240" width="120" height="120" as="geometry" />
 12 |         </mxCell>
 13 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-2" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#dae8fc;strokeColor=#6c8ebf;gradientColor=#7ea6e0;" vertex="1" parent="1">
 14 |           <mxGeometry x="110" y="290" width="20" height="20" as="geometry" />
 15 |         </mxCell>
 16 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-3" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;" vertex="1" parent="1">
 17 |           <mxGeometry x="580" y="290" width="20" height="20" as="geometry" />
 18 |         </mxCell>
 19 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-4" value="" style="endArrow=none;html=1;rounded=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;" edge="1" parent="1" source="sKgzW7D7Zy8gm6ZvRh1t-2" target="sKgzW7D7Zy8gm6ZvRh1t-3">
 20 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
 21 |             <mxPoint x="400" y="370" as="sourcePoint" />
 22 |             <mxPoint x="450" y="320" as="targetPoint" />
 23 |           </mxGeometry>
 24 |         </mxCell>
 25 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-16" value="" style="triangle;whiteSpace=wrap;html=1;rotation=-139;" vertex="1" parent="1">
 26 |           <mxGeometry x="445" y="358" width="20" height="20" as="geometry" />
 27 |         </mxCell>
 28 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-17" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#d5e8d4;strokeColor=#82b366;" vertex="1" parent="1">
 29 |           <mxGeometry x="300" y="250" width="20" height="20" as="geometry" />
 30 |         </mxCell>
 31 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-18" value="Starting point" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=10;" vertex="1" parent="1">
 32 |           <mxGeometry x="552.5" y="267" width="75" height="23" as="geometry" />
 33 |         </mxCell>
 34 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-19" value="Goal" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=10;" vertex="1" parent="1">
 35 |           <mxGeometry x="102.5" y="270" width="35" height="21" as="geometry" />
 36 |         </mxCell>
 37 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-20" value="Temporary goal" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=10;" vertex="1" parent="1">
 38 |           <mxGeometry x="270" y="230" width="80" height="20" as="geometry" />
 39 |         </mxCell>
 40 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-21" value="" style="endArrow=none;dashed=1;html=1;rounded=0;fillColor=#e6d0de;gradientColor=#d5739d;strokeColor=#996185;" edge="1" parent="1">
 41 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
 42 |             <mxPoint x="310" y="260" as="sourcePoint" />
 43 |             <mxPoint x="309" y="369" as="targetPoint" />
 44 |           </mxGeometry>
 45 |         </mxCell>
 46 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-22" value="" style="endArrow=none;dashed=1;html=1;rounded=0;fillColor=#dae8fc;gradientColor=#7ea6e0;strokeColor=#6c8ebf;" edge="1" parent="1">
 47 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
 48 |             <mxPoint x="457.71" y="300" as="sourcePoint" />
 49 |             <mxPoint x="458" y="372" as="targetPoint" />
 50 |           </mxGeometry>
 51 |         </mxCell>
 52 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-26" value="&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; text-align: start; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;font style=&quot;&quot; face=&quot;ui-monospace, SFMono-Regular, SF Mono, Menlo, Consolas, Liberation Mono, monospace&quot;&gt;&lt;font style=&quot;--darkreader-inline-color: #d8d4cf;&quot; data-darkreader-inline-color=&quot;&quot;&gt;side_offset_mult&lt;/font&gt;&lt;font style=&quot;--darkreader-inline-color: #c8c4bd;&quot; data-darkreader-inline-color=&quot;&quot;&gt;&lt;br&gt;&lt;/font&gt;&lt;/font&gt;&lt;/pre&gt;&lt;/pre&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=9;fontColor=#000000;" vertex="1" parent="1">
 53 |           <mxGeometry x="220" y="280" width="90" height="10" as="geometry" />
 54 |         </mxCell>
 55 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-28" value="" style="endArrow=none;dashed=1;html=1;rounded=0;fillColor=#dae8fc;gradientColor=#7ea6e0;strokeColor=#6c8ebf;" edge="1" parent="1">
 56 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
 57 |             <mxPoint x="310" y="370" as="sourcePoint" />
 58 |             <mxPoint x="460" y="370" as="targetPoint" />
 59 |           </mxGeometry>
 60 |         </mxCell>
 61 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-31" value="" style="endArrow=classic;html=1;rounded=0;fillColor=#f8cecc;strokeColor=#b85450;" edge="1" parent="1">
 62 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
 63 |             <mxPoint x="446" y="362" as="sourcePoint" />
 64 |             <mxPoint x="326" y="270" as="targetPoint" />
 65 |           </mxGeometry>
 66 |         </mxCell>
 67 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-34" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#dae8fc;strokeColor=#6c8ebf;gradientColor=#7ea6e0;" vertex="1" parent="1">
 68 |           <mxGeometry x="110" y="419" width="20" height="20" as="geometry" />
 69 |         </mxCell>
 70 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-35" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;" vertex="1" parent="1">
 71 |           <mxGeometry x="580" y="419" width="20" height="20" as="geometry" />
 72 |         </mxCell>
 73 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-36" value="" style="endArrow=none;html=1;rounded=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;" edge="1" parent="1" source="sKgzW7D7Zy8gm6ZvRh1t-37" target="sKgzW7D7Zy8gm6ZvRh1t-35">
 74 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
 75 |             <mxPoint x="400" y="499" as="sourcePoint" />
 76 |             <mxPoint x="450" y="449" as="targetPoint" />
 77 |           </mxGeometry>
 78 |         </mxCell>
 79 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-47" value="" style="endArrow=none;dashed=1;html=1;rounded=0;fillColor=#dae8fc;gradientColor=#7ea6e0;strokeColor=#6c8ebf;" edge="1" parent="1">
 80 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
 81 |             <mxPoint x="250" y="449.85" as="sourcePoint" />
 82 |             <mxPoint x="458" y="448" as="targetPoint" />
 83 |           </mxGeometry>
 84 |         </mxCell>
 85 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-51" value="" style="endArrow=none;html=1;rounded=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;" edge="1" parent="1" source="sKgzW7D7Zy8gm6ZvRh1t-34" target="sKgzW7D7Zy8gm6ZvRh1t-38">
 86 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
 87 |             <mxPoint x="130" y="429" as="sourcePoint" />
 88 |             <mxPoint x="580" y="429" as="targetPoint" />
 89 |           </mxGeometry>
 90 |         </mxCell>
 91 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-38" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#d5e8d4;strokeColor=#82b366;" vertex="1" parent="1">
 92 |           <mxGeometry x="240" y="420" width="20" height="20" as="geometry" />
 93 |         </mxCell>
 94 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-52" value="" style="endArrow=none;html=1;rounded=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;" edge="1" parent="1" source="sKgzW7D7Zy8gm6ZvRh1t-38" target="sKgzW7D7Zy8gm6ZvRh1t-37">
 95 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
 96 |             <mxPoint x="260" y="430" as="sourcePoint" />
 97 |             <mxPoint x="580" y="429" as="targetPoint" />
 98 |           </mxGeometry>
 99 |         </mxCell>
100 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-37" value="" style="triangle;whiteSpace=wrap;html=1;rotation=-180;" vertex="1" parent="1">
101 |           <mxGeometry x="445" y="419" width="20" height="20" as="geometry" />
102 |         </mxCell>
103 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-53" value="" style="endArrow=none;dashed=1;html=1;rounded=0;fillColor=#dae8fc;gradientColor=#7ea6e0;strokeColor=#6c8ebf;" edge="1" parent="1">
104 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
105 |             <mxPoint x="250" y="449.85" as="sourcePoint" />
106 |             <mxPoint x="250" y="430" as="targetPoint" />
107 |           </mxGeometry>
108 |         </mxCell>
109 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-54" value="" style="endArrow=none;dashed=1;html=1;rounded=0;fillColor=#dae8fc;gradientColor=#7ea6e0;strokeColor=#6c8ebf;" edge="1" parent="1">
110 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
111 |             <mxPoint x="458" y="447.85" as="sourcePoint" />
112 |             <mxPoint x="458" y="428" as="targetPoint" />
113 |           </mxGeometry>
114 |         </mxCell>
115 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-49" value="" style="endArrow=classic;html=1;rounded=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;fillColor=#f8cecc;strokeColor=#b85450;" edge="1" parent="1">
116 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
117 |             <mxPoint x="445" y="429" as="sourcePoint" />
118 |             <mxPoint x="358" y="429" as="targetPoint" />
119 |           </mxGeometry>
120 |         </mxCell>
121 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-58" value="Projection distance" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=10;" vertex="1" parent="1">
122 |           <mxGeometry x="334" y="367.5" width="110" height="23" as="geometry" />
123 |         </mxCell>
124 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-59" value="&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; text-align: start; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;font style=&quot;&quot; face=&quot;ui-monospace, SFMono-Regular, SF Mono, Menlo, Consolas, Liberation Mono, monospace&quot;&gt;&lt;font style=&quot;--darkreader-inline-color: #d8d4cf;&quot; data-darkreader-inline-color=&quot;&quot;&gt;min_project_dist&lt;/font&gt;&lt;font style=&quot;--darkreader-inline-color: #c8c4bd;&quot; data-darkreader-inline-color=&quot;&quot;&gt;&lt;br&gt;&lt;/font&gt;&lt;/font&gt;&lt;/pre&gt;&lt;/pre&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=9;fontColor=#000000;" vertex="1" parent="1">
125 |           <mxGeometry x="351" y="500" width="110" height="10" as="geometry" />
126 |         </mxCell>
127 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-61" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#dae8fc;strokeColor=#6c8ebf;gradientColor=#7ea6e0;" vertex="1" parent="1">
128 |           <mxGeometry x="110" y="513" width="20" height="20" as="geometry" />
129 |         </mxCell>
130 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-62" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;" vertex="1" parent="1">
131 |           <mxGeometry x="580" y="513" width="20" height="20" as="geometry" />
132 |         </mxCell>
133 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-68" value="" style="endArrow=none;html=1;rounded=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;" edge="1" parent="1" source="sKgzW7D7Zy8gm6ZvRh1t-61" target="sKgzW7D7Zy8gm6ZvRh1t-62">
134 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
135 |             <mxPoint x="130" y="523" as="sourcePoint" />
136 |             <mxPoint x="580" y="523" as="targetPoint" />
137 |           </mxGeometry>
138 |         </mxCell>
139 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-69" value="" style="ellipse;whiteSpace=wrap;html=1;aspect=fixed;fillColor=#d5e8d4;strokeColor=#82b366;" vertex="1" parent="1">
140 |           <mxGeometry x="387" y="514" width="20" height="20" as="geometry" />
141 |         </mxCell>
142 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-71" value="" style="triangle;whiteSpace=wrap;html=1;rotation=-99;" vertex="1" parent="1">
143 |           <mxGeometry x="404" y="638" width="20" height="20" as="geometry" />
144 |         </mxCell>
145 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-74" value="" style="endArrow=classic;html=1;rounded=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;fillColor=#f8cecc;strokeColor=#b85450;" edge="1" parent="1">
146 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
147 |             <mxPoint x="412" y="640" as="sourcePoint" />
148 |             <mxPoint x="402" y="560" as="targetPoint" />
149 |           </mxGeometry>
150 |         </mxCell>
151 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-78" value="" style="endArrow=none;dashed=1;html=1;rounded=0;fillColor=#dae8fc;gradientColor=#7ea6e0;strokeColor=#6c8ebf;" edge="1" parent="1">
152 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
153 |             <mxPoint x="398.5" y="648.9200000000001" as="sourcePoint" />
154 |             <mxPoint x="411" y="649" as="targetPoint" />
155 |           </mxGeometry>
156 |         </mxCell>
157 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-79" value="" style="endArrow=none;dashed=1;html=1;rounded=0;fillColor=#dae8fc;gradientColor=#7ea6e0;strokeColor=#6c8ebf;" edge="1" parent="1">
158 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
159 |             <mxPoint x="397" y="524" as="sourcePoint" />
160 |             <mxPoint x="398" y="648" as="targetPoint" />
161 |           </mxGeometry>
162 |         </mxCell>
163 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-80" value="&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; text-align: start; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;font face=&quot;ui-monospace, SFMono-Regular, SF Mono, Menlo, Consolas, Liberation Mono, monospace&quot;&gt;max_line_divergence&lt;/font&gt;&lt;font style=&quot;--darkreader-inline-color: #c8c4bd;&quot; data-darkreader-inline-color=&quot;&quot; face=&quot;ui-monospace, SFMono-Regular, SF Mono, Menlo, Consolas, Liberation Mono, monospace&quot; color=&quot;#d2cec8&quot;&gt;&lt;span style=&quot;font-size: 13.6px;&quot;&gt;&lt;br&gt;&lt;/span&gt;&lt;/font&gt;&lt;/pre&gt;&lt;/pre&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=9;" vertex="1" parent="1">
164 |           <mxGeometry x="294" y="578" width="100" height="10" as="geometry" />
165 |         </mxCell>
166 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-81" value="" style="endArrow=none;dashed=1;html=1;rounded=0;fillColor=#dae8fc;gradientColor=#7ea6e0;strokeColor=#6c8ebf;" edge="1" parent="1">
167 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
168 |             <mxPoint x="411" y="525" as="sourcePoint" />
169 |             <mxPoint x="412" y="649" as="targetPoint" />
170 |           </mxGeometry>
171 |         </mxCell>
172 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-82" value="" style="endArrow=none;dashed=1;html=1;rounded=0;fillColor=#dae8fc;gradientColor=#7ea6e0;strokeColor=#6c8ebf;" edge="1" parent="1">
173 |           <mxGeometry width="50" height="50" relative="1" as="geometry">
174 |             <mxPoint x="398.5" y="523.9200000000001" as="sourcePoint" />
175 |             <mxPoint x="411" y="524" as="targetPoint" />
176 |           </mxGeometry>
177 |         </mxCell>
178 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-83" value="Side offset" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=10;" vertex="1" parent="1">
179 |           <mxGeometry x="457" y="320" width="59" height="23" as="geometry" />
180 |         </mxCell>
181 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-84" value="&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; text-align: start; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;font style=&quot;&quot; face=&quot;ui-monospace, SFMono-Regular, SF Mono, Menlo, Consolas, Liberation Mono, monospace&quot;&gt;&lt;font style=&quot;--darkreader-inline-color: #c8c4bd;&quot; data-darkreader-inline-color=&quot;&quot;&gt;goal_distance_threshold&lt;br&gt;&lt;/font&gt;&lt;/font&gt;&lt;/pre&gt;&lt;/pre&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=9;fontColor=#000000;" vertex="1" parent="1">
182 |           <mxGeometry x="60" y="363" width="125" height="10" as="geometry" />
183 |         </mxCell>
184 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-87" value="&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; text-align: start; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;pre data-darkreader-inline-bgcolor=&quot;&quot; style=&quot;box-sizing: border-box; margin-top: 0px; margin-bottom: 0px; overflow-wrap: normal; padding: 16px; overflow: auto; line-height: 1.45; background-color: var(--darkreader-bg--color-canvas-subtle); border-radius: 6px; word-break: normal; --darkreader-inline-bgcolor: var(--darkreader-bg--darkreader-bg--color-canvas-subtle);&quot;&gt;&lt;font style=&quot;&quot; face=&quot;ui-monospace, SFMono-Regular, SF Mono, Menlo, Consolas, Liberation Mono, monospace&quot;&gt;&lt;font style=&quot;--darkreader-inline-color: #c8c4bd;&quot; data-darkreader-inline-color=&quot;&quot;&gt;robot_frame&lt;br&gt;&lt;/font&gt;&lt;/font&gt;&lt;/pre&gt;&lt;/pre&gt;" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=9;fontColor=#000000;" vertex="1" parent="1">
185 |           <mxGeometry x="465" y="374" width="57.5" height="10" as="geometry" />
186 |         </mxCell>
187 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-88" value="In-between state:" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=10;fontStyle=1" vertex="1" parent="1">
188 |           <mxGeometry x="60" y="200" width="90" height="20" as="geometry" />
189 |         </mxCell>
190 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-89" value="Colinear state:" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=10;fontStyle=1" vertex="1" parent="1">
191 |           <mxGeometry x="60" y="390.5" width="72.5" height="20" as="geometry" />
192 |         </mxCell>
193 |         <mxCell id="sKgzW7D7Zy8gm6ZvRh1t-90" value="Extreme correction state:" style="text;html=1;strokeColor=none;fillColor=none;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;fontSize=10;fontStyle=1" vertex="1" parent="1">
194 |           <mxGeometry x="60" y="480" width="122.5" height="20" as="geometry" />
195 |         </mxCell>
196 |       </root>
197 |     </mxGraphModel>
198 |   </diagram>
199 | </mxfile>
200 | 


--------------------------------------------------------------------------------