Paltech Assignment

Usage

install the following python packages

pip install numpy
pip install matplotlib
pip install networkx
pip install scipy
pip install dubins

Part 1: Basic waypoint manager in ROS2

Task 1: Set waypoints from geojson file.

Complete the set_waypoints_callback function in waypoint_manager.py in order to load the waypoints to the node.

• I use set_waypoints_callback function as a service callback in the to handles requests to load and set waypoints for a robot from a GeoJSON file.The function uses a service request message that includes a file path to a GeoJSON file. It processes this file, extracting waypoint data and converting it into a format suitable for the robot's navigation system.The waypoints are stored as Waypoint messages, which include latitude , longitude information an orentiation. These messages are appended to the waypoint_list_geo attribute of the class instance

Task 2:

• In this part i implement function to transforms geographic coordinates (latitude and longitude) of waypoints into a robot's local Cartesian coordinate frame (X, Y coordinates). The transformation is done using the Equirectangular approximation, which is a simple method to convert geographic coordinates into Cartesian coordinates. This method is suitable for small distances on the Earth's surface.The transformed waypoints are then stored in waypoint_list_robot_frame for further use.

Task 3 :

• Visualization waypoints in robot frame

  

  Waypoints in robot frame

Part 2: Path Planning

Method 1 - Nearest Neighbor Algorithm with Dubins Path Constraints

To implement the nearest neighbor algorithm, i begin at robot position. From there, i find the closest unvisited waypoint and add it to the sequence. Then, we move to the next node and repeat the process of finding the nearest unvisited node until all nodes are included in the tour.To find the neareast unvisited node i use The KDTree class from scipy.spatial to find the k nearest neighbors of the current waypoint instead of searching the whole waypoint.

Summary of the Nearest Neighbor

1. Start at a robot position.
2. Find k neareast neighbors waypoints to the current waypoint.
3. Remove neareast waypoint with robot non holonomic constrain:omit waypoints inside the turning radius of the current waypoint**
4. Calculate Dubins path to each selected waypoint.
5. Select waypoint with minimum dubins path lenght.
6. Add to Path and Repeat: Add the nearest point to the path, mark it as visited, and move to this point. Repeat the process until all points are visited.**

MEthod 2 : Using Traveling Salesman Problem (TSP) approximate Algorithm

This method uses the NetworkX library to build a weighted graph between waypoints, allowing for efficient path planning. It then calculates an approximate solution to the Traveling Salesman Problem (TSP) using this graph. Finally, it utilizes the TSP solution to compute new Dubins paths between waypoints, ensuring adherence to non-holonomic constraints.

steps

1. Add Nodes: Add a node to the graph for each waypoint, including the robot's starting position.

2. Add Edges: Adds edges to the graph with the calculated path length as the weight. instead of create a graph with the whole node i use the KD-tree for efficient neighbor searching.

3. Find an approximate solution to the Traveling Salesman Problem (TSP) starting from a start waypoint.:uses the NetworkX approximation algorithm to solve the TSP for the graph

4. Calculate dubins path: calculate the dubins path from the approximate tps path , omit points that does not adhere the non holonomic constrain of te robot.

Path Efficiency Optimization

1. Smooth and Continuous Paths: By calculating paths based on the robot's turning constraints and the. direction to the next waypoint, and choosing the next waypoint based on the dubins path lenght makes resulting paths are smooth , continuous and more to forward motion, avoiding abrupt changes in direction that might be impractical or impossible for the robot to follow.

2. Orientation bettewen Way points:By implementing the angle to the next waypoint based on the slope (i.e., the direction from the current point to the next waypoint) ensures the path efficiency and adheres to these constraints.

The following figure shows optimaized dubins path bettewen waypoints

5sample points

5 sample points

Obstacle Avoidance Considertion

if we consider there is an obstacle in the area i included two files.which can be used while connecting to the nextwaypoint.

1.RRT_DUBINS : This takes the start and the next waypoint and gives us a efficient dubins path between the two waypoints considering the obstacle(uses the StateValidityChecker to check for state of position and path). The dubins path calculation is integrated in the RRT_STAR while stearing to the next point considring the non holonomic constrain of the robot.

2.State_Validity_Checker: This class, StateValidityChecker, is responsible for checking the validity of individual positions and paths (sequences of positions) with respect to obstacel list.

Bonus

1.How does the path planning change if we allow the robot to move in reverse? (REEDS_SHEPP motion model instead of DUBINS) Explain.

The path planning changes significantly if we allow the robot to move in reverse.The Dubins motion model assumes that the robot can only move forward, and it finds the shortest path under this constraint. This model is based on the assumption that the robot has a minimum turning radius and cannot move in reverse. The paths generated by this model are sequences of straight lines and arcs.

On the other hand, the Reeds-Shepp motion model allows the robot to move in reverse. This means that the robot can perform more complex maneuvers, such as moving straight, turning while moving forward, and turning while moving in reverse. As a result, the Reeds-Shepp model can find shorter paths than the Dubins model in many cases, especially in tight spaces or complex environments.This reduces the total distance the robot needs to traverse, leading to time and energy savings.

2.What would happen if the working area of the robot is taken into account? E.G. the robot can remove weeds that are in a radius of 0,5m around its center. Explain.

Taking into account the working area of a robot, such as a radius of 0.5m around its center where it can perform tasks, can significantly enhance the efficiency of its operations. This is because the robot doesn't need to move to every single point of operation. Instead, it can position itself in a location where multiple tasks can be performed within its working area.The path planning algorithm can be optimized to take the working area into account. Instead of planning a path that visits each weed individually, the algorithm can plan a path that visits a series of points such that all weeds are within the working area of at least one point. This can result in a shorter and more efficient path.

one solution to implement this this process, we can employ a clustering method. Clustering algorithms, such as K-means or DBSCAN, can group nearby weeds into clusters. Each cluster can then be represented by a single point, such as the centroid or the medoid of the cluster.In the path planning algorithm, instead of considering each weed as a separate point, we consider each cluster's representative point as a single point.

3.What would happen with the path planning if the robot can remove weeds inside a rectangle of 20cm x 60cm instead of a circular shape? Explain.

considering circular working area we can cluster the waypoints in circluar cluster and the robot can cover the cluster with out nedding to turn or reposition itself,we can use standard clustering algorithms with the cluster radius set to the working radius of the robot.

When the robot has a rectangular working area, the orientation of the rectangle becomes critical for maximizing coverage and ensuring all weeds within its dimensions are effectively removed. This involves determining the optimal orientation for the robot at each waypoint to cover the maximum number of weed positions within the rectangular working area.

The path planning algorithm can be changed as follow:

1. Use clustering algorithms to group nearby weed positions into clusters. These clusters must consider the dimensions of the rectangular working area.
2. For each cluster or waypoint, calculate the optimal orientation of the rectangular working area to maximize coverage. This may involve rotating the rectangle and evaluating the number of weed positions covered in each orientation.
3. Plan paths between clustered waypoints, taking into account the robot's non-holonomic constraints in the dubins path planning make the orientation of the goal point to the rectangular cluster waypoint orientation

Another approach could be to divide the entire working area into a grid of cells. The size of each cell should be at most the dimensions of the robot's working area. Then, we can identify the cells that contain weeds. Because of the rectangular shape of the cells, the robot can cover the grid cells efficiently if it reaches them in two opposite orientations only. Therefore, the path planning must consider the orientation of the robot to reach the cell while covering the whole rectangular area. After determining the path, the robot would start moving from cell to cell that contains weeds, always keeping the same orientation or alternating between opposite orientations.