# I am building a path planning and obstacle avoidance bot using ros, lidar, etc . Can someone please provide a python program to implement?

I am building a robot to perform path planning using djikstra algorithm and obstacle avoidance using gaussian potential field. The robot uses raspberry Pi 4 with ubuntu 20.04 os containing gazebo, ros noetic and rviz. It has 4 motors, 2 l298n motor drivers and tf luna lidar. I need a code containing path planning using djikstra Algorithm, obstacle avoidance, perception mapping, motor control and getting values from lidar preferably in python. As I am new to this, can someone please help me? I was trying to search for code and found these:

1.Djikstra+motor control:

    from gpiozero import Motor
import time

# Define motor pins and create motor objects
flmotor = Motor(forward=16, backward=17)
frmotor = Motor(forward=18, backward=13)
blmotor = Motor(forward=9, backward=11)
brmotor = Motor(forward=10, backward=12)

# Define a function to perform Dijkstra's algorithm for path planning
def dijkstra(graph, start):
distances = {node: float('inf') for node in graph}
distances[start] = 0
visited = set()
queue = [start]

while queue:
current = min(queue, key=lambda node: distances[node])
queue.remove(current)

for neighbor, weight in graph[current].items():
distance = distances[current] + weight
if distance < distances[neighbor]:
distances[neighbor] = distance
queue.append(neighbor)

return distances

# Example path planning function
def plan_path():
# Example graph representing the map
graph = {
'A': {'B': 5, 'C': 3},
'B': {'A': 5, 'C': 2, 'D': 1},
'C': {'A': 3, 'B': 2, 'D': 4, 'E': 6},
'D': {'B': 1, 'C': 4, 'E': 7},
'E': {'C': 6, 'D': 7}
}

start_node = 'A'
distances = dijkstra(graph, start_node)

# Sort the distances dictionary by values
sorted_distances = sorted(distances.items(), key=lambda x: x[1])

# Extract the waypoints from the sorted distances
waypoints = [node for node, _ in sorted_distances]

return waypoints

# Movement functions
def move_forward():
flmotor.forward()
frmotor.forward()
blmotor.forward()
brmotor.forward()

def stop_motors():
flmotor.stop()
frmotor.stop()
blmotor.stop()
brmotor.stop()

# Main function for autonomous control
def main():
waypoints = plan_path()
for waypoint in waypoints:
# Move the robot towards the waypoint (not implemented in this example)
move_forward()
time.sleep(1)  # Simulated movement time
print(f"Reached waypoint: {waypoint}")

# Reached last waypoint, stop the motors
stop_motors()

if __name__ == "__main__":
main()

1. Obstacle avoidance:

 import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist
from gpiozero import Motor
import time

# ... (Dijkstra's algorithm and path planning functions remain the same)

class ObstacleAvoidanceRobot:
def __init__(self):
# ... (motor setup and ROS node initialization remain the same)

def run(self):
while not rospy.is_shutdown():
try:
# Plan a path to the goal
waypoints = self.plan_path(self.graph, 'A', 'E')  # Adjust start and end nodes as needed

# Navigate towards waypoints with obstacle avoidance
for waypoint in waypoints:
self.navigate_to_waypoint(waypoint)

self.stop_motors()  # Reached destination
except rospy.ROSInterruptException:
pass

def navigate_to_waypoint(self, waypoint):
target_x, target_y = waypoint
curr_x, curr_y = 0, 0  # Initialize current position (adjust as needed)

while (curr_x, curr_y) != (target_x, target_y):
laser_data = rospy.wait_for_message('/scan', LaserScan)  # Wait for LiDAR data
self.update_position(laser_data)  # Implement this function for positioning

if self.obstacle_detected(laser_data):
self.avoid_obstacle()
else:
self.move_toward_waypoint(waypoint)

def move_toward_waypoint(self, waypoint):
# Calculate direction and move forward (implement this logic)
pass

# ... (obstacle avoidance functions remain the same)

if __name__ == '__main__':
robot = ObstacleAvoidanceRobot()
robot.run()