class PlannerStraightLine
This class calculates and displays the linear path between two points. As we are on a 2.5d surface, Z coordinate is taken from the map.
This class is a base for A* Graph, Grid, Dynamic Grid and Graph classes. These classes provide a path in the form of an array of centers of cells we are passing through (in the case of grid based methods) and coordinates of graph nodes (in the case of a graph based method). As these points can be far from each other AND Path Follower (that we will revisit in one of the following sections) requires path with points that are dense enough, especially when the road turns, the PlannerStraightLine class is responsible for filling the gaps.
Note that all planner classes (PlannerStraightLine, PlannerAStarGrid, PlannerAStarGridDynamic, PlannerAStarGraph) have their own "main" functions that are used for demo purposes. You can run the file without ROS2, as a standalone, for example:
python3 PlannerStraightLine.py
That will execute its "main" and show the map and a path in a popup window.
PlannerStraightLine.py
# (c) robotics.snowcron.com
# Use: MIT license
from geometry_msgs.msg import PoseStamped
from nav_msgs.srv import GetPlan
from nav_msgs.msg import Path
from geometry_msgs.msg import Pose, Twist
import math
from geometry_msgs.msg import Quaternion
from PIL import Image
import math
import cv2
import numpy as np
# A trick that allows to run the code from ROS2 (which for all my projects starts in "harsh" folder)
# as well as from python3, in terminal at harsh/src/nav25d_07
# This approach is ugly and should be replaced by a universal one... sometimes.
try:
from nav25d_07.utils import *
except:
from utils import *
class PlannerStraightLine:
def __init__(self):
# 1 meter long segments.
# TBD: make segments longer on straight parts of a road
self.dSegmentLen = 1.0
# --- In a cycle, with a fixed step... A very primitive line drawing algorithm
def generateStraightLinePath(self, dx, dy, path, start):
# Calculate distance between start and goal
distance = math.sqrt(dx**2 + dy**2)
# Number of points to include in path
num_points = int(distance / self.dSegmentLen)
for i in range(1, num_points + 1):
pose_prev = path.poses[-1]
pose = PoseStamped()
pose.pose.position.x = start.pose.position.x + i * dx / num_points
pose.pose.position.y = start.pose.position.y + i * dy / num_points
# Calculate quaternion based on segment direction
alpha = math.atan2(pose.pose.position.y - pose_prev.pose.position.y,
pose.pose.position.x - pose_prev.pose.position.x)
quat = euler_to_quaternion(0, 0, alpha)
# Set orientation
pose.pose.orientation.x = quat[0] # qx
pose.pose.orientation.y = quat[1] # qy
pose.pose.orientation.z = quat[2] # qz
pose.pose.orientation.w = quat[3] # qw
# ---
path.poses.append(pose)
return path
# --- Loading YAML file and initializing map (background image)
# What does grid have to do with straight line planner?
# We use keepout grid (pgm) and YAML that wraps it in order to get map resolution and origin.
# That is for displaying the result, including displaying it in ROS2 / Gazebo
def loadKeepoutGrid(self, strYamlImageFilePath):
img, self.dOriginMetersX, self.dOriginMetersY, strMode, self.dResolution, nNegate, nOccupiedThresh, nFreeThresh = loadKeepoutHelper(strYamlImageFilePath)
self.width, self.height = img.shape
self.keepout_yaml = img, self.dOriginMetersX, self.dOriginMetersY, strMode, self.dResolution, nNegate, nOccupiedThresh, nFreeThresh
# ---
if __name__ == "__main__":
straightLine = PlannerStraightLine()
# Coordinates in meters
start = (120., 120.)
goal = (0., 300.)
straightLine.loadKeepoutGrid("./../harsh/src/maps/perfect_map_01.yaml")
start_pose = PoseStamped()
start_pose.pose.position.x = start[0]
start_pose.pose.position.y = start[1]
goal_pose = PoseStamped()
goal_pose.pose.position.x = goal[0]
goal_pose.pose.position.y = goal[1]
path = Path()
path.poses.append(start_pose)
dx = goal_pose.pose.position.x - start_pose.pose.position.x
dy = goal_pose.pose.position.y - start_pose.pose.position.y
path = straightLine.generateStraightLinePath(dx, dy, path, start_pose)
# drawPath displays map, draws decorations (if drawDecorations function is passed, currently
# it is set to None) and displays path on top of it.
drawPath(path, "./../harsh/src/maps/perfect_map_01.yaml", straightLine.keepout_yaml, None, None)
Result:
