def prediction_publisher(prediction_topic, rate):
"""publisher"""
pub = rospy.Publisher(prediction_topic, String, queue_size=1)
rospy.init_node('prediction', anonymous=True)
rate = rospy.Rate(rate)
seq_num = 1
while not rospy.is_shutdown():
prediction = PredictionObstacles()
prediction.header.sequence_num = seq_num
prediction.header.timestamp_sec = rospy.Time.now().to_sec()
prediction.header.module_name = "prediction"
print str(prediction)
s = String()
s.data = prediction.SerializeToString()
pub.publish(s)
seq_num += 1
rate.sleep()
def prediction_publisher(prediction_channel, rate):
"""publisher"""
cyber.init()
node = cyber.Node("prediction")
writer = node.create_writer(prediction_channel, PredictionObstacles)
prediction = PredictionObstacles()
prediction.header.sequence_num = seq_num
prediction.header.timestamp_sec = cyber_time.Time.now().to_sec()
prediction.header.module_name = "prediction"
i=0
writer.write(prediction)
seq_num += 1
time.sleep(sleep_time)
def prediction_publisher(prediction_channel, rate):
"""publisher"""
cyber.init()
node = cyber.Node("prediction")
writer = node.create_writer(prediction_channel, PredictionObstacles)
sleep_time = 1.0 / rate
seq_num = 1
while not cyber.is_shutdown():
prediction = PredictionObstacles()
prediction.header.sequence_num = seq_num
prediction.header.timestamp_sec = cyber_time.Time.now().to_sec()
prediction.header.module_name = "prediction"
print(str(prediction))
writer.write(prediction)
seq_num += 1
time.sleep(sleep_time)
second_last = points[point_num - 2]
last_point = points[point_num - 1]
x_diff = last_point.x - second_last.x
y_diff = last_point.y - second_last.y
t_diff = last_point.t - second_last.t
delta_diff = math.sqrt(x_diff ** 2 + y_diff ** 2)
cur_len = trajectory_length
while cur_len < min_length:
last_point.x += x_diff
last_point.y += y_diff
last_point.t += t_diff
p = points.add()
p.CopyFrom(last_point)
last_point = p
cur_len += delta_diff
return prediction
if __name__ == '__main__':
parser = argparse.ArgumentParser(description="extend prediction trajectory")
parser.add_argument("prediction", action="store", type=str, help="set the prediction file")
parser.add_argument("-p", "--period", action="store", type=float, default=10.0,
help="set the prediction period")
parser.add_argument("-d", "--distance", action="store", type=float, default=70.0,
help="set the prediction distance")
args = parser.parse_args()
prediction_data = proto_utils.get_pb_from_file(args.prediction,
PredictionObstacles())
extended_prediction = extend_prediction(prediction_data, args.distance, args.period)
print extended_prediction
y_diff = last_point.path_point.y - second_last.path_point.y
t_diff = last_point.path_point.theta - second_last.path_point.theta
delta_diff = math.sqrt(x_diff ** 2 + y_diff ** 2)
cur_len = trajectory_length
while cur_len < min_length and abs(cur_len) > 0.000001:
last_point.path_point.x += x_diff
last_point.path_point.y += y_diff
last_point.path_point.theta += t_diff
p = points.add()
p.CopyFrom(last_point)
last_point = p
cur_len += delta_diff
return prediction
if __name__ == '__main__':
parser = argparse.ArgumentParser(
description="extend prediction trajectory")
parser.add_argument("prediction", action="store",
type=str, help="set the prediction file")
parser.add_argument("-p", "--period", action="store", type=float, default=10.0,
help="set the prediction period")
parser.add_argument("-d", "--distance", action="store", type=float, default=70.0,
help="set the prediction distance")
args = parser.parse_args()
prediction_data = proto_utils.get_pb_from_file(
args.prediction, PredictionObstacles())
extended_prediction = extend_prediction(
prediction_data, args.distance, args.period)
print(extended_prediction)
prediction.header.module_name = "prediction"
i=0
writer.write(prediction)
seq_num += 1
time.sleep(sleep_time)
if __name__ == '__main__':
parser = argparse.ArgumentParser(description="create empty prediction message",
prog="replay_prediction.py")
parser.add_argument("-c", "--channel", action="store", type=str, default="/apollo/prediction",
help="set the prediction channel")
parser.add_argument("-r", "--rate", action="store", type=int, default=10,
help="set the prediction channel publish time duration")
args = parser.parse_args()
cyber.init()
node = cyber.Node("prediction")
node.create_reader("/apollo/perception/obstacles", PerceptionObstacles, receive_perception_obstacles)
writer = node.create_writer(args.channel, PredictionObstacles)
sleep_time = 1.0 / args.rate
seq_num = 1
while not cyber.is_shutdown():
prediction = PredictionObstacles()
prediction.header.sequence_num = seq_num
prediction.header.timestamp_sec=cyber_time.Time.now().to_sec()
prediction.header.module_name="fake_prediction"
for obstacle in obstacles_:
prediction_obstacle=prediction.prediction_obstacle.add()
prediction_obstacle.perception_obstacle=obstacle
writer.write(prediction)
time.sleep(sleep_time)