class TrajectoryCaller(object):
""" Call a pre-defined trajectory from the file system and publishes it to a ROS topic.
"""
def __init__(self):
# Define Topics for Publishing and Subscribing Messages
self.sub_topic = rospy.get_param("~sub_topic", "trajectory/new")
self.pub_topic = rospy.get_param("~pub_topic", "trajectory/current")
# Internal Use Variables - Do not modify without consultation
self.counts = 0
# Initialize and Load the trajectory
self.trajectory = LineTrajectory("/called_trajectory")
# Subscriber
self.traj_sub = rospy.Subscriber(self.sub_topic,
String,
self.trajCB,
queue_size=1)
# Publisher
self.traj_pub = rospy.Publisher(self.pub_topic,
PolygonStamped,
queue_size=1)
# Callback Function for receiving path
def trajCB(self, msg):
path_id = msg.data
if self.counts == 1:
self.publish_trajectory(path_id)
self.counts = 0
self.counts = self.counts + 1
# Republish the trajectory after being called
def publish_trajectory(self, path_name):
# clear the trajectory whenever going to load new path
self.trajectory.clear()
# load a new received path
self.trajectory.load(path_name)
print "Publishing trajectory to:", self.pub_topic
self.traj_pub.publish(self.trajectory.toPolygon())
class AutoCharging():
def __init__(self):
# Define Topics for publishing or subscribing messages
self.init_charging_topic = rospy.get_param("~init_charging_topic")
self.battery_topic = rospy.get_param("~battery_topic")
self.pose_topic = rospy.get_param("~pose_topic")
self.drive_topic = rospy.get_param("~drive_topic")
self.mag_drive_topic = rospy.get_param("~mag_drive_topic")
self.contactor_topic = rospy.get_param("~contactor_topic")
self.stop_charging_topic = rospy.get_param("~stop_charging_topic")
self.finish_charging_topic = rospy.get_param("~finish_charging_topic")
self.suspend_topic = rospy.get_param("~suspend_topic")
self.notify_topic = rospy.get_param("~notify_topic")
self.magnetic_topic = rospy.get_param("~magnetic_topic")
# Define Adjustable Parameters
self.stop_pt = Point(float(rospy.get_param("~stop_pt_x")),
float(rospy.get_param("~stop_pt_y")), 0)
self.check_pt = Point(float(rospy.get_param("~check_pt_x")),
float(rospy.get_param("~check_pt_y")), 0)
self.finish_pt = Point(float(rospy.get_param("~finish_pt_x")),
float(rospy.get_param("~finish_pt_y")), 0)
self.heading_min = float(rospy.get_param("~heading_min"))
self.heading_max = float(rospy.get_param("~heading_max"))
self.charging_route = rospy.get_param("~charging_route")
self.charge_stop_voltage = int(rospy.get_param("~charge_stop_voltage"))
self.charge_stop_offset = int(rospy.get_param("~charge_stop_offset"))
# Internal Use Variables - Do not modify without consultation
self.trajectory = LineTrajectory("/charging_trajectory")
self.init = False
self.step_counter = 0
self.first_time = True
self.contactor_counter = 0
self.finish_charging_counter = 0
self.mag_data = []
self.mag_end = False
self.cnt = 0
self.voltage_percent = 0
self.spike_cnt = 0
self.charge_OK = False
# Subscribers
self.init_charging_sub = rospy.Subscriber(self.init_charging_topic,
Bool,
self.init_chargingCB,
queue_size=1)
self.battery_sub = rospy.Subscriber(self.battery_topic,
String,
self.batteryCB,
queue_size=1)
self.pose_sub = rospy.Subscriber(self.pose_topic,
Odometry,
self.poseCB,
queue_size=1)
self.mag_sub = rospy.Subscriber(self.magnetic_topic,
String,
self.magCB,
queue_size=1)
# Publishers
self.drive_pub = rospy.Publisher(self.drive_topic, Twist, queue_size=1)
self.mag_drive_pub = rospy.Publisher(self.mag_drive_topic,
Twist,
queue_size=1)
self.contactor_pub = rospy.Publisher(self.contactor_topic,
Bool,
queue_size=1)
self.stop_charging_pub = rospy.Publisher(self.stop_charging_topic,
Bool,
queue_size=1)
self.finish_charging_pub = rospy.Publisher(self.finish_charging_topic,
Bool,
queue_size=1)
self.traj_pub = rospy.Publisher("/charging_navi/trajectory",
PolygonStamped,
queue_size=1)
self.suspend_pub = rospy.Publisher(self.suspend_topic,
Bool,
queue_size=1)
self.notify_pub = rospy.Publisher(self.notify_topic,
String,
queue_size=1)
def init_chargingCB(self, msg):
self.init = msg.data
def batteryCB(self, msg):
self.voltage_percent = float(msg.data)
def poseCB(self, msg):
print "step_counter : " + str(self.step_counter) + ", Batt(%):" + str(
self.voltage_percent)
now_position = msg.pose.pose.position
now_heading = self.quaternion_to_angle(msg.pose.pose.orientation)
# Initiate self charging procedure
if (self.init == True and self.first_time == True):
self.step_counter = 1
self.trajectory.clear()
print "Step 0 - Load Trajectory"
self.trajectory.load(self.charging_route)
time.sleep(0.5)
self.traj_pub.publish(self.trajectory.toPolygon())
self.first_time = False
self.stop_charging_pub.publish(False)
# Step 1 - When arriving check point, disable the charging navi and start to tune heading
if (self.step_counter == 1 and self.dist_checker(
now_position, self.check_pt, 0.25) == True):
print "Step 1 - Tune heading"
print np.rad2deg(now_heading)
self.init = False
if (now_heading > np.deg2rad(self.heading_max)
or now_heading < np.deg2rad(self.heading_min)):
self.step_counter = 2
else:
self.tuning_heading(0.3)
self.stop_charging_pub.publish(True)
self.finish_charging_pub.publish(False)
# Step 2 - Continue enable Charging Navi after the checking point
elif (self.step_counter == 2
and self.dist_checker(now_position, self.stop_pt, 0.1) == False):
print "Step 2 - Continue enable Charging Navi"
self.stop_charging_pub.publish(False)
self.step_counter = 3
#------------------Stat
# Step 3 - Stop
elif (self.step_counter == 3
and self.dist_checker(now_position, self.stop_pt, 0.1) == True):
print "Step 3 - Stop Vehicle at Charging Point"
self.stopping()
self.stop_charging_pub.publish(True)
self.step_counter = 4
# Step 4 - Python Magnetic
elif (self.step_counter == 4):
if (self.mag_end == True):
print "Magnetic Drive... Charging"
self.contactor_pub.publish(True)
self.stopping()
self.stop_charging_pub.publish(True)
if (self.contactor_counter >= 60):
self.step_counter = 5
self.contactor_counter = 0
self.mag_end = False
self.contactor_counter = self.contactor_counter + 1
else:
self.notify_pub.publish("1")
self.mag_drive()
print "Magnetic Drive... Moving To Charger"
#Step 5 - Turn Off the Contactor after finish charging
elif (self.step_counter == 5 and self.charge_OK == True):
print "Step 5 - Turn OFF Contactor-----" + str(
self.finish_charging_counter)
if (self.finish_charging_counter >= 30):
self.contactor_pub.publish(False)
self.charge_OK = False
self.step_counter = 6
self.finish_charging_counter = 0
self.finish_charging_counter = self.finish_charging_counter + 1
#------------------END
elif (self.step_counter == 6):
print "Step 6 - Navi to Finish Location"
self.stop_charging_pub.publish(False)
self.step_counter = 7
# Step 7 - Report finish self_charging procedure, Reset counter
elif (self.step_counter == 7 and self.dist_checker(
now_position, self.finish_pt, 1.0) == True):
print "Step 7 - Finish self_charging"
self.finish_charging_pub.publish(True)
self.step_counter = 0
self.first_time = True
self.contactor_counter = 0
self.finish_charging_counter = 0
#measure voltage 10 times before confirming it full prevent false reading from spike
if (self.voltage_percent >=
(self.charge_stop_voltage + self.charge_stop_offset)):
if (self.spike_cnt > 100):
self.charge_OK = True
self.spike_cnt = 0
self.spike_cnt = self.spike_cnt + 1
print "Done Charged Count:" + str(self.spike_cnt)
else:
print "Done Charge:" + str(self.charge_OK)
def magCB(self, msg):
raw = map(int, msg.data.split("/"))
self.mag_data = raw
def mag_drive(self):
x = 0
z = 0
mg = self.mag_data
j = [i for i in mg if i > 150]
self.cnt = self.cnt + 1
if (len(j) > 8):
#stop
self.notify_pub.publish("0,0") #Boot = 0, Low_level_Drive = 0
x = 0
z = 0
self.mag_end = True
print str(len(j)) + "----STOP, " + str(self.mag_end)
else:
self.notify_pub.publish("0,1") #Boot = 0, Low_level_Drive = 1
# line_trace speed set
x = 0.18
drive_msg = Twist()
drive_msg.linear = Vector3(x, 0, 0)
drive_msg.angular = Vector3(0, 0, z)
self.mag_drive_pub.publish(drive_msg)
# Distance Checker, Checking whether 2 points are within distance tolerance or not
def dist_checker(self, a, b, dist_tolerance):
distance = np.sqrt((a.x - b.x)**2 + (a.y - b.y)**2)
if (distance <= dist_tolerance):
return True
else:
return False
# Tuning the Heading of Vehicle
def tuning_heading(self, rotspeed):
drive_msg = Twist()
drive_msg.linear = Vector3(0, 0, 0)
drive_msg.angular = Vector3(0, 0, rotspeed)
self.drive_pub.publish(drive_msg)
# Stopping Vehicle
def stopping(self):
drive_msg = Twist()
drive_msg.linear = Vector3(0, 0, 0)
drive_msg.angular = Vector3(0, 0, 0)
self.drive_pub.publish(drive_msg)
# Convert Quaternion to Angle
def quaternion_to_angle(self, q):
x, y, z, w = q.x, q.y, q.z, q.w
roll, pitch, yaw = tf.transformations.euler_from_quaternion(
(x, y, z, w))
return yaw
class PsaAutoChargingV2():
def __init__(self):
# Define Adjustable Parameters
self.stop_pt = Point(float(rospy.get_param("~stop_pt_x")),
float(rospy.get_param("~stop_pt_y")), 0)
self.check_pt = Point(float(rospy.get_param("~check_pt_x")),
float(rospy.get_param("~check_pt_y")), 0)
self.finish_pt = Point(float(rospy.get_param("~finish_pt_x")),
float(rospy.get_param("~finish_pt_y")), 0)
self.heading_min = float(rospy.get_param("~heading_min"))
self.heading_max = float(rospy.get_param("~heading_max"))
self.charging_route = rospy.get_param("~charging_route")
self.charge_stop_voltage = int(rospy.get_param("~charge_stop_voltage"))
self.charge_stop_offset = int(rospy.get_param("~charge_stop_offset"))
# Internal Use Variables - Do not modify without consultation
self.trajectory = LineTrajectory("/charging_trajectory")
self.init = False
self.step_counter = 0
self.first_time = True
self.contactor_counter = 0
self.finish_charging_counter = 0
self.mag_data = []
self.mag_end = False
self.cnt = 0
self.voltage_percent = 0
self.spike_cnt = 0
self.charge_OK = False
self.iters = 0
# Subscribers
self.init_charging_sub = rospy.Subscriber(
rospy.get_param("~init_charging_topic"),
Bool,
self.init_chargingCB,
queue_size=1)
self.battery_sub = rospy.Subscriber(rospy.get_param("~battery_topic"),
String,
self.batteryCB,
queue_size=1)
self.pose_sub = rospy.Subscriber(rospy.get_param("~pose_topic"),
Odometry,
self.poseCB,
queue_size=1)
self.amcl_sub = rospy.Subscriber(rospy.get_param("~amcl_topic"),
PoseWithCovarianceStamped,
self.poseCB,
queue_size=1)
self.magnetic_sub = rospy.Subscriber(
rospy.get_param("~magnetic_topic"),
String,
self.magCB,
queue_size=1)
# Publishers
self.drive_pub = rospy.Publisher(rospy.get_param("~drive_topic"),
Twist,
queue_size=1)
self.mag_drive_pub = rospy.Publisher(
rospy.get_param("~mag_drive_topic"), Twist, queue_size=1)
self.contactor_pub = rospy.Publisher(
rospy.get_param("~contactor_topic"), Bool, queue_size=1)
self.stop_charging_pub = rospy.Publisher(
rospy.get_param("~stop_charging_topic"), Bool, queue_size=1)
self.finish_charging_pub = rospy.Publisher(
rospy.get_param("~finish_charging_topic"), Bool, queue_size=1)
self.traj_pub = rospy.Publisher("/charging_navi/trajectory",
PolygonStamped,
queue_size=1)
self.suspend_pub = rospy.Publisher(rospy.get_param("~suspend_topic"),
Bool,
queue_size=1)
self.notify_pub = rospy.Publisher(rospy.get_param("~notify_topic"),
String,
queue_size=1)
self.init_turn_pub = rospy.Publisher("/turning/init",
String,
queue_size=1)
def init_chargingCB(self, msg):
self.init = msg.data
def batteryCB(self, msg):
self.voltage_percent = float(msg.data)
def poseCB(self, msg):
print "step_counter : " + str(self.step_counter) + ", Batt(%):" + str(
self.voltage_percent)
if isinstance(msg, Odometry):
now_position = msg.pose.pose.position
now_heading = self.quaternion_to_yaw(msg.pose.pose.orientation)
elif isinstance(msg, PoseWithCovarianceStamped):
now_position = msg.pose.pose.position
now_heading = self.quaternion_to_yaw(msg.pose.pose.orientation)
# Initiate self charging procedure
if (self.init == True and self.first_time == True):
self.step_counter = 1
self.trajectory.clear()
print "Step 0 - Load Trajectory"
self.trajectory.load(self.charging_route)
time.sleep(0.5)
self.traj_pub.publish(self.trajectory.toPolygon())
self.first_time = False
self.stop_charging_pub.publish(False)
# Step 1 - When arriving check point, disable the charging navi and start to tune heading
if (self.step_counter == 1 and self.dist_checker(
now_position, self.check_pt, 0.25) == True):
print "Step 1 - Tune heading"
print "current_heading : " + str(now_heading)
self.init = False
if (self.heading_min < now_heading < self.heading_max):
self.step_counter = 2
else:
self.init_turn_pub.publish("90")
self.stop_charging_pub.publish(True)
self.finish_charging_pub.publish(False)
# Step 2 - Continue enable Charging Navi after the checking point
elif (self.step_counter == 2
and self.dist_checker(now_position, self.stop_pt, 0.1) == False):
print "Step 2 - Continue enable Charging Navi"
self.stop_charging_pub.publish(False)
self.step_counter = 3
# Step 3 - Pass over to Magnetic Following Mode
elif (self.step_counter == 3
and self.dist_checker(now_position, self.stop_pt, 0.1) == True):
print "Step 3 - Ready to Swap into Magnetic Following"
self.stopping()
self.stop_charging_pub.publish(True)
self.step_counter = 4
# Step 4 - Magnetic Following and ON Charging after arriving the end of magnetic line
elif (self.step_counter == 4):
if (self.mag_end == True):
print "Turn ON - Charging"
if (self.contactor_counter % 50 == 0):
self.contactor_pub.publish(True)
self.stopping()
self.stop_charging_pub.publish(True)
if (self.contactor_counter >= 60 and self.charge_OK == True):
self.step_counter = 5
self.contactor_counter = 0
self.mag_end = False
self.contactor_counter += 1
else:
self.mag_drive()
print "Step 4 - Magnetic Drive... Moving To Charger"
# Step 5 - Turn Off the Contactor after finish charging
elif (self.step_counter == 5 and self.charge_OK == True):
print "Step 5 - Turn OFF Contactor-----" + str(
self.finish_charging_counter)
self.contactor_pub.publish(False)
if (self.finish_charging_counter >= 50):
self.charge_OK = False
self.step_counter = 6
self.finish_charging_counter = -1
self.finish_charging_counter += 1
# Step 6 - Revert back to LIDAR Navigation Mode
elif (self.step_counter == 6):
print "Step 6 - LIDAR Navi to Finished Location"
self.stop_charging_pub.publish(False)
self.step_counter = 7
# Step 7 - Report finish self_charging procedure, Reset counter
elif (self.step_counter == 7 and self.dist_checker(
now_position, self.finish_pt, 1.0) == True):
print "Step 7 - Finish self_charging"
self.finish_charging_pub.publish(True)
self.step_counter = 0
self.first_time = True
self.contactor_counter = 0
self.finish_charging_counter = 0
# Measure voltage 100 times before confirming it full prevent false reading from spike
if (self.voltage_percent >=
(self.charge_stop_voltage + self.charge_stop_offset)):
if (self.spike_cnt > 100):
self.charge_OK = True
self.spike_cnt = 0
self.spike_cnt += 1
print "Done Charged Count:" + str(self.spike_cnt)
else:
print "Done Charge:" + str(self.charge_OK)
def magCB(self, msg):
raw = map(int, msg.data.split("/"))
self.mag_data = raw
def mag_drive(self):
x = 0
z = 0
mg = self.mag_data
j = [i for i in mg if i > 130] #threshold between magnetic tape or not
self.cnt = self.cnt + 1
if (len(j) > 8): #if magnetic detected more than 8 line
#stop
self.notify_pub.publish("0,0") #Boot = 0, Low_level_Drive = 0
x = 0
self.mag_end = True
print str(len(j)) + "----STOP, " + str(self.mag_end)
else:
self.notify_pub.publish("0,1") #Boot = 0, Low_level_Drive = 1
# line_trace speed set
x = 0.2
print str(len(j)) + "----MOVING, " + str(self.mag_end)
drive_msg = Twist()
drive_msg.linear = Vector3(x, 0, 0)
drive_msg.angular = Vector3(0, 0, 0)
self.mag_drive_pub.publish(drive_msg)
# Distance Checker, Checking whether 2 points are within distance tolerance or not
def dist_checker(self, a, b, dist_tolerance):
distance = np.sqrt((a.x - b.x)**2 + (a.y - b.y)**2)
if (distance <= dist_tolerance):
return True
else:
return False
# Stopping Vehicle
def stopping(self):
drive_msg = Twist()
drive_msg.linear = Vector3(0, 0, 0)
drive_msg.angular = Vector3(0, 0, 0)
self.drive_pub.publish(drive_msg)
# Convert Quaternion to Yaw Angle [deg]
def quaternion_to_yaw(self, q):
x, y, z, w = q.x, q.y, q.z, q.w
roll, pitch, yaw = np.rad2deg(tftr.euler_from_quaternion((x, y, z, w)))
return yaw
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.odom_topic = rospy.get_param("~odom_topic")
self.map_sub = rospy.Subscriber("/obstacles", OccupancyGrid,
self.obstacle_map_cb)
self.trajectory = LineTrajectory("/final_trajectory")
self.traj_sub = rospy.Subscriber("/trajectory/current",
PoseArray,
self.trajectory_cb,
queue_size=10)
self.goal_sub = rospy.Subscriber("/move_base_simple/goal",
PoseStamped,
self.goal_cb,
queue_size=10)
self.traj_pub = rospy.Publisher("/trajectory/final",
PoseArray,
queue_size=10)
self.odom_sub = rospy.Subscriber(self.odom_topic,
Odometry,
self.odom_cb,
queue_size=10)
self.rest_of_trajectory = LineTrajectory("/rest_of_trajectory")
self.planning_dist = 50.0
# FOR TESTING #
self.vis_pub = rospy.Publisher("/particles", Marker, queue_size=1)
self.nearest_pub = rospy.Publisher("/nearest", Marker, queue_size=1)
self.start_pose = None
self.goal_pose = None
#self.max_iterations = 1000 # Max size of RRT* tree before breaking.
self.dim = 2 # Configuration space dimensionality ([x, y] -> 2, [x, y, theta] -> 3)
self.gamma = 10 # I don't actually know what this is called lol, it's used in rrt_star.radius()
self.eta = .75
self.goal_bias = 0.15
self.map = None
# FOR TESTING PURPOSES ONLY -- set to true if you want to see the tree grow
self.enable_vis = True
def obstacle_map_cb(self, msg):
"""
Convert from OccupancyGrid to actual array.
"""
self.map = Map(msg, dilate=5)
if True:
rospy.loginfo("Obstacle map received! Origin: " +
str(msg.info.origin))
self.pick_intermediate_goal(self.map)
if True:
rospy.loginfo("start pose: " + str(self.start_pose))
rospy.loginfo("end pose: " + str(self.end_pose))
self.start_time = rospy.get_time()
self.time_thresh = 0.5
self.plan_path(self.start_pose, self.end_pose, self.map)
self.extend_trajectory(self.rest_of_trajectory)
# publish trajectory
self.traj_pub.publish(self.trajectory.toPoseArray())
# visualize trajectory Markers
self.trajectory.publish_viz()
def extend_trajectory(self):
"""
adds rest of trajectory after intermediate points to the planned obstacle avoiding trajectory
"""
traj_poses = self.trajectory.toPoseArray()
rest_poses = self.rest_of_trajectory.toPoseArray()
self.trajectory.fromPoseArray(traj_poses.poses + rest_poses.poses)
def pick_intermediate_goal(self):
"""
precondition: self.trajectory is populated w/new obstacle-agnostic trajectory
takes in current planned path, and obstacle map (self.map)
finds first segment within planning_dist that is blocked by obstacle + replans to endpoint of segment
return false if we can just use current traj
otherwise returns true&saves goal to self.end_pose, save rest of previous path to something else
"""
first_i, _ = self.get_closest_seg(self.start_pose)
if first_i + 1 >= self.num_pts:
raise Exception("something bad happened")
if self.map.generate_line_path_unlimited(
self.start_pose, self.trajectory.np_points[first_i + 1]):
safe_pt = first_i + 1
while self.trajectory.distance_along_trajectory(
safe_pt) - self.trajectory.distance_along_trajectory(
first_i) < self.planning_dist:
if safe_pt + 1 >= self.num_pts:
return False
if not self.map.generate_line_path_unlimited(
self.trajectory.np_points[safe_pt],
self.trajectory.np_points[safe_pt + 1]):
self.end_pose = self.trajectory.np_points(safe_pt + 1)
self.rest_of_trajectory.clear()
poseArr = self.trajectory.toPoseArray()
self.rest_of_trajectory.fromPoseArray(
poseArr.poses[safe_pt + 2:])
return True
safe_pt += 1
else:
self.end_pose = self.trajectory.np_points(first_i + 1)
self.rest_of_trajectory.clear()
poseArr = self.trajectory.toPoseArray()
self.rest_of_trajectory.fromPoseArray(poseArr.poses[first_i + 2:])
return True
def trajectory_cb(self, msg):
if self.trajectory.empty():
self.trajectory.fromPoseArray(msg)
self.trajectory.make_np_array()
#TODO: extend last pt??
self.num_pts = len(self.trajectory.np_points)
def next_pt(i):
return (i + 1) % self.num_pts
# n entries, one for each line segment + connect the last to the first point
self.trajectory_dists = np.array([
np.linalg.norm(self.trajectory.np_points[next_pt(i)] -
self.trajectory.np_points[i])**2
for i in range(self.num_pts)
])
# also vectors bw points can be calculated once only
self.wv = np.array([
self.trajectory.np_points[next_pt(i)] -
self.trajectory.np_points[i] for i in range(self.num_pts)
])
def get_closest_seg(self, pose_np):
"""
given 1x2 numpy pose array, find closest segment from nx2 trajectory.np_points
return 2 points from line segment (?)
may need to be optimized
let the point the robot is at be p, and for each line segment the start and end points be v and w respect~
"""
pv = pose_np - self.trajectory.np_points[:last_pt] # self.num_pts x 2
# projecting p onto line bw vw, constraining b/w 0 and 1 to keep within line segment
t = np.maximum(
0,
np.minimum(1, (pv * self.wv[:last_pt]).sum(axis=-1) /
self.trajectory_dists[:last_pt]))
proj = self.trajectory.np_points[:
last_pt] + self.wv[:
last_pt] * t[:, np.
newaxis]
# return index of minimum distance, and closest pt
minseg = np.argmin(np.linalg.norm(pose_np - proj, axis=1))
return minseg, proj[minseg]
def odom_cb(self, msg):
"""
"""
self.start_pose = np.array(
[msg.pose.pose.position.x, msg.pose.pose.position.y])
def goal_cb(self, msg):
self.end_pose = np.array([msg.pose.position.x, msg.pose.position.y])
if VERBOSE:
rospy.loginfo("start pose: " + str(self.start_pose))
rospy.loginfo("end pose: " + str(self.end_pose))
self.start_time = rospy.get_time()
self.time_thresh = 117.
self.plan_path(self.start_pose, self.end_pose, self.map)
def branched_from_existing_path(self, final_node, depth_underestimate=0):
'''
depth_underestimate: bias to add to depth. Because we don't
recursively update it, depth will steadily get less and less
accurate as the path is denser than expected due to rewirings.
Return a random free space that was near the existing
path up to the final_node
'''
def find_ancestor(node, n):
if n <= 0 or node.parent is None:
return node
else:
return find_ancestor(node.parent, n - 1)
# back_track_depth = np.random.choice(int(final_node.depth + depth_underestimate))
back_track_depth = np.random.choice(int(final_node.depth))
x_temp = find_ancestor(final_node, back_track_depth)
if x_temp.parent is None:
# No point in exploring directly from the origin
return self.map.sample_free_space()
extend = 1
frac = .1 * extend
while extend > 0:
rand = np.random.uniform() * 2 * np.pi
free_space_near_path = x_temp.value + extend * np.array(
[np.cos(rand), np.sin(rand)])
if self.map.check_free(free_space_near_path):
return free_space_near_path
else:
extend -= frac
# unlucky -- just give a random free point
return self.map.sample_free_space()
def find_nearby_connectable(self, x_nearest, x_new):
radius = self.rrt_star.radius(self.gamma, self.dim)
X_nearby = self.rrt_star.nearest_in_n_sphere(
x_new, 1) #min(radius, self.eta)) # Find all nodes near x_new
# Find all X_near nodes that can actually be connected to x_new
X_nearby_connectable = [x_nearest
] # List of candidate parent nodes for x_new
for node in X_nearby:
TEMP_PATH = self.map.generate_line_path_unlimited(
node.value, x_new)
if (TEMP_PATH is not None) and (
node != x_nearest
): # If the path exists between node and x_new, append node to list of candidate parents. (x_nearest starts in the list, so it's gonna be excluded here.)
X_nearby_connectable.append(node)
return X_nearby_connectable
def find_best_parent(self, X_nearby_connectable, x_new):
cost_min = np.inf
node_min = None
# Find which candidate parent node will impart the minimum cost to the x_new node when connected
for node in X_nearby_connectable:
TEMP_COST = node.cost + self.rrt_star.cost(node.value, x_new)
if TEMP_COST < cost_min:
cost_min = TEMP_COST
node_min = node
return cost_min, node_min
def rewire(self, cost_min, node_new, X_nearby_connectable):
# Rewire to minimize costs
for node in X_nearby_connectable:
potential_cost = cost_min + self.rrt_star.cost(
node_new.value, node.value)
if potential_cost < node.cost:
self.rrt_star.adoption(node, node_new)
def plan_path(self, start_point, end_point, map_obj):
"""
RRT*. Tries to find a collision free path from start to goal.
"""
# STUFF FOR TESTING
self.trajectory.clear()
if self.enable_vis:
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.1
self.vis_pub.publish(marker)
exploration_bias = 1.0 - self.goal_bias
final_node = None
num_existing_path_points_added = 0
self.rrt_star = RRTStar(Node(start_point))
self.max_iterations = self.rrt_star.max_size
while self.rrt_star.size <= self.max_iterations:
p = np.random.uniform()
if p < exploration_bias:
x_rand = self.map.sample_free_space()
else:
if final_node is None:
x_rand = end_point
else:
x_rand = self.branched_from_existing_path(
final_node,
depth_underestimate=num_existing_path_points_added)
num_existing_path_points_added += 1
x_nearest = self.rrt_star.nearest(
x_rand) # Find the nearest node to x_rand
path = self.map.generate_line_path(x_nearest.value,
x_rand,
eta=self.eta)
if path is not None: # no obstacles between x_nearest and x_rand
x_new = path[-1]
X_nearby_connectable = self.find_nearby_connectable(
x_nearest, x_new)
cost_min, node_min = self.find_best_parent(
X_nearby_connectable, x_new)
X_nearby_connectable.remove(
node_min
) # Remove x_new's parent node from the list of nearby nodes so it is not considered for rewiring
# Create the new node at x_new!
node_new = self.rrt_star.add_config(node_min, x_new)
if self.enable_vis:
# FOR TESTING ONLY #
# Code to publish marker for new node
###########################################################################################
TEMP = Point()
TEMP.x = x_new[0]
TEMP.y = x_new[1]
TEMP.z = .05
marker.points.append(TEMP)
TEMP = ColorRGBA()
TEMP.r = 1
TEMP.g = 0
TEMP.b = 0
TEMP.a = 1
marker.colors.append(TEMP)
self.vis_pub.publish(marker)
###########################################################################################
self.rewire(cost_min, node_new, X_nearby_connectable)
if np.allclose(node_new.value, end_point, .05, 0) and (
final_node is
None): #np.array_equal(node_new.value, end_point):
final_node = node_new
# reduce exploration bias so that we reinforce the existing path
exploration_bias = .5
if VERBOSE:
print("Path found!!!!")
print(final_node.cost)
if rospy.get_time() - self.start_time > self.time_thresh:
if VERBOSE:
print(self.rrt_star.size)
break
if final_node is not None:
if self.enable_vis:
marker = Marker()
marker.header.frame_id = "/map"
marker.type = marker.POINTS
marker.action = marker.ADD
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.points = []
marker.colors = []
def recur(node):
if self.enable_vis:
TEMP = Point()
TEMP.x = node.value[0]
TEMP.y = node.value[1]
TEMP.z = .05
marker.points.append(TEMP)
TEMP = ColorRGBA()
TEMP.r = 1
TEMP.g = 0
TEMP.b = 0
TEMP.a = 1
marker.colors.append(TEMP)
self.trajectory.points.append([node.value[0], node.value[1]])
parent = node.parent
if parent is not None:
recur(parent)
recur(final_node)
self.trajectory.points.reverse()
if self.enable_vis:
self.vis_pub.publish(marker)
if VERBOSE:
print(final_node.depth)
else:
# TODO:give best guess- find closest node to goal
if VERBOSE:
print("No path found! Please try again.")
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 TrajectoryWithSpeedLoader():
def __init__(self):
# Define Adjustaable Parameters
self.min_speed = rospy.get_param("~min_speed")
self.max_speed = rospy.get_param("~max_speed")
self.max_acceleration = rospy.get_param("~max_acceleration")
self.max_decceleration = rospy.get_param("~max_decceleration")
# Internal USE Variables - Do not modify without consultation
self.dynamic_model = utils.ApproximateDynamicsModel()
self.ackermann_model = utils.AckermannModel(0.36)
self.trajectory = LineTrajectory("/speed_track")
self.counts = 0
# Publishers
self.viz_track_pub = rospy.Publisher("/speed_track/viz",
MarkerArray,
queue_size=1)
self.traj_pub = rospy.Publisher(rospy.get_param("~pub_topic"),
PolygonStamped,
queue_size=1)
# Subscriber
self.traj_sub = rospy.Subscriber(rospy.get_param("~sub_topic"),
String,
self.trajCB,
queue_size=1)
print "Initialized. Waiting on messages..."
# need to wait a short period of time before publishing the first message
time.sleep(0.5)
# Callback Function for receiving path
def trajCB(self, msg):
path_id = msg.data
if (self.counts == 1):
self.generate_speed_profile(path_id)
self.counts = 0
self.counts += 1
# Generating Speed Profile
def generate_speed_profile(self, path):
self.trajectory.clear()
self.trajectory.load(path)
self.trajectory.make_np_array()
points = self.trajectory.np_points
local_polar_points = utils.piecewise_linear_local_waypoints_polar(
points)
speeds = np.zeros(points.shape[0])
for i in xrange(local_polar_points.shape[0]):
max_speed = self.dynamic_model.max_speed_dist(
local_polar_points[i, 0])
speeds[i] = max_speed
speeds[0] = 0 # speed of the beginning of the track
speeds[-1] = 0 # speed of the last of the track
speeds = np.clip(speeds, self.min_speed, self.max_speed)
# ensure that acceleration bounds are respected
for i in xrange(speeds.shape[0] - 1):
x = local_polar_points[i, 0]
v_i = speeds[i] # initial speed of a straight line
v_f = speeds[i + 1] # final speed of a straight line
a = (v_f**2 - v_i**2) / (2.0 * x) # vf^2 - vi^2 = 2ax
if a > self.max_acceleration:
a = self.max_acceleration
v_f = np.sqrt(v_i**2 + 2.0 * a * x)
speeds[i + 1] = v_f
# ensure that decceleration bounds are respected
for i in xrange(speeds.shape[0] - 2, -1, -1):
x = local_polar_points[i, 0]
v_i = speeds[i]
v_f = speeds[i + 1]
a = (v_f**2 - v_i**2) / (2.0 * x) # vf^2 - vi^2 = 2ax
if a < self.max_decceleration:
a = self.max_decceleration
v_i = np.sqrt(v_f**2 - 2.0 * a * x)
speeds[i] = v_i
# counting estimated travesal time
t_traversal = 0.0
for i in xrange(speeds.shape[0] - 1):
v, x = speeds[i], local_polar_points[i, 0]
t = x / v
t_traversal += t
print "Estimated traversal time: ", t_traversal
print speeds
# Publish the generated Speed Profile
self.trajectory.speed_profile = speeds.tolist()
self.viz_track_pub.publish(
viz_speed_track(self.trajectory.speed_profile,
self.trajectory.np_points))
self.traj_pub.publish(self.trajectory.toPolygon())
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]
