class BuildTrajectory(object):
""" Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system.
"""
def __init__(self):
self.save_path = os.path.join(
rospy.get_param("~save_path"),
time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj")
self.trajectory = LineTrajectory("/built_trajectory")
# receive either goal points or clicked points
self.publish_point_sub = rospy.Subscriber("/clicked_point",
PointStamped,
self.clicked_point_callback,
queue_size=1)
self.nav_goal_sub = rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.clicked_point_callback,
queue_size=1)
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
def clicked_point_callback(self, msg):
if isinstance(msg, PointStamped):
self.trajectory.addPoint(msg.point)
elif isinstance(msg, PoseStamped):
self.trajectory.addPoint(msg.pose.position)
# publish visualization of the path being built
self.trajectory.publish_viz()
def saveTrajectory(self):
self.trajectory.save(self.save_path)
class BuildTrajectory(object):
""" Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system.
"""
def __init__(self):
self.save_path = os.path.join(
rospy.get_param("~save_path"),
time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj")
self.trajectory = LineTrajectory("/built_trajectory")
'''
Insert appropriate subscribers/publishers here
'''
self.point_listener = rospy.Subscriber("/clicked_point",
PointStamped,
self.point_cb,
queue_size=1)
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
def point_cb(self, clicked_point):
self.trajectory.addPoint(clicked_point.point)
self.trajectory.publish_start_point()
self.trajectory.publish_end_point()
self.trajectory.publish_trajectory()
def saveTrajectory(self):
self.trajectory.save(self.save_path)
class BuildTrajectory(object):
""" Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system.
"""
def __init__(self):
self.save_path = os.path.join("trajectories/",
time.strftime("%Y-%m-%d-%H-%M-%S") +
".traj") #%Y-%m-%d-%H-%M-%S
self.trajectory = LineTrajectory("/built_trajectory")
self.data_points = []
self.count = 0
self.click_sub = rospy.Subscriber("/clicked_point",
PointStamped,
self.clicked_pose,
queue_size=10)
self.traj_pub = rospy.Publisher("/trajectory/current",
PoseArray,
queue_size=10)
self.trajectory_points = rospy.Publisher("/traj_pts",
Marker,
queue_size=20)
self.trajectory.publish_viz()
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
def publish_trajectory(self):
self.traj_pub.publish(self.trajectory.toPoseArray())
def saveTrajectory(self):
self.trajectory.save(self.save_path)
def clicked_pose(self, msg):
self.count += 1
point = Point()
point.x = msg.point.x
point.y = msg.point.y
self.trajectory.addPoint(point)
self.data_points.append(point)
self.mark_pt(self.trajectory_points, (0, 1, 0), self.data_points)
if self.count > 2:
rospy.loginfo("PUBLISH TRAJ")
self.publish_trajectory()
self.trajectory.publish_viz()
def mark_pt(self, subscriber, color_tup, data):
mark_pt = Marker()
mark_pt.header.frame_id = "/map"
mark_pt.header.stamp = rospy.Time.now()
mark_pt.type = mark_pt.SPHERE_LIST
mark_pt.action = mark_pt.ADD
mark_pt.scale.x = .5
mark_pt.scale.y = .5
mark_pt.scale.z = .5
mark_pt.color.a = 1.0
mark_pt.color.r = color_tup[0]
mark_pt.color.g = color_tup[1]
mark_pt.color.b = color_tup[2]
mark_pt.points = data
subscriber.publish(mark_pt)
class PathBuilder(object):
def __init__(self):
self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry,
self.odom_callback)
self.traj = LineTrajectory()
rospack = rospkg.RosPack()
fc_path = rospack.get_path("final_challenge")
self.save_path = os.path.join(
fc_path + "/trajectories/",
time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj")
rospy.on_shutdown(self.traj.save(self.save_path))
def odom_callback(self, msg):
point = msg.pose.pose.position #get Point from Odometry msg
self.traj.addPoint(point)
class BuildTrajectory(object):
""" Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system.
"""
def __init__(self):
self.save_path = os.path.join(
rospy.get_param(
"~save_path",
default=
"/home/nvidia/workspace/catkin_ws/src/ddl_0725/pf_localization/trajectories"
),
time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj")
self.trajectory = LineTrajectory("/built_trajectory")
self.odom_topic = rospy.get_param("~odom_topic", default="/pf/odom")
self.sub_odom = rospy.Subscriber(self.odom_topic,
Odometry,
self.odomCB,
queue_size=2)
self.sub_pose = rospy.Subscriber("/current_pose",
PoseStamped,
self.poseCB,
queue_size=1)
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
def odomCB(self, msg):
self.trajectory.addPoint(
msg.pose.pose.position.x, msg.pose.pose.position.y,
quaternion_to_angle(msg.pose.pose.orientation))
self.trajectory.publish_viz()
def poseCB(self, msg):
self.trajectory.addPoint(msg.pose.position.x, msg.pose.position.y,
quaternion_to_angle(msg.pose.orientation))
self.trajectory.publish_viz()
def saveTrajectory(self):
self.trajectory.save(self.save_path)
class BuildTrajectory(object):
""" Listens for points published by RViz and uses them to build a trajectory. Saves the output to the file system.
"""
def __init__(self):
self.save_path = os.path.join(
rospy.get_param(
"~save_path",
default=
"/home/lenovo/zh/workspace/catkin_ws/src/ddl_0725/localization_0725/trajectories"
),
time.strftime("%Y-%m-%d-%H-%M-%S") + ".traj")
self.trajectory = LineTrajectory("/built_trajectory")
self.odom_topic = rospy.get_param("~odom_topic", default="/pf/odom")
self.sub_odom = rospy.Subscriber(self.odom_topic,
Odometry,
self.odomCB,
queue_size=2)
# save the built trajectory on shutdown
rospy.on_shutdown(self.saveTrajectory)
def clicked_point_callback(self, msg):
if isinstance(msg, PointStamped):
self.trajectory.addPoint(msg.point)
elif isinstance(msg, PoseStamped):
self.trajectory.addPoint(msg.pose.position)
# publish visualization of the path being built
self.trajectory.publish_viz()
def odomCB(self, msg):
self.trajectory.addPoint(
msg.pose.pose.position.x, msg.pose.pose.position.y,
quaternion_to_angle(msg.pose.pose.orientation))
self.trajectory.publish_viz()
def saveTrajectory(self):
self.trajectory.save(self.save_path)
class TableFilteringV6():
def __init__(self):
# Define Adjustable Parameters
# - Scan Region (Rectangular)
self.x_min = float(rospy.get_param("~x_min")) # [m]
self.x_max = float(rospy.get_param("~x_max")) # [m]
self.y_min = float(rospy.get_param("~y_min")) # [m]
self.y_max = float(rospy.get_param("~y_max")) # [m]
# - Table Size
self.table_width = float(rospy.get_param("~table_width")) # [m]
self.table_length = float(rospy.get_param("~table_length")) # [m]
self.table_diagonal = float(rospy.get_param("~table_diagonal")) # [m]
self.table_tolerance = float(rospy.get_param("~table_tolerance")) # [m]
# - Route Trim
self.route_trim_x = rospy.get_param("~route_trim_x") # [m]
self.route_trim_y = rospy.get_param("~route_trim_y") # [m]
# Internal Use Variables - Do not modify without consultation
self.init = False # Need to be "False" when integrated with others
self.cluster_tolerance = 0.1 # [m]
self.table = False
self.trajectory = LineTrajectory("/table_trajectory")
self.tfTL = TransformListener()
self.pathpoint_1 = None
self.pathpoint_2 = None
self.refresh_rate = rospy.Rate(10)
self.stop_counter = 0
self.centerpoint = None
# Subscribers
self.scan_sub = rospy.Subscriber(rospy.get_param("~scan_topic"), LaserScan, self.scanCB, queue_size=1)
self.pose_sub = rospy.Subscriber(rospy.get_param("~pose_topic"), Odometry, self.poseCB, queue_size=1)
self.init_table_sub = rospy.Subscriber(rospy.get_param("~init_table_topic"), Bool, self.init_tableCB, queue_size=1)
# Publishers
self.route_pub = rospy.Publisher(rospy.get_param("~route_topic"), PolygonStamped, queue_size=1)
self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"), Twist, queue_size=1)
self.finish_table_pub = rospy.Publisher(rospy.get_param("~finish_table_topic"), Bool, queue_size=1)
self.viz_table_pub = rospy.Publisher(rospy.get_param("~viz_table_topic"), Marker, queue_size=1)
self.viz_points_pub = rospy.Publisher(rospy.get_param("~viz_points_topic"), Marker, queue_size=1)
# Publish Route for Navigating Under Table
self.route_publisher()
# Check when to start the Table-Filtering
def init_tableCB(self, msg):
self.init = msg.data
if(msg.data != True):
self.table = False
# Receive every LaserScan and do Filtering to find out Table
def scanCB(self, msg):
if(self.init == True):
maybe_pts = []
clusters = []
mean_pts = []
precheck_pts = []
checked_pts = []
table_pts = []
# Finding Points within selected Scan_Region (Rectangular)
for i in xrange(len(msg.ranges)):
pt = Point()
pt.x = msg.ranges[i]*np.cos(msg.angle_min + msg.angle_increment*i)
pt.y = msg.ranges[i]*np.sin(msg.angle_min + msg.angle_increment*i)
pt.z = 0.0
if(self.x_min <= pt.x <= self.x_max and self.y_min <= pt.y <= self.y_max):
maybe_pts.append(pt)
# Filter the maybe_pts into clusters of points
for j in range(0,10):
cluster_counter = 0
temp_cluster = []
if(len(maybe_pts) == 0):
break
for i in xrange(len(maybe_pts)):
if(self.dist(maybe_pts[i-1], maybe_pts[i]) <= self.cluster_tolerance):
temp_cluster.append(maybe_pts[i-1])
if(self.dist(maybe_pts[i-1], maybe_pts[i]) > self.cluster_tolerance):
cluster_counter = cluster_counter + 1
if(cluster_counter == 2):
temp_cluster.append(maybe_pts[i-1])
break
clusters.append(temp_cluster) # Save each clusters that meet the requirement
for i in xrange(len(temp_cluster)):
del maybe_pts[0]
# Filter and Find the Mean Point of survived clusters (possible as table legs)
for i in xrange(len(clusters)):
if(len(clusters[i]) > 5):
mean_pts.append(self.mean_point(clusters[i]))
# Visualize the detected Clusters
for i in xrange(len(mean_pts)):
self.viz_point(i, mean_pts[i], ColorRGBA(1,0,0,1))
# Filter the Possibilities and Find out which Sets of Clusters is equivalent as Table
for j in xrange(len(mean_pts)):
correct_counter = 0
for i in xrange(len(mean_pts)):
if(self.check_length_width_diagonal(mean_pts[j], mean_pts[i])):
correct_counter = correct_counter + 1
if(correct_counter == 3):
precheck_pts.append(mean_pts[j])
break
checked_pts = self.remove_duplicates(precheck_pts)
# Visualize the Table
print len(checked_pts)
if(len(checked_pts) == 4):
for i in xrange(len(checked_pts)):
table_pts.append(checked_pts[i])
table_pts.append(checked_pts[0])
self.viz_table(table_pts)
self.table = True
self.pathpoint_1 = self.middle_point(checked_pts[0], checked_pts[3])
self.pathpoint_2 = self.middle_point(checked_pts[1], checked_pts[2])
else:
self.pathpoint_1 = self.pathpoint_1
self.pathpoint_2 = self.pathpoint_2
# Checking Position (Map Frame) for when to stop
def poseCB(self, msg):
if(self.init == True):
current_pose = msg.pose.pose.position
current_yaw = utils.quaternion_to_angle(msg.pose.pose.orientation)
if(self.centerpoint != None):
# print "current_pose: " + str(current_pose)
# print "centerpoint : " + str(self.centerpoint)
print self.dist(current_pose, self.centerpoint)
if(self.dist(current_pose, self.centerpoint) <= 0.17): # 0.141 / 0.145 / 0.17 / 0.28 / 0.31
self.stopping()
self.stop_counter = self.stop_counter + 1
if(self.stop_counter == 30):
self.finish_table_pub.publish(True)
self.stop_counter = 0
print "----------------------------- reporting finish --------------------------------------" # Testing USE - find hidden bug
# Keep on Publishing the Latest Route for the Table-Navi
def route_publisher(self):
while not rospy.is_shutdown():
if(self.tfTL.frameExists("base_link")):
if(self.init == True and self.pathpoint_1 != None and self.pathpoint_2 != None and self.table == True):
self.trajectory.clear()
# Transform the table mid-pts (base_link) into (map)
traj_pt_1 = self.transforming_point(self.pathpoint_1, "/map")
traj_pt_1.x += self.route_trim_x
traj_pt_1.y += self.route_trim_y
traj_pt_2 = self.transforming_point(self.pathpoint_2, "/map")
traj_pt_2.x += self.route_trim_x
traj_pt_2.y += self.route_trim_y
# Line Extension(2m before the table)
traj_pt_0 = Point()
traj_pt_0.x = traj_pt_1.x + (2.0*(traj_pt_1.x - traj_pt_2.x)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_x
traj_pt_0.y = traj_pt_1.y + (2.0*(traj_pt_1.y - traj_pt_2.y)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_y
traj_pt_0.z = 0.0
# Line Extension (0.3m after the table)
traj_pt_3 = Point()
traj_pt_3.x = traj_pt_2.x - (0.3*(traj_pt_1.x - traj_pt_2.x)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_x
traj_pt_3.y = traj_pt_2.y - (0.3*(traj_pt_1.y - traj_pt_2.y)/self.dist(traj_pt_1, traj_pt_2)) + self.route_trim_y
traj_pt_3.z = 0.0
self.trajectory.addPoint(traj_pt_0)
self.trajectory.addPoint(traj_pt_1)
self.trajectory.addPoint(traj_pt_2)
self.trajectory.addPoint(traj_pt_3)
self.route_pub.publish(self.trajectory.toPolygon())
self.centerpoint = self.middle_point(traj_pt_1, traj_pt_2)
# Visualize the Path Points
self.viz_point(100, traj_pt_0, ColorRGBA(0,0,1,1))
self.viz_point(101, traj_pt_1, ColorRGBA(0,0,1,1))
self.viz_point(102, traj_pt_2, ColorRGBA(0,0,1,1))
self.viz_point(103, traj_pt_3, ColorRGBA(0,0,1,1))
else:
self.centerpoint = self.centerpoint
self.refresh_rate.sleep()
# Distance Calculator
def dist(self, a, b):
distance = np.sqrt((a.x-b.x)**2 + (a.y-b.y)**2)
return distance
# Mean Point Calculator for each Cluster
def mean_point(self, point_set):
mean_pt = Point()
for i in xrange(len(point_set)):
mean_pt.x = mean_pt.x + point_set[i].x # x-position
mean_pt.y = mean_pt.y + point_set[i].y # y-position
mean_pt.x = mean_pt.x/len(point_set)
mean_pt.y = mean_pt.y/len(point_set)
mean_pt.z = 0.0
return mean_pt
# a & b Middle Point Calculator
def middle_point(self, a, b):
middle_point = Point((a.x + b.x)/2.0, (a.y + b.y)/2.0, 0.0)
return middle_point
# Table Length or Width Checker
def check_length_width_diagonal(self, a, b):
if(0.0001<abs(self.dist(a,b) - self.table_length)<=self.table_tolerance or 0.0001<abs(self.dist(a,b) - self.table_width)<=self.table_tolerance or 0.0001<abs(self.dist(a,b) - self.table_diagonal)<self.table_tolerance):
return True
else:
return False
# Duplicates Remover
def remove_duplicates(self, values):
output = []
seen = set()
for value in values:
if value not in seen:
output.append(value)
seen.add(value)
return output
# Transform Point from coordinate frame (base_link) to (map)
def transforming_point(self, pre_pos, frame):
pre_transform_pt = PointStamped()
pre_transform_pt.header.frame_id = "base_link"
pre_transform_pt.header.stamp = rospy.Time(0)
pre_transform_pt.point = pre_pos
transformed_pt = self.tfTL.transformPoint(frame, pre_transform_pt)
after_transform_pt = Point(transformed_pt.point.x, transformed_pt.point.y, 0.0)
return after_transform_pt
# Use for Visualizing the Points
def viz_point(self, sid, pt_position, color):
point = Marker()
point.header.frame_id = "base_link"
point.header.stamp = rospy.Time.now()
point.ns = "points"
point.id = sid
point.type = 1 # CUBE
point.action = 0 # add/modify
point.lifetime = rospy.Duration(0.1)
point.scale = Vector3(0.1,0.1,0.1)
point.color = color
point.pose.orientation = Quaternion(0,0,0,1)
point.pose.position = pt_position
self.viz_points_pub.publish(point)
# Use for Visualizing the Table
def viz_table(self, table_points):
table = Marker()
table.header.frame_id = "base_link"
table.header.stamp = rospy.Time.now()
table.ns = "table"
table.id = 0
table.type = 4 # LINE STRIP
table.action = 0 # add/modify
table.lifetime = rospy.Duration(0.1)
table.scale = Vector3(0.05,0,0)
table.color = ColorRGBA(1,1,0,1) # Yellow
table.pose.orientation = Quaternion(0,0,0,1)
for p in table_points:
t_point = Point(p.x, p.y, 0.0)
table.points.append(t_point)
self.viz_table_pub.publish(table)
# Stopping the vehicle
def stopping(self):
stop_cmd = Twist()
stop_cmd.linear = Vector3(0, 0, 0)
stop_cmd.angular = Vector3(0, 0, 0)
self.drive_pub.publish(stop_cmd)
class PathPlan(object):
""" Listens for goal pose published by RViz and uses it to plan a path from
current car pose.
"""
def __init__(self):
self.current_pose = None
self.goal_pose = None
# The map data, in row-major order, starting with (0,0). Occupancy
# probabilities are in the range [0,100]. Unknown is -1.
self.map_occupancy_prob = None
self.obstacles = None
# The origin of the map [m, m, rad]. This is the real-world pose of the
# cell (0,0) in the map.
self.map_origin_pose = None
# Map width, height [cells]
self.map_dimensions = (0, 0)
# The map resolution [m/cell]
self.map_res = None
# MAKE SURE YOU UNCOMMENT THIS AND SET THE ODOM TOPIC RIGHT
# self.odom_topic = rospy.get_param("/particle_filter/odom_topic", "/odom")
self.trajectory = LineTrajectory("/planned_trajectory")
self.map_sub = rospy.Subscriber("/map", OccupancyGrid, self.map_cb)
self.traj_pub = rospy.Publisher("/trajectory/current",
PoseArray,
queue_size=10)
self.goal_sub = rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.goal_cb,
queue_size=10)
self.odom_sub = rospy.Subscriber("/pf/pose/odom", Odometry,
self.odom_cb)
def map_cb(self, msg):
self.map_origin_pose = msg.info.origin
_, _, self.theta = tf.transformations.euler_from_quaternion(
(self.map_origin_pose.orientation.x,
self.map_origin_pose.orientation.y,
self.map_origin_pose.orientation.z,
self.map_origin_pose.orientation.w))
self.map_dimensions = (msg.info.width, msg.info.height)
map = np.reshape(msg.data, (msg.info.height, msg.info.width))
self.map_occupancy_prob = dilation(map, square(4))
print(self.map_occupancy_prob[339][359])
self.map_res = msg.info.resolution
obstacles1 = np.where(self.map_occupancy_prob == 100)
obstacles2 = np.where(self.map_occupancy_prob == -1)
self.obstacles = set(zip(obstacles1[1], obstacles1[0])).union(
set(zip(obstacles2[1], obstacles2[0])))
def odom_cb(self, msg):
if self.current_pose is None:
print("intialize current pose")
#Has .position (Point) and .orientation (Quaternion) attributes
self.current_pose = msg.pose.pose
pixel_coord = self.map_to_pixel(
(self.current_pose.position.x, self.current_pose.position.y))
self.current_pose.position.x = pixel_coord[0]
self.current_pose.position.y = pixel_coord[1]
def goal_cb(self, msg):
print("intialize goal pose")
#Has .position (Point) and .orientation (Quaternion) attributes
self.goal_pose = msg.pose
pixel_coord = self.map_to_pixel(
(self.goal_pose.position.x, self.goal_pose.position.y))
self.goal_pose.position.x = pixel_coord[0]
self.goal_pose.position.y = pixel_coord[1]
while self.current_pose is None or self.goal_pose is None or self.obstacles is None:
print('waiting for all the required info')
self.plan_path(
(self.current_pose.position.x, self.current_pose.position.y),
(self.goal_pose.position.x, self.goal_pose.position.y))
# def trace_path(self, node, path = []):
# point = Point()
# map_coord = self.pixel_to_map((node[0][0], node[0][1]))
# point.x = map_coord[0]
# point.y = map_coord[1]
# path.append(point)
# if node[1] != None:
# return self.trace_path(node[1], path)
# else:
# path.reverse()
# return path
def trace_path(self, start, end, parent):
path = []
point, node = Point(), end
map_coord = self.pixel_to_map((node[0], node[1]))
point.x = map_coord[0]
point.y = map_coord[1]
path.append(point)
while parent[node] != node:
node = parent[node]
point = Point()
map_coord = self.pixel_to_map((node[0], node[1]))
point.x = map_coord[0]
point.y = map_coord[1]
path.append(point)
return path[-1::-1]
def astar(self, start_position, end_position, obstacles):
queue = []
heapq.heappush(queue, (0, start_position))
visited = set()
parent = {start_position: start_position} # for path reconstruction
mins = {start_position: 0}
while queue:
_, node = heapq.heappop(queue)
if node in visited:
continue
if node == end_position:
return self.trace_path(start_position, end_position, parent)
visited.add(node)
for neighbor in self.adj_nodes(node):
if neighbor in visited or neighbor in obstacles:
continue
new_cost = mins[node] + self.euclidian_cost(node, neighbor)
if neighbor not in mins or mins[neighbor] > new_cost:
mins[neighbor] = new_cost
priority = new_cost + self.euclidian_cost(
neighbor, end_position)
heapq.heappush(queue, (priority, neighbor))
parent[neighbor] = node
return []
def dijkstra(self, start_position, end_position, obstacles):
queue = []
heapq.heappush(queue, (0, start_position))
visited = set()
parent = {start_position: start_position} # for path reconstruction
mins = {start_position: 0}
while queue:
_, node = heapq.heappop(queue)
if node in visited:
continue
if node == end_position:
return self.trace_path(start_position, end_position, parent)
visited.add(node)
for neighbor in self.adj_nodes(node):
if neighbor in visited or neighbor in obstacles:
continue
new_cost = mins[node] + self.euclidian_cost(node, neighbor)
if neighbor not in mins or mins[neighbor] > new_cost:
mins[neighbor] = new_cost
heapq.heappush(queue, (new_cost, neighbor))
parent[neighbor] = node
return []
# def astar(self, start_position, end_position, obstacles):
# print("starting astar")
#
# queue = []
# heapq.heappush(queue, (0, 0, (start_position, None)))
# visited = set()
# while queue:
# _, cost, path = heapq.heappop(queue)
# current = path[0]
# if current in visited:
# continue
# if current == end_position:
# return self.trace_path(path, [])
# visited.add(current)
#
# for neighbor in self.adj_nodes(current):
# if neighbor in visited or neighbor in obstacles:
# continue
#
# new_cost = cost + self.euclidian_cost(current, neighbor)
# priority = new_cost + self.euclidian_cost(neighbor, end_position)
# heapq.heappush(queue, (priority, new_cost, (neighbor, path)))
# return []
def plan_path(self, start_position, end_position):
print('planning path')
if end_position in self.obstacles:
print("End goal is occupied")
return
path = self.dijkstra(start_position, end_position, self.obstacles)
if not path:
print("trajectory not found")
self.trajectory.clear()
for point in path:
self.trajectory.addPoint(point)
self.traj_pub.publish(self.trajectory.toPoseArray())
# visualize trajectory Markers
self.trajectory.publish_viz()
def euclidian_cost(self, position1, position2):
#Idk how the dubin curve thing right now so here's a 2D Euclidan heuristic lol
return ((position2[0] - position1[0]))**2 + (
(position2[1] - position1[1]))**2
# def dubin_cost(self, pose1, pose2):
# x0 = pose1.position.x
# y0 = pose1.position.y
# _, _, theta0 = tf.transformations.euler_from_quaternion((pose1.orientation.x, pose1.orientation.y, pose1.orientation.z, pose1.orientation.w))
#
# x1 = pose2.position.x
# y1 = pose2.position.y
# _, _, theta1 = tf.transformations.euler_from_quaternion((pose2.orientation.x, pose2.orientation.y, pose2.orientation.z, pose2.orientation.w))
# q0 = (x0, y0, theta0)
# q1 = (x1, y1, theta1)
# turning_radius = 0.5
# step_size = 0.1
# path = dubins.shortest_path(q0, q1, turning_radius)
# configurations, _ = path.sample_many(step_size)
def adj_nodes(self, point):
#return nodes that are adjacent to point
adj_nodes = []
for i in range(-1, 2):
for j in range(-1, 2):
if 0 <= point[0] + i < self.map_dimensions[
0] and 0 <= point[1] + j < self.map_dimensions[1]:
adj_nodes.append((point[0] + i, point[1] + j))
return adj_nodes
def map_to_pixel(self, c):
#converts a pose to a pixel
shift = np.array([[self.map_origin_pose.position.x],
[self.map_origin_pose.position.y]])
c_np = np.array([[c[0]], [c[1]]])
res = self.map_res
rotation = np.array([[math.cos(self.theta),
math.sin(self.theta)],
[-1 * math.sin(self.theta),
math.cos(self.theta)]])
p = (np.linalg.inv(rotation).dot((c_np - shift))) / res
p = p.flatten()
return round(p[0]), round(p[1])
def pixel_to_map(self, c):
#converts a pose to a pixel
shift = np.array([[self.map_origin_pose.position.x],
[self.map_origin_pose.position.y]])
c_np = np.array([[c[0]], [c[1]]])
res = self.map_res
rotation = np.array([[math.cos(self.theta),
math.sin(self.theta)],
[-1 * math.sin(self.theta),
math.cos(self.theta)]])
m = res * rotation.dot(c_np) + shift
m = m.flatten()
return m[0], m[1]
