├── .gitignore ├── README.md ├── launch └── bringup.launch.py ├── media ├── bfs.webm ├── bfs_b.gif └── playground.png ├── package.xml ├── pathplanners ├── __init__.py ├── breadthfirstsearch.py ├── dijkstra.py └── map.py ├── resource └── pathplanners ├── rviz └── visualizer.rviz ├── setup.cfg ├── setup.py └── test ├── test_copyright.py ├── test_flake8.py └── test_pep257.py /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode 2 | pathplanners/__pycache__/ -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Path-Planning-Algorithms-with-ROS2 2 | 3 |

4 |

Table of Contents

5 |

About
Description of Algorithms
Using this Project

10 |

11 | 12 | 13 | **ToDo:** 14 | 15 | - Potential function for the complex algo 16 | - Explore neighbors 17 | - Takes a node and returns all its neighbor 18 | - Should this be encapsulated in base Algorithm class? 19 | 20 | 22 | 23 | 24 | 25 | ## About 26 | 27 | ### Design aspects of system 28 | 29 | Preliminary data such as map with obstacle, initial and goal position, common to all algorithms will be provided with node - `map_node` 30 | 31 | Each algorithms can be executed using its own python node. 32 | 33 | Complex algorithms which requires functionalities present in simpler can be derived easily as the codebase it OOP based. 34 | 35 |

36 | PlayGround 37 |

38 |

39 | 40 |

106 | Breadth First Search 107 |

108 | 109 |

110 | 111 |

112 | 113 | 114 | ```python 115 | Explores the neighbor nodes first, before moving to the next level of neighbors. 116 | Utilizes queue to store the node yet to be visited. 117 | ``` 118 |

119 | Depth First Search 120 |

121 | 122 | ```python 123 | Plunges depth first into a graph without regard for which edge it take next 124 | until it cannot go further at which point it backtracks and continues. 125 | ``` 126 | 127 |

128 | Minimal Spanning Tree 129 |

130 | 131 | 132 | 133 | ### Grid-based search algorithms 134 | - dijkstra 135 | - greedy 136 | - A* 137 | - D* 138 | - Uniform Cost Search 139 | - Best-First Searching 140 | - A* 141 | - Bidirectional A* 142 | - Anytime Repairing A* 143 | - Learning Real-time A* (LRTA*) 144 | - Real-time Adaptive A* (RTAA*) 145 | - Lifelong Planning A* (LPA*) 146 | - Dynamic A* (D*) 147 | - D* Lite 148 | - Anytime D* 149 | - Dynamic Window Approach. 150 | 151 | ### Sampling-based search algorithms 152 | - Rapidly-Exploring Random Trees RRT 153 | - RRT* 154 | - Probabilistic Roadmap (PRM) 155 | - RRT 156 | - RRT-Connect 157 | - Extended-RRT 158 | - Dynamic-RRT 159 | - RRT* 160 | - Informed RRT* 161 | - RRT* Smart 162 | - Anytime RRT* 163 | - Closed-Loop RRT* 164 | - Spline-RRT* 165 | - Fast Marching Trees (FMT*) 166 | - Batch Informed Trees (BIT*) 167 | 168 | ### Potential Field Algorithms 169 | 170 | - Artificial Potential Field (APF) 171 | 172 | ### Cell Decomposition Algorithms: 173 | 174 | - Voronoi Diagrams 175 | - Visibility Graphs: 176 | 177 | ## Optimal Control Algorithms: 178 | 179 | - Trajectory Optimization 180 | - Model Predictive Control (MPC) 181 | 182 | ### Search-Based Algorithms: 183 | 184 | -Probabilistic Graph Search 185 | - Anytime Algorithm 186 | 187 | ### Sampling-Based Hybrid Methods: 188 | 189 | - PRM* (Probabilistic Roadmap Star) 190 | - Informed RRT* 191 | 192 | ## Using this Project 193 | 194 | Move into your workspace's src folder 195 | ``` 196 | cd ~/ros2_ws/src 197 | ``` 198 | Clone the project 199 | ``` 200 | git clone 201 | ``` 202 | Build the project. 203 | ``` 204 | cd ~/ros2_ws && colcon build 205 | ``` 206 | 207 | Launch rviz2 and map_node using bringup launch file 208 | ``` 209 | ros2 launch pathplanners bringup.launch.py 210 | ``` 211 | 212 | To see individual algorithms in action, run individual scripts. 213 | ``` 214 | ros2 run pathplanners 215 | ``` 216 | 217 | Executable list 218 | 219 | ```python 220 | - bfs 221 | ``` 222 | Replace these with to run the specific algorithm 223 | 224 | 225 | ## SiteMap 226 | 227 | ```python 228 | ├── launch 229 | │ └── bringup.launch.py 230 | │ 231 | ├── media 232 | │ ├── bfs.gif 233 | │ ├── bfs.webm 234 | │ └── playground.png 235 | │ 236 | ├── pathplanners 237 | │ ├── breadthfirstsearch.py 238 | │ ├── __init__.py 239 | │ ├── map.py 240 | │ 241 | ├── rviz 242 | │ └── visualizer.rviz 243 | │ 244 | ├── README.md 245 | │ 246 | ├── package.xml 247 | ├── setup.cfg 248 | └── setup.py 249 | 250 | ``` -------------------------------------------------------------------------------- /launch/bringup.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python.packages import get_package_share_directory 3 | from launch import LaunchDescription 4 | from launch_ros.actions import Node 5 | 6 | 7 | def generate_launch_description(): 8 | 9 | rviz_config = os.path.join(get_package_share_directory('pathplanners'), 10 | 'rviz', 'visualizer.rviz') 11 | 12 | rviz2_launcher = Node( 13 | package='rviz2', 14 | executable='rviz2', 15 | name='rviz2', 16 | output='screen', 17 | arguments=['-d', rviz_config] 18 | ) 19 | 20 | ref_map_launcher = Node( 21 | package='pathplanners', 22 | executable='map_node', 23 | ) 24 | 25 | algo_map_launcher = Node( 26 | package='pathplanners', 27 | executable='bfs', 28 | ) 29 | 30 | static_transform_publisher = Node( 31 | package='tf2_ros', 32 | executable='static_transform_publisher', 33 | name='static_transform_publisher', 34 | namespace='', 35 | output='screen', 36 | arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'map', 'algo_map'] 37 | ) 38 | ld = LaunchDescription() 39 | 40 | ld.add_action(rviz2_launcher) 41 | ld.add_action(ref_map_launcher) 42 | ld.add_action(static_transform_publisher) 43 | # ld.add_action(algo_map_launcher) 44 | 45 | 46 | return ld -------------------------------------------------------------------------------- /media/bfs.webm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maker-ATOM/Path-Planning-Algorithms/d4353017ead9b72df8c05ceb5a2bb6b877069e6f/media/bfs.webm -------------------------------------------------------------------------------- /media/bfs_b.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maker-ATOM/Path-Planning-Algorithms/d4353017ead9b72df8c05ceb5a2bb6b877069e6f/media/bfs_b.gif -------------------------------------------------------------------------------- /media/playground.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maker-ATOM/Path-Planning-Algorithms/d4353017ead9b72df8c05ceb5a2bb6b877069e6f/media/playground.png -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | pathplanners 5 | 0.0.0 6 | TODO: Package description 7 | aditya 8 | TODO: License declaration 9 | 10 | rclpy 11 | 12 | ament_copyright 13 | ament_flake8 14 | ament_pep257 15 | python3-pytest 16 | 17 | 18 | ament_python 19 | 20 | 21 | -------------------------------------------------------------------------------- /pathplanners/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maker-ATOM/Path-Planning-Algorithms/d4353017ead9b72df8c05ceb5a2bb6b877069e6f/pathplanners/__init__.py -------------------------------------------------------------------------------- /pathplanners/breadthfirstsearch.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | from collections import deque 4 | 5 | import rclpy 6 | from rclpy.node import Node 7 | 8 | from nav_msgs.msg import OccupancyGrid 9 | from custom_interfaces.msg import AdjacencyList, Neighbors, Point 10 | from std_msgs.msg import Empty 11 | 12 | from nav_msgs.srv import GetMap 13 | 14 | 15 | class BFS(Node): 16 | 17 | def __init__(self): 18 | 19 | # constants 20 | self.FREE_SPACE = -1 21 | self.START = -2 22 | self.GOAL = 1 23 | self.TRAVERSAL_COLOR_INCREMENT = 0.104 24 | self.FINAL_COLOR = -127 25 | self.TRAVERSAL_COLOR_UPDATE = 98 26 | 27 | # Flag instead of sync service call 28 | self.received_data = False 29 | 30 | # transmitted on topic 31 | self.map_data = GetMap.Response() 32 | 33 | # start and end points 34 | self.initial_point = Point() 35 | self.goal_point = Point() 36 | 37 | # map 38 | self.map = [] 39 | self.parent_map = [] 40 | 41 | self.queue = [] 42 | 43 | self.color = self.START 44 | 45 | self.goal_reached = False 46 | 47 | # Node setup 48 | super().__init__('bfs_traversal') 49 | self.traversal_pub = self.create_publisher(OccupancyGrid, 'algo_viz', 10) 50 | self.get_map_service = self.create_client(GetMap, 'get_map') 51 | 52 | # timer to call master_callback repetitively 53 | self.timer_period = 0.005 # seconds 54 | self.timer = self.create_timer(self.timer_period, self.master_callback) 55 | 56 | # Wait until service is available 57 | while not self.get_map_service.wait_for_service(timeout_sec=1.0): 58 | self.get_logger().warn('Service not available, waiting again...') 59 | 60 | # Request for initial goal pose 61 | request = GetMap.Request() 62 | self.get_map_future = self.get_map_service.call_async(request) 63 | self.get_map_future.add_done_callback(self.get_map_callback) 64 | 65 | def get_map_callback(self, future): 66 | self.received_data = True 67 | 68 | self.map_data = future.result() 69 | self.map_data.map.header.frame_id = 'algo_map' 70 | 71 | # extract height and width of map 72 | height = self.map_data.map.info.height 73 | width = self.map_data.map.info.width 74 | 75 | map_list = list(self.map_data.map.data) 76 | 77 | # generate the 2D array 78 | 79 | for j in range(0, width): 80 | row = [] 81 | for i in range(0, height): 82 | row.append(map_list[width*j + i]) 83 | if row[-1] == self.GOAL: 84 | self.goal_point.x = i 85 | self.goal_point.y = j 86 | if row[-1] == self.START: 87 | self.initial_point.x = i 88 | self.initial_point.y = j 89 | self.map.append(row) 90 | 91 | # initial_point to queue 92 | self.queue = deque([self.initial_point]) 93 | 94 | # set parent map 95 | self.parent_map = [[-1] * height for _ in range(width)] 96 | 97 | # required to generate the map 98 | self.path_node = self.goal_point 99 | 100 | self.get_logger().info("Received map data") 101 | self.get_logger().info(f"Initial pose => x: {self.initial_point.x}, y: {self.initial_point.y}") 102 | self.get_logger().info(f"Goal pose => x: {self.goal_point.x}, y: {self.goal_point.y}") 103 | 104 | def master_callback(self): 105 | 106 | # direction vector to generate neighbors 107 | dx = [0, 1, 0, -1] 108 | dy = [1, 0, -1, 0] 109 | 110 | if self.received_data: 111 | # do traversal 112 | 113 | if len(self.queue) > 0: 114 | # get recent node and mark it as visited 115 | node = self.queue.popleft() 116 | # self.map[node.y][node.x] = int(self.color) 117 | 118 | for i in range(0, 4): 119 | # generate new neighbors 120 | neighbor = Point() 121 | neighbor.x = node.x + dx[i] 122 | neighbor.y = node.y + dy[i] 123 | 124 | # neighbor is traversable free space 125 | if self.map[neighbor.y][neighbor.x] == self.FREE_SPACE: 126 | 127 | # add to queue 128 | self.queue.append(neighbor) 129 | # update the map indicating neighbor visited 130 | self.map[neighbor.y][neighbor.x] = int(self.color) 131 | 132 | # parent of neighbor is node 133 | self.parent_map[neighbor.y][neighbor.x] = node 134 | 135 | # if any of the neighbor is goal pose 136 | elif self.map[neighbor.y][neighbor.x] == self.GOAL: 137 | self.received_data = False 138 | self.goal_reached = True 139 | 140 | # parent of neighbor is node 141 | self.parent_map[neighbor.y][neighbor.x] = node 142 | 143 | 144 | self.color -= self.TRAVERSAL_COLOR_INCREMENT 145 | # uneven transition 146 | if self.color < self.FINAL_COLOR: 147 | self.color = self.TRAVERSAL_COLOR_UPDATE 148 | 149 | elif self.goal_reached: 150 | self.get_logger().info("Goal Reached",once=True) 151 | 152 | try: 153 | # get parent node to generate path 154 | path_element = self.parent_map[self.path_node.y][self.path_node.x] 155 | except: 156 | self.goal_reached = False 157 | self.get_logger().info("Path Generated") 158 | 159 | # program should not exit but else does it has to do? 160 | exit() 161 | 162 | # update map with path 163 | self.map[self.path_node.y][self.path_node.x] = 101 164 | 165 | # node is path-element 166 | self.path_node = path_element 167 | 168 | 169 | self.map_data.map.data = [] 170 | 171 | # convert 2D array to single list 172 | for list in self.map: 173 | self.map_data.map.data.extend(list) 174 | 175 | # publish data 176 | self.traversal_pub.publish(self.map_data.map) 177 | 178 | def main(args=None): 179 | rclpy.init(args=args) 180 | 181 | bfs_node = BFS() 182 | 183 | rclpy.spin(bfs_node) 184 | 185 | bfs_node.destroy_node() 186 | rclpy.shutdown() 187 | 188 | 189 | if __name__ == "__main__": 190 | main() -------------------------------------------------------------------------------- /pathplanners/dijkstra.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | from collections import deque 4 | 5 | import rclpy 6 | from rclpy.node import Node 7 | 8 | from nav_msgs.msg import OccupancyGrid 9 | from custom_interfaces.msg import AdjacencyList, Neighbors, Point 10 | from std_msgs.msg import Empty 11 | 12 | from nav_msgs.srv import GetMap 13 | 14 | 15 | class BFS(Node): 16 | 17 | def __init__(self): 18 | 19 | # constants 20 | self.FREE_SPACE = -1 21 | self.START = -2 22 | self.GOAL = 1 23 | self.TRAVERSAL_COLOR_INCREMENT = 0.104 24 | self.FINAL_COLOR = -127 25 | self.TRAVERSAL_COLOR_UPDATE = 98 26 | 27 | # Flag instead of sync service call 28 | self.received_data = False 29 | 30 | # transmitted on topic 31 | self.map_data = GetMap.Response() 32 | 33 | # start and end points 34 | self.initial_point = Point() 35 | self.goal_point = Point() 36 | 37 | # map 38 | self.map = [] 39 | self.parent_map = [] 40 | 41 | self.queue = [] 42 | 43 | self.color = self.START 44 | 45 | self.goal_reached = False 46 | 47 | # Node setup 48 | super().__init__('bfs_traversal') 49 | self.traversal_pub = self.create_publisher(OccupancyGrid, 'algo_viz', 10) 50 | self.get_map_service = self.create_client(GetMap, 'get_map') 51 | 52 | # timer to call master_callback repetitively 53 | self.timer_period = 0.005 # seconds 54 | self.timer = self.create_timer(self.timer_period, self.master_callback) 55 | 56 | # Wait until service is available 57 | while not self.get_map_service.wait_for_service(timeout_sec=1.0): 58 | self.get_logger().warn('Service not available, waiting again...') 59 | 60 | # Request for initial goal pose 61 | request = GetMap.Request() 62 | self.get_map_future = self.get_map_service.call_async(request) 63 | self.get_map_future.add_done_callback(self.get_map_callback) 64 | 65 | def get_map_callback(self, future): 66 | self.received_data = True 67 | 68 | self.map_data = future.result() 69 | self.map_data.map.header.frame_id = 'algo_map' 70 | 71 | # extract height and width of map 72 | height = self.map_data.map.info.height 73 | width = self.map_data.map.info.width 74 | 75 | map_list = list(self.map_data.map.data) 76 | 77 | # generate the 2D array 78 | 79 | for j in range(0, width): 80 | row = [] 81 | for i in range(0, height): 82 | row.append(map_list[width*j + i]) 83 | if row[-1] == self.GOAL: 84 | self.goal_point.x = i 85 | self.goal_point.y = j 86 | if row[-1] == self.START: 87 | self.initial_point.x = i 88 | self.initial_point.y = j 89 | self.map.append(row) 90 | 91 | # initial_point to queue 92 | self.queue = deque([self.initial_point]) 93 | 94 | # set parent map 95 | self.parent_map = [[-1] * height for _ in range(width)] 96 | 97 | # required to generate the map 98 | self.path_node = self.goal_point 99 | 100 | self.get_logger().info("Received map data") 101 | self.get_logger().info(f"Initial pose => x: {self.initial_point.x}, y: {self.initial_point.y}") 102 | self.get_logger().info(f"Goal pose => x: {self.goal_point.x}, y: {self.goal_point.y}") 103 | 104 | def master_callback(self): 105 | 106 | # direction vector to generate neighbors 107 | dx = [0, 1, 0, -1] 108 | dy = [1, 0, -1, 0] 109 | 110 | if self.received_data: 111 | # do traversal 112 | 113 | if len(self.queue) > 0: 114 | # get recent node and mark it as visited 115 | node = self.queue.popleft() 116 | # self.map[node.y][node.x] = int(self.color) 117 | 118 | for i in range(0, 4): 119 | # generate new neighbors 120 | neighbor = Point() 121 | neighbor.x = node.x + dx[i] 122 | neighbor.y = node.y + dy[i] 123 | 124 | # neighbor is traversable free space 125 | if self.map[neighbor.y][neighbor.x] == self.FREE_SPACE: 126 | 127 | # add to queue 128 | self.queue.append(neighbor) 129 | # update the map indicating neighbor visited 130 | self.map[neighbor.y][neighbor.x] = int(self.color) 131 | 132 | # parent of neighbor is node 133 | self.parent_map[neighbor.y][neighbor.x] = node 134 | 135 | # if any of the neighbor is goal pose 136 | elif self.map[neighbor.y][neighbor.x] == self.GOAL: 137 | self.received_data = False 138 | self.goal_reached = True 139 | 140 | # parent of neighbor is node 141 | self.parent_map[neighbor.y][neighbor.x] = node 142 | 143 | 144 | self.color -= self.TRAVERSAL_COLOR_INCREMENT 145 | # uneven transition 146 | if self.color < self.FINAL_COLOR: 147 | self.color = self.TRAVERSAL_COLOR_UPDATE 148 | 149 | elif self.goal_reached: 150 | self.get_logger().info("Goal Reached",once=True) 151 | 152 | try: 153 | # get parent node to generate path 154 | path_element = self.parent_map[self.path_node.y][self.path_node.x] 155 | except: 156 | self.goal_reached = False 157 | self.get_logger().info("Path Generated") 158 | 159 | # program should not exit but else does it has to do? 160 | exit() 161 | 162 | # update map with path 163 | self.map[self.path_node.y][self.path_node.x] = 101 164 | 165 | # node is path-element 166 | self.path_node = path_element 167 | 168 | 169 | self.map_data.map.data = [] 170 | 171 | # convert 2D array to single list 172 | for list in self.map: 173 | self.map_data.map.data.extend(list) 174 | 175 | # publish data 176 | self.traversal_pub.publish(self.map_data.map) 177 | 178 | def main(args=None): 179 | rclpy.init(args=args) 180 | 181 | bfs_node = BFS() 182 | 183 | rclpy.spin(bfs_node) 184 | 185 | bfs_node.destroy_node() 186 | rclpy.shutdown() 187 | 188 | 189 | if __name__ == "__main__": 190 | main() -------------------------------------------------------------------------------- /pathplanners/map.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python3 2 | 3 | import rclpy 4 | from rclpy.node import Node 5 | from nav_msgs.msg import OccupancyGrid 6 | from nav_msgs.srv import GetMap 7 | 8 | class MetaData(Node): 9 | 10 | def __init__(self): 11 | # Node setup 12 | super().__init__('map_pub') 13 | self.map_pub = self.create_publisher(OccupancyGrid, 'map_viz', 10) 14 | self.srv = self.create_service(GetMap, 'get_map', self.get_map_callback) 15 | 16 | # timer to call master_callback repetitively 17 | self.timer_period = 0.02 # seconds 18 | self.timer = self.create_timer(self.timer_period, self.master_callback) 19 | 20 | # initial start pose 21 | self.initial_point_x = 8 22 | self.initial_point_y = 41 23 | 24 | # goal pose 25 | self.goal_point_x = 41 26 | self.goal_point_y = 8 27 | 28 | # 2D array of the map 29 | rows = 50 30 | cols = 50 31 | 32 | data_array = [[0 for _ in range(rows)] for _ in range(cols)] 33 | 34 | for i in range(1, rows - 1): 35 | data_array[i][1:-1] = [-1] * (cols - 2) 36 | 37 | 38 | data_array[int(rows/3)][-34:-1] = [0] * 33 39 | data_array[int(2 * rows/3)][1:34] = [0] * 33 40 | 41 | """ 42 | data_array = [ 43 | [0,0],[1,0] . . . 44 | [1,0] . 45 | . . 46 | . . 47 | . . . . . [49,49] 48 | ] 49 | """ 50 | 51 | # playground map 52 | self.map_data = OccupancyGrid() 53 | self.map_data.header.frame_id = 'map' 54 | self.map_data.info.resolution = 1.0 55 | self.map_data.info.width = rows 56 | self.map_data.info.height = cols 57 | self.map_data.info.origin.position.x = 0.0 58 | self.map_data.info.origin.position.y = 0.0 59 | self.map_data.info.origin.position.z = 0.0 60 | self.map_data.info.origin.orientation.x = 0.00 61 | self.map_data.info.origin.orientation.y = 0.00 62 | self.map_data.info.origin.orientation.z = 0.00 63 | self.map_data.info.origin.orientation.w = 1.00 64 | 65 | # Assign a value to a specific cell based on init anf goal pose 66 | data_array[self.initial_point_y][self.initial_point_x] = -2 67 | data_array[self.goal_point_y][self.goal_point_x] = 1 68 | 69 | # convert 2D array to single list 70 | for list in data_array: 71 | self.map_data.data.extend(list) 72 | 73 | 74 | def get_map_callback(self, request, response): 75 | # generated ref map 76 | self.get_logger().info(f"{request}") 77 | response.map = self.map_data 78 | self.get_logger().info("Now this looks like a job for me!") 79 | 80 | return response 81 | 82 | def master_callback(self): 83 | # publish data 84 | self.map_pub.publish(self.map_data) 85 | 86 | def main(args=None): 87 | rclpy.init(args=args) 88 | 89 | metadata_node = MetaData() 90 | rclpy.spin(metadata_node) 91 | 92 | metadata_node.destroy_node() 93 | rclpy.shutdown() 94 | 95 | 96 | if __name__ == "__main__": 97 | main() -------------------------------------------------------------------------------- /resource/pathplanners: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maker-ATOM/Path-Planning-Algorithms/d4353017ead9b72df8c05ceb5a2bb6b877069e6f/resource/pathplanners -------------------------------------------------------------------------------- /rviz/visualizer.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Grid1 8 | - /Map1 9 | - /Map2 10 | Splitter Ratio: 0.5 11 | Tree Height: 659 12 | - Class: rviz_common/Selection 13 | Name: Selection 14 | - Class: rviz_common/Tool Properties 15 | Expanded: 16 | - /2D Goal Pose1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz_common/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz_common/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.20000000298023224 34 | Cell Size: 1 35 | Class: rviz_default_plugins/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.029999999329447746 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 25 45 | Y: 25 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 50 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz_default_plugins/Map 53 | Color Scheme: costmap 54 | Draw Behind: false 55 | Enabled: true 56 | Name: Map 57 | Topic: 58 | Depth: 5 59 | Durability Policy: Volatile 60 | Filter size: 10 61 | History Policy: Keep Last 62 | Reliability Policy: Reliable 63 | Value: /algo_viz 64 | Update Topic: 65 | Depth: 5 66 | Durability Policy: Volatile 67 | History Policy: Keep Last 68 | Reliability Policy: Reliable 69 | Value: /algo_viz_updates 70 | Use Timestamp: false 71 | Value: true 72 | - Alpha: 0.20000000298023224 73 | Class: rviz_default_plugins/Map 74 | Color Scheme: costmap 75 | Draw Behind: false 76 | Enabled: true 77 | Name: Map 78 | Topic: 79 | Depth: 5 80 | Durability Policy: Volatile 81 | Filter size: 10 82 | History Policy: Keep Last 83 | Reliability Policy: Reliable 84 | Value: /map_viz 85 | Update Topic: 86 | Depth: 5 87 | Durability Policy: Volatile 88 | History Policy: Keep Last 89 | Reliability Policy: Reliable 90 | Value: /map_viz_updates 91 | Use Timestamp: false 92 | Value: true 93 | Enabled: true 94 | Global Options: 95 | Background Color: 48; 48; 48 96 | Fixed Frame: map 97 | Frame Rate: 30 98 | Name: root 99 | Tools: 100 | - Class: rviz_default_plugins/Interact 101 | Hide Inactive Objects: true 102 | - Class: rviz_default_plugins/MoveCamera 103 | - Class: rviz_default_plugins/Select 104 | - Class: rviz_default_plugins/FocusCamera 105 | - Class: rviz_default_plugins/Measure 106 | Line color: 128; 128; 0 107 | - Class: rviz_default_plugins/SetInitialPose 108 | Covariance x: 0.25 109 | Covariance y: 0.25 110 | Covariance yaw: 0.06853891909122467 111 | Topic: 112 | Depth: 5 113 | Durability Policy: Volatile 114 | History Policy: Keep Last 115 | Reliability Policy: Reliable 116 | Value: /initialpose 117 | - Class: rviz_default_plugins/SetGoal 118 | Topic: 119 | Depth: 5 120 | Durability Policy: Volatile 121 | History Policy: Keep Last 122 | Reliability Policy: Reliable 123 | Value: /goal_pose 124 | - Class: rviz_default_plugins/PublishPoint 125 | Single click: true 126 | Topic: 127 | Depth: 5 128 | Durability Policy: Volatile 129 | History Policy: Keep Last 130 | Reliability Policy: Reliable 131 | Value: /clicked_point 132 | Transformation: 133 | Current: 134 | Class: rviz_default_plugins/TF 135 | Value: true 136 | Views: 137 | Current: 138 | Class: rviz_default_plugins/Orbit 139 | Distance: 62 140 | Enable Stereo Rendering: 141 | Stereo Eye Separation: 0.05999999865889549 142 | Stereo Focal Distance: 1 143 | Swap Stereo Eyes: false 144 | Value: false 145 | Focal Point: 146 | X: 25.316682815551758 147 | Y: 24.91035270690918 148 | Z: -0.024869605898857117 149 | Focal Shape Fixed Size: false 150 | Focal Shape Size: 0.05000000074505806 151 | Invert Z Axis: false 152 | Name: Current View 153 | Near Clip Distance: 0.009999999776482582 154 | Pitch: -1.5697963237762451 155 | Target Frame: 156 | Value: Orbit (rviz_default_plugins) 157 | Yaw: 4.714000225067139 158 | Saved: ~ 159 | Window Geometry: 160 | Displays: 161 | collapsed: true 162 | Height: 888 163 | Hide Left Dock: true 164 | Hide Right Dock: true 165 | QMainWindow State: 000000ff00000000fd0000000400000000000001560000031efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000031e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000031efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000031e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003a10000003efc0100000002fb0000000800540069006d00650000000000000003a1000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003a10000031e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 166 | Selection: 167 | collapsed: false 168 | Time: 169 | collapsed: false 170 | Tool Properties: 171 | collapsed: false 172 | Views: 173 | collapsed: true 174 | Width: 929 175 | X: 82 176 | Y: 27 177 | -------------------------------------------------------------------------------- /setup.cfg: -------------------------------------------------------------------------------- 1 | [develop] 2 | script_dir=$base/lib/pathplanners 3 | [install] 4 | install_scripts=$base/lib/pathplanners 5 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import find_packages, setup 2 | import os 3 | from glob import glob 4 | 5 | package_name = 'pathplanners' 6 | 7 | setup( 8 | name=package_name, 9 | version='0.0.0', 10 | packages=find_packages(exclude=['test']), 11 | data_files=[ 12 | ('share/ament_index/resource_index/packages', 13 | ['resource/' + package_name]), 14 | ('share/' + package_name, ['package.xml']), 15 | (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), 16 | (os.path.join('share', package_name, 'rviz'), ['rviz/visualizer.rviz']), 17 | 18 | ], 19 | install_requires=['setuptools'], 20 | zip_safe=True, 21 | maintainer='aditya', 22 | maintainer_email='makersatom@gmail.com', 23 | description='TODO: Package description', 24 | license='TODO: License declaration', 25 | tests_require=['pytest'], 26 | entry_points={ 27 | 'console_scripts': [ 28 | "map_node = pathplanners.map:main", 29 | "bfs = pathplanners.breadthfirstsearch:main" 30 | "dijkstra = pathplanners.dijkstra:main" 31 | ], 32 | }, 33 | ) 34 | -------------------------------------------------------------------------------- /test/test_copyright.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_copyright.main import main 16 | import pytest 17 | 18 | 19 | # Remove the `skip` decorator once the source file(s) have a copyright header 20 | @pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') 21 | @pytest.mark.copyright 22 | @pytest.mark.linter 23 | def test_copyright(): 24 | rc = main(argv=['.', 'test']) 25 | assert rc == 0, 'Found errors' 26 | -------------------------------------------------------------------------------- /test/test_flake8.py: -------------------------------------------------------------------------------- 1 | # Copyright 2017 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_flake8.main import main_with_errors 16 | import pytest 17 | 18 | 19 | @pytest.mark.flake8 20 | @pytest.mark.linter 21 | def test_flake8(): 22 | rc, errors = main_with_errors(argv=[]) 23 | assert rc == 0, \ 24 | 'Found %d code style errors / warnings:\n' % len(errors) + \ 25 | '\n'.join(errors) 26 | -------------------------------------------------------------------------------- /test/test_pep257.py: -------------------------------------------------------------------------------- 1 | # Copyright 2015 Open Source Robotics Foundation, Inc. 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from ament_pep257.main import main 16 | import pytest 17 | 18 | 19 | @pytest.mark.linter 20 | @pytest.mark.pep257 21 | def test_pep257(): 22 | rc = main(argv=['.', 'test']) 23 | assert rc == 0, 'Found code style errors / warnings' 24 | --------------------------------------------------------------------------------