class LoadTrajectory(object):
""" Loads a trajectory from the file system and publishes it to a ROS topic.
"""
def __init__(self):
self.path = rospy.get_param("~trajectory")
self.should_publish = bool(rospy.get_param("~publish"))
self.pub_topic = rospy.get_param("~topic")
# initialize and load the trajectory
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
if self.should_publish:
self.traj_pub = rospy.Publisher(self.pub_topic,
PolygonStamped,
queue_size=1)
# need to wait a short period of time before publishing the first message
time.sleep(0.5)
# visualize the loaded trajectory for 5 seconds
self.trajectory.publish_viz(duration=3.0)
# send the trajectory
if self.should_publish:
self.publish_trajectory()
def publish_trajectory(self):
print "Publishing trajectory to:", self.pub_topic
self.traj_pub.publish(self.trajectory.toPolygon())
class LoadTrajectory(object):
""" Loads a trajectory from the file system and publishes it to a ROS topic.
"""
def __init__(self):
self.path = rospy.get_param("~trajectory")
# initialize and load the trajectory
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
self.pub_topic = "/trajectory/current"
self.traj_pub = rospy.Publisher(self.pub_topic,
PoseArray,
queue_size=1)
# need to wait a short period of time before publishing the first message
rospy.loginfo("waiting to publish trajectory")
time.sleep(3.0)
# visualize the loaded trajectory
self.trajectory.publish_viz()
# send the trajectory
self.publish_trajectory()
def publish_trajectory(self):
print "Publishing trajectory to:", self.pub_topic
self.traj_pub.publish(self.trajectory.toPoseArray())
class LoadTrajectory(object):
""" Loads a trajectory from the file system and publishes it to a ROS topic.
"""
def __init__(self):
self.path = rospy.get_param(
"~trajectory",
default=
"/home/nvidia/workspace/catkin_ws/src/ddl_0725/pf_localization/trajectories/2018-08-09-16-16-58.traj"
)
self.should_publish = bool(rospy.get_param("~publish", default=True))
self.pub_topic = rospy.get_param(
"~topic", default="/loaded_trajectory/recorded_path")
# initialize and load the trajectory
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
if self.should_publish:
self.traj_pub = rospy.Publisher(self.pub_topic, Path, queue_size=1)
# need to wait a short period of time before publishing the first message
time.sleep(0.5)
# visualize the loaded trajectory for 5 seconds
self.trajectory.publish_viz(duration=3.0)
self.duration = rospy.Duration(1)
self.times = 0
self.srv_get_raw_path_ = rospy.Service('/get_raw_path', GetRawPath,
self.handle_get_raw_path)
# send the trajectory
if self.should_publish:
self.publish_trajectory()
def publish_trajectory(self):
print "Publishing trajectory to:", self.pub_topic
while not rospy.is_shutdown():
if self.traj_pub.get_num_connections() > 0:
self.traj_pub.publish(self.trajectory.toPath())
time.sleep(self.duration.to_sec())
self.times = self.times + 1
if self.times > 20:
break
def handle_get_raw_path(self, req):
if not isinstance(self.trajectory, LineTrajectory):
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
return GetRawPathResponse(self.trajectory.toPath())
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 LoadTrajectory(object):
""" Loads a trajectory from the file system and publishes it to a ROS topic.
"""
def __init__(self):
self.path = rospy.get_param(
"~trajectory",
default=
"/home/lenovo/zh/workspace/catkin_ws/src/ddl_0725/localization_0725/trajectories/2018-08-09-16-16-58.traj"
)
self.should_publish = bool(rospy.get_param("~publish", default=True))
self.pub_topic = rospy.get_param(
"~topic", default="/loaded_trajectory/recorded_path")
# initialize and load the trajectory
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
if self.should_publish:
self.traj_pub = rospy.Publisher(self.pub_topic, Path, queue_size=1)
# need to wait a short period of time before publishing the first message
time.sleep(0.5)
# visualize the loaded trajectory for 5 seconds
self.trajectory.publish_viz(duration=3.0)
self.duration = rospy.Duration(1)
# send the trajectory
if self.should_publish:
self.publish_trajectory()
def publish_trajectory(self):
print "Publishing trajectory to:", self.pub_topic
while not rospy.is_shutdown():
self.traj_pub.publish(self.trajectory.toPath())
time.sleep(self.duration.to_sec())
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 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())
