def spin(self):
self.cyber_node.create_reader('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
self.localization_callback)
# wait for first localization message
while not cyber.is_shutdown() and not self.localization_received:
time.sleep(0.1)
while not cyber.is_shutdown():
now = cyber_time.Time.now().to_sec()
self.publish_trajectory()
if (self.at_end_of_trajectory()):
if (self.params.loop):
self.reset_trajectory_tracking()
else:
sys.exit(0)
sleep_time = 1.0 / self.params.rate - (
cyber_time.Time.now().to_sec() - now)
if sleep_time > 0.0:
time.sleep(sleep_time)
def gnss_simulation(q):
cyber.init()
#rospy.init_node("pose_offline",anonymous=True)
node = cyber.Node("pose_offline")
#localization_pub = rospy.Publisher(
# "/apollo/localization/pose",localization_pb2.LocalizationEstimate,queue_size=1)
localization_pub = node.create_writer(
"/apollo/localization/pose", localization_pb2.LocalizationEstimate)
#generate localization info
LocalizationEstimate = localization_pb2.LocalizationEstimate()
#send pose to /apollo/localization/pose
count = 0
#r = rospy.Rate(20)
#while not rospy.is_shutdown():
while not cyber.is_shutdown():
now = time.time()
#localization_pub.publish(gps_list(LocalizationEstimate,count,q))
localization_pub.write(gps_list(LocalizationEstimate, count, q))
sleep_time = 0.05 - (time.time() - now)
if sleep_time > 0:
time.sleep(sleep_time)
count += 1
def main(argv):
"""
Main node
"""
node = cyber.Node("rtk_recorder")
argv = FLAGS(argv)
log_dir = os.path.dirname(os.path.abspath(__file__)) + "/../../../data/log/"
if not os.path.exists(log_dir):
os.makedirs(log_dir)
Logger.config(
log_file=log_dir + "rtk_recorder.log",
use_stdout=True,
log_level=logging.DEBUG)
print("runtime log is in %s%s" % (log_dir, "rtk_recorder.log"))
record_file = log_dir + "/garage.csv"
recorder = RtkRecord(record_file)
atexit.register(recorder.shutdown)
node.create_reader('/apollo/canbus/chassis', chassis_pb2.Chassis,
recorder.chassis_callback)
node.create_reader('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
recorder.localization_callback)
while not cyber.is_shutdown():
time.sleep(0.002)
def loop(self):
while not cyber.is_shutdown():
# self.msg.steer_angle = random.random()
# self.msg.throttle = random.random()
self.writer.write(self.msg)
# ratio domain 100hz
time.sleep(0.01)
def main(argv):
"""
Main node
"""
node = cyber.Node("rtk_recorder")
argv = FLAGS(argv)
log_dir = os.path.dirname(os.path.abspath(
__file__)) + "/../../../data/log/"
if not os.path.exists(log_dir):
os.makedirs(log_dir)
Logger.config(
log_file=log_dir + "rtk_recorder.log",
use_stdout=True,
log_level=logging.DEBUG)
print("runtime log is in %s%s" % (log_dir, "rtk_recorder.log"))
record_file = log_dir + "/garage.csv"
recorder = RtkRecord(record_file)
atexit.register(recorder.shutdown)
node.create_reader('/apollo/canbus/chassis',
chassis_pb2.Chassis,
recorder.chassis_callback)
node.create_reader('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
recorder.localization_callback)
while not cyber.is_shutdown():
time.sleep(0.002)
def callback(self, data):
# TODO
# print(data.frame_no)
# TODO reshape
self.reshape(data)
# TODO publish, write to channel
if not cyber.is_shutdown():
self.write_to_channel()
def test_init(self):
"""
Test cyber.
"""
self.assertTrue(cyber.init())
self.assertTrue(cyber.ok())
cyber.shutdown()
self.assertTrue(cyber.is_shutdown())
def main(args):
if not os.path.exists(args.out_dir):
os.makedirs(args.out_dir)
meta_msg = g_message_manager.get_msg_meta_by_topic(args.topic)
if not meta_msg:
print('Unknown topic name: %s' % args.topic)
sys.exit(1)
cyber.init()
node = cyber.Node("transcribe_node")
node.create_reader(args.topic, meta_msg.msg_type, transcribe)
while not cyber.is_shutdown():
time.sleep(0.005)
def main():
"""
Main rosnode
"""
node = cyber.Node('ins_stat_publisher')
ins_stat = InsStat(node)
while not cyber.is_shutdown():
now = cyber_time.Time.now().to_sec()
ins_stat.publish_statmsg()
sleep_time = 0.5 - (cyber_time.Time.now().to_sec() - now)
if sleep_time > 0:
time.sleep(sleep_time)
def topic_publisher(topic, filename, period):
"""publisher"""
cyber.init()
node = cyber.Node("replay_file")
meta_msg = None
msg = None
if not topic:
print("Topic not specified, start to guess")
meta_msg, msg = g_message_manager.parse_file(filename)
topic = meta_msg.topic()
else:
meta_msg = g_message_manager.get_msg_meta_by_topic(topic)
if not meta_msg:
print("Failed to find meta info for topic: %s" % (topic))
return False
msg = meta_msg.parse_file(filename)
if not msg:
print("Failed to parse file[%s] with topic[%s]" %
(filename, topic))
return False
if not msg or not meta_msg:
print("Unknown topic: %s" % topic)
return False
writer = node.create_writer(topic, meta_msg.msg_type)
if period == 0:
while not cyber.is_shutdown():
raw_input("Press any key to publish one message...")
writer.write(msg)
print("Topic[%s] message published" % topic)
else:
print("started to publish topic[%s] message with rate period %s" %
(topic, period))
while not cyber.is_shutdown():
writer.write(msg)
print(msg)
time.sleep(period)
def main():
"""
Main rosnode
"""
node = cyber.Node('odom_publisher')
odom = OdomPublisher(node)
node.create_reader('/apollo/localization/pose', localization_pb2.LocalizationEstimate, odom.localization_callback)
while not cyber.is_shutdown():
now = cyber_time.Time.now().to_sec()
odom.publish_odom()
sleep_time = 0.01 - (cyber_time.Time.now().to_sec() - now)
if sleep_time > 0:
time.sleep(sleep_time)
def main():
"""
Main cyber
"""
parser = argparse.ArgumentParser(
description='Generate Planning Trajectory from Data File')
parser.add_argument(
'-s',
'--speedmulti',
help='Speed multiplier in percentage (Default is 100) ',
type=float,
default='100')
parser.add_argument('-c',
'--complete',
help='Generate complete path (t/F)',
default='F')
parser.add_argument('-r',
'--replan',
help='Always replan based on current position(t/F)',
default='F')
args = vars(parser.parse_args())
node = cyber.Node("rtk_player")
Logger.config(log_file=os.path.join(APOLLO_ROOT,
'data/log/rtk_player.log'),
use_stdout=True,
log_level=logging.DEBUG)
record_file = os.path.join(APOLLO_ROOT, 'data/log/garage.csv')
player = RtkPlayer(record_file, node, args['speedmulti'],
args['complete'].lower(), args['replan'].lower())
atexit.register(player.shutdown)
node.create_reader('/apollo/canbus/chassis', chassis_pb2.Chassis,
player.chassis_callback)
node.create_reader('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
player.localization_callback)
node.create_reader('/apollo/control/pad', pad_msg_pb2.PadMessage,
player.padmsg_callback)
while not cyber.is_shutdown():
now = time.time()
player.publish_planningmsg()
sleep_time = 0.1 - (time.time() - now)
if sleep_time > 0:
time.sleep(sleep_time)
def plan(self):
print(hasattr(self, 'data'))
while not cyber.is_shutdown():
print("in plan ********************************************")
if not hasattr(self, 'data'):
time.sleep(0.2)
print("no data sleep firstly")
continue
sx = self.data.start_point.x # start x position [m]
sy = self.data.start_point.y # start y positon [m]
gx = self.data.end_point.x # goal x position [m]
gy = self.data.end_point.y # goal y position [m]
grid_size = 0.051 # potential grid size [m]
robot_radius = 0.0725 # robot radius [m]
print('start point,{},{} goal point,{}, {}'.format(sx, sy, gx, gy))
ox, oy = [], []
for obstacle in self.data.obs_points:
ox.append(obstacle.x)
oy.append(obstacle.y)
print('obstacle information, ox:{}, oy:{} '.format(ox, oy))
# ox = [-0.03464780002832413] # obstacle x position list [m]
# oy = [0.6250196099281311] # obstacle y position list [m]
# if show_animation:
# plt.grid(True)
# plt.axis("equal")
# path generation
rx, ry = [], []
start = time.time()
rx, ry = potential_field_planning(
sx, sy, gx, gy, ox, oy, grid_size, robot_radius)
end = time.time()
print('time cost: {}'.format(end - start))
print('rx,{}, ry.{}'.format(rx, ry))
self.planning_path = Trajectory()
if not rx:
print("Failed to find a path")
else:
point = Point()
for i, _ in enumerate(rx):
point.x = rx[i]
point.y = ry[i]
self.planning_path.point.append(point)
print('trajectory,{}'.format(self.planning_path))
self.write_to_channel()
def topic_publisher(topic, filename, period):
"""publisher"""
cyber.init()
node = cyber.Node("replay_file")
meta_msg = None
msg = None
if not topic:
print "Topic not specified, start to guess"
meta_msg, msg = g_message_manager.parse_file(filename)
topic = meta_msg.topic()
else:
meta_msg = g_message_manager.get_msg_meta_by_topic(topic)
if not meta_msg:
print("Failed to find meta info for topic: %s" % (topic))
return False
msg = meta_msg.parse_file(filename)
if not msg:
print("Failed to parse file[%s] with topic[%s]" % (filename,
topic))
return False
if not msg or not meta_msg:
print("Unknown topic: %s" % topic)
return False
writer = node.create_writer(topic, meta_msg.msg_type)
if period == 0:
while not cyber.is_shutdown():
raw_input("Press any key to publish one message...")
writer.write(msg)
print("Topic[%s] message published" % topic)
else:
print("started to publish topic[%s] message with rate period %s" %
(topic, period))
while not cyber.is_shutdown():
writer.write(msg)
time.sleep(period)
def main():
"""
Main cyber
"""
parser = argparse.ArgumentParser(
description='Generate Planning Trajectory from Data File')
parser.add_argument(
'-s',
'--speedmulti',
help='Speed multiplier in percentage (Default is 100) ',
type=float,
default='100')
parser.add_argument(
'-c', '--complete', help='Generate complete path (t/F)', default='F')
parser.add_argument(
'-r',
'--replan',
help='Always replan based on current position(t/F)',
default='F')
args = vars(parser.parse_args())
node = cyber.Node("rtk_player")
Logger.config(
log_file=os.path.join(APOLLO_ROOT, 'data/log/rtk_player.log'),
use_stdout=True,
log_level=logging.DEBUG)
record_file = os.path.join(APOLLO_ROOT, 'data/log/garage.csv')
player = RtkPlayer(record_file, node, args['speedmulti'],
args['complete'].lower(), args['replan'].lower())
atexit.register(player.shutdown)
node.create_reader('/apollo/canbus/chassis', chassis_pb2.Chassis,
player.chassis_callback)
node.create_reader('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
player.localization_callback)
node.create_reader('/apollo/control/pad', pad_msg_pb2.PadMessage,
player.padmsg_callback)
while not cyber.is_shutdown():
now = time.time()
player.publish_planningmsg()
sleep_time = 0.1 - (time.time() - now)
if sleep_time > 0:
time.sleep(sleep_time)
def main(args):
drive_event_meta_msg = g_message_manager.get_msg_meta_by_topic(
args.drive_event_topic)
if not drive_event_meta_msg:
print('Unknown drive_event topic name: %s' % args.drive_event_topic)
sys.exit(1)
localization_meta_msg = g_message_manager.get_msg_meta_by_topic(
args.localization_topic)
if not localization_meta_msg:
print('Unknown localization topic name: %s' % args.localization_topic)
sys.exit(1)
cyber.init()
node = cyber.Node("derive_event_node")
node.create_reader(localization_meta_msg.topic,
localization_meta_msg.msg_type, OnReceiveLocalization)
writer = node.create_writer(drive_event_meta_msg.topic,
drive_event_meta_msg.msg_type)
seq_num = 0
while not cyber.is_shutdown():
event_type = input(
"Type in Event Type('d') and press Enter (current time: " +
str(datetime.datetime.now()) + ")\n>")
event_type = event_type.strip()
if len(event_type) != 1 or event_type[0].lower() != 'd':
continue
current_time = cyber_time.Time.now().to_sec()
event_str = None
while not event_str:
event_str = input("Type Event:>")
event_str = event_str.strip()
event_msg = drive_event_meta_msg.msg_type()
event_msg.header.timestamp_sec = current_time
event_msg.header.module_name = 'drive_event'
seq_num += 1
event_msg.header.sequence_num = seq_num
event_msg.header.version = 1
event_msg.event = event_str
if g_localization:
event_msg.location.CopyFrom(g_localization.pose)
writer.write(event_msg)
time_str = datetime.datetime.fromtimestamp(current_time).strftime(
"%Y%m%d%H%M%S")
filename = os.path.join(args.dir, "%s_drive_event.pb.txt" % time_str)
proto_utils.write_pb_to_text_file(event_msg, filename)
print('Logged to rosbag and written to file %s' % filename)
time.sleep(0.1)
def perception_publisher(perception_channel, files, period):
"""publisher"""
cyber.init()
node = cyber.Node("perception")
writer = node.create_writer(perception_channel, PerceptionObstacles)
perception_description = load_descrptions(files)
sleep_time = int(1.0 / period) # 10hz
global _s_delta_t
_s_delta_t = period
perception = None
while not cyber.is_shutdown():
perception = generate_perception(perception_description, perception)
print str(perception)
writer.write(perception)
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)
def __init__(self, node):
self.node = node
self.msg = Tags()
self.localization = localization()
self.position_0 = pos()
self.position_1 = pos()
self.pos = pos()
self.kalman_pos = pos()
self.kalman_pos.x = 0
self.kalman_pos.y = 0
self.kalman_pos.z = 0
self.kalman_pos.yaw = 0
self.obstacle_pos = pos()
self.marker_pos = {0: [1.0, 0], 1: [2.0, 0]}
self.node.create_reader("/localization/tag", Tags, self.callback)
self.node.create_reader("/realsense/pose", Pose, self.posecallback)
self.node.create_reader("/chassis", Chassis, self.Chassiscallback)
self.writer = self.node.create_writer("/localization", localization)
self.writer_kalman = self.node.create_writer(
"/localization/kalman_filter", pos)
self.writer_obstacle = self.node.create_writer(
"/localization/obstacle", pos)
self.start_yaw = 0
self.init_flag = 0
self.obstacle_flag = 0
self.speed = 0
self.yaw = 0
self.kalman = kalman_filter()
while not cyber.is_shutdown():
time.sleep(0.05)
self.localization_with_odometer_calculation(
self.speed, self.yaw, 0.05)
# self.localization_with_kalman();
self.kalman.localization_with_kalman(self.speed, self.yaw)
self.writer.write(self.localization)
self.writer_kalman.write(self.kalman.kalman_pos)
if self.obstacle_flag:
self.obstacle_flag = 0
else:
self.obstacle_pos.x = -1
self.obstacle_pos.y = -1
self.obstacle_pos.z = -1
self.obstacle_pos.yaw = -1
self.writer_obstacle.write(self.kalman.obstacle_pos)
print(self.localization)
def test_client_class():
"""
Client send request
"""
node = cyber.Node("client_node")
client = node.create_client("server_01", ChatterBenchmark,
ChatterBenchmark)
req = ChatterBenchmark()
req.content = "clt:Hello service!"
req.seq = 0
count = 0
while not cyber.is_shutdown():
time.sleep(1)
count = count + 1
req.seq = count
print "-" * 80
response = client.send_request(req)
print "get Response [ ", response, " ]"
def test_talker_class():
"""
Test talker.
"""
msg = SimpleMessage()
msg.text = "talker:send Alex!"
msg.integer = 0
test_node = cyber.Node("node_name1")
g_count = 1
writer = test_node.create_writer("channel/chatter", SimpleMessage, 6)
while not cyber.is_shutdown():
time.sleep(1)
g_count = g_count + 1
msg.integer = g_count
print "=" * 80
print "write msg -> %s" % msg
writer.write(msg)
def callback(self, data):
global obslist, planning_path, best_trajectory
obslist = []
print(" start!!")
# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
x = np.array([0.0, 0.0, math.pi / 2.0, 0.3, 0.0])
# goal position [x(m), y(m)]
goal = np.array([data.end_point.x, data.end_point.y])
# obstacles [x(m) y(m), ....]
for point in data.obs_points:
obslist.append([point.x, point.y])
ob = np.matrix(obslist)
# input [forward speed, yawrate]
u = np.array([0.3, 0.0])
config = Config()
best_trajectory = np.array(x)
u, best_trajectory = dwa_control(x, u, config, goal, ob)
x = motion(x, u, config.dt)
self.planning_path = Trajectory()
if not best_trajectory.any():
print("Failed to find a path")
else:
for path_point in best_trajectory:
point_xy.x = path_point[0]
point_xy.y = path_point[1]
self.planning_path.point.append(point_xy)
if not cyber.is_shutdown() and self.planning_path:
self.writer.write(self.planning_path)
print("Done")
def callback(self, data):
global obslist, map_w, map_h, map_x_min, map_x_max, map_y_min, map_y_max
obslist = []
pStart_x = int(20 * data.start_point.x)
pStart_y = int(20 * data.start_point.y)
pEnd_x = int(20 * data.end_point.x)
pEnd_y = int(20 * data.end_point.y - 1)
pStart = Point(pStart_x, pStart_y)
pEnd = Point(pEnd_x, pEnd_y)
for point in data.obs_points:
obslist.append([int(20 * point.x), int(20 * point.y)])
for i in range(0):
obslist.append(
[random.uniform(2, pEnd.y),
random.uniform(2, pEnd.y)])
#time_start = time.time()
# 创建AStar对象,并设置起点终点
aStar = AStar(pStart, pEnd)
aStar.expansion(offset=9)
# 开始寻路
pathList = aStar.start()
#time_end = time.time()
#print('totally cost', time_end - time_start)
self.planning_path = Trajectory()
if not pathList:
print("Failed to find a path")
else:
for path_point in pathList:
point_xy.x = path_point.x * 0.05
point_xy.y = path_point.y * 0.05
self.planning_path.point.append(point_xy)
if not cyber.is_shutdown() and self.planning_path:
self.writer.write(self.planning_path)
def test_talker_class():
"""
test talker.
"""
msg = SimpleMessage()
msg.text = "talker:send Alex!"
msg.integer = 0
test_node = cyber.Node("node_name1")
g_count = 1
writer = test_node.create_writer("channel/chatter",
SimpleMessage, 6)
while not cyber.is_shutdown():
time.sleep(1)
g_count = g_count + 1
msg.integer = g_count
print "="*80
print "write msg -> %s" % msg
writer.write(msg)
def missioncallback(self, Point):
self.goal_x = int(Point.x)
self.goal_y = int(Point.y)
pathList = self.start(self.start_x, self.start_y, self.goal_x,
self.goal_y)
self.planning_path = Trajectory()
if not pathList:
print("Failed to find a path")
else:
for path_point in pathList:
point_xy.x = path_point[0]
point_xy.y = path_point[1]
self.planning_path.point.append(point_xy)
if not cyber.is_shutdown() and self.planning_path:
self.writer.write(self.planning_path)
def __init__(self, node):
self.node = node
self.msg = Tags()
self.localization = localization()
self.position_0 = pos()
self.position_1 = pos()
self.pos = pos()
self.marker_pos = {0: [1.0, 0], 1: [2.0, 0]}
self.node.create_reader("/localization/tag", Tags, self.callback)
self.node.create_reader("/realsense/pose", Pose, self.posecallback)
self.node.create_reader("/chassis", Chassis, self.chassiscallback)
self.writer = self.node.create_writer("/localization", localization)
self.start_yaw = 0
self.init_flag = 0
self.speed = 0
self.yaw = 0
while not cyber.is_shutdown():
time.sleep(0.05)
self.localization_with_odometer_calculation()
self.writer.write(self.localization)
print(self.localization)
def test_talker_class():
"""
Test talker.
"""
msg = ChatterBenchmark()
msg.content = "py:talker:send Alex!"
msg.stamp = 9999
msg.seq = 0
print(msg)
test_node = cyber.Node("node_name1")
g_count = 1
writer = test_node.create_writer("channel/chatter", ChatterBenchmark, 6)
while not cyber.is_shutdown():
time.sleep(1)
g_count = g_count + 1
msg.seq = g_count
msg.content = "I am python talker."
print("=" * 80)
print("write msg -> %s" % msg)
writer.write(msg)
def chassis_simulation(q, q2, q3):
cyber.init()
#rospy.init_node("chassis_offline", anonymous=True)
node = cyber.Node("chassis_offline")
#chassis_pub = rospy.Publisher(
# "/apollo/canbus/chassis", chassis_pb2.Chassis, queue_size=1)
chassis_pub = node.create_writer("/apollo/canbus/chassis",
chassis_pb2.Chassis)
# generate chassis info
chassis = chassis_pb2.Chassis()
# send pose to /apollo/canbus/chassis
count = 0
#r = rospy.Rate(20)
#while not rospy.is_shutdown():
while not cyber.is_shutdown():
now = time.time()
chassis_pub.write(chassis_data(chassis, count, q, q2, q3))
count += 1
sleep_time = 0.05 - (time.time() - now)
if sleep_time > 0:
time.sleep(sleep_time)
def loop(self):
try:
while not cyber.is_shutdown():
try :
c = sys.stdin.read(1)
if c:
print("Got character", repr(c))
#TODO 3 update your logic by keyboad
if c == 'w': self.hotkey_w()
elif c == 's': self.hotkey_s()
elif c == 'a': self.hotkey_a()
elif c == 'd': self.hotkey_d()
else:
print('please type one of " W A S D"/n')
print('*'*80)
print(self.msg)
#TODO 4 write control message to channel /control
self.writer.write(self.msg)
#ratio domain 100hz
time.sleep(0.01)
except IOError: pass
finally:
termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm)
fcntl.fcntl(fd, fcntl.F_SETFL, oldflags)
def mobileye_simulation(q):
cyber.init()
#rospy.init_node("mobileye_offline", anonymous=True)
node = cyber.Node("mobileye_offline")
#mobileye_pub = rospy.Publisher(
#"/apollo/sensor/mobileye", mobileye_pb2.Mobileye, queue_size=1)
mobileye_pub = node.create_writer("/apollo/sensor/mobileye",
mobileye_pb2.Mobileye)
# generate mobileye info
mobileye = mobileye_pb2.Mobileye()
# send pose to /apollo/drivers/mobileye
count = 0
#r = rospy.Rate(10)
while not cyber.is_shutdown():
now = time.time()
#mobileye_pub.publish(mobileye_data(mobileye,count,q))
mobileye_pub.write(mobileye_data(mobileye,count,q))
count += 1
sleep_time = 0.1 - (time.time() - now)
if sleep_time > 0:
time.sleep(sleep_time)
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Recode Analyzer is a tool to analyze record files.",
prog="main.py")
parser.add_argument(
"-f", "--file", action="store", type=str, required=True,
help="Specify the message file for sending.")
args = parser.parse_args()
cyber.init()
node = cyber.Node("perception_obstacle_sender")
perception_pub = node.create_writer(
"/apollo/perception/obstacles",
perception_obstacle_pb2.PerceptionObstacles)
perception_obstacles = perception_obstacle_pb2.PerceptionObstacles()
with open(args.file, 'r') as f:
text_format.Merge(f.read(), perception_obstacles)
while not cyber.is_shutdown():
now = cyber_time.Time.now().to_sec()
perception_obstacles = update(perception_obstacles)
perception_pub.write(perception_obstacles)
sleep_time = 0.1 - (cyber_time.Time.now().to_sec() - now)
if sleep_time > 0:
time.sleep(sleep_time)
cyber.shutdown()
def main(argv):
"""Main function"""
argv = FLAGS(argv)
print """
Keyboard Shortcut:
[q]: Quit Tool
[s]: Save Figure
[a]: Auto-adjust x, y axis to display entire plot
[x]: Update Figure to Display last few Planning Trajectory instead of all
[h][r]: Go back Home, Display all Planning Trajectory
[f]: Toggle Full Screen
[n]: Reset all Plots
[b]: Unsubscribe Topics
Legend Description:
Red Line: Current Planning Trajectory
Blue Line: Past Car Status History
Green Line: Past Planning Target History at every Car Status Frame
Cyan Dashed Line: Past Planning Trajectory Frames
"""
cyber.init()
planning_sub = cyber.Node("stat_planning")
fig = plt.figure()
if not FLAGS.show_st_graph:
ax1 = plt.subplot(2, 2, 1)
item1 = Xyitem(ax1, WindowSize, VehicleLength, "Trajectory", "X [m]",
"Y [m]")
ax2 = plt.subplot(2, 2, 2)
item2 = Item(ax2, "Speed", "Time [sec]", "Speed [m/s]", 0, 30)
ax3 = plt.subplot(2, 2, 3, sharex=ax2)
item3 = Item(ax3, "Curvature", "Time [sec]", "Curvature [m-1]", -0.2,
0.2)
ax4 = plt.subplot(2, 2, 4, sharex=ax2)
if not FLAGS.show_heading:
item4 = Item(ax4, "Acceleration", "Time [sec]",
"Acceleration [m/sec^2]", -5, 5)
else:
item4 = Item(ax4, "Heading", "Time [sec]", "Heading [radian]", -4,
4)
else:
ax1 = plt.subplot(2, 2, 1)
item1 = Stitem(ax1, "ST Graph", "Time [sec]", "S [m]")
ax2 = plt.subplot(2, 2, 2)
item2 = Stitem(ax2, "ST Graph", "Time [sec]", "S [m]")
ax3 = plt.subplot(2, 2, 3)
item3 = Stitem(ax3, "ST Graph", "Time [sec]", "S [m]")
ax4 = plt.subplot(2, 2, 4)
item4 = Stitem(ax4, "ST Graph", "Time [sec]", "S [m]")
plt.tight_layout(pad=0.20)
plt.ion()
plt.show()
plotter = Plotter(item1, item2, item3, item4, FLAGS.show_st_graph)
fig.canvas.mpl_connect('key_press_event', plotter.press)
planning_sub.create_reader('/apollo/planning', ADCTrajectory,
plotter.callback_planning)
if not FLAGS.show_st_graph:
localization_sub = cyber.Node("localization_sub")
localization_sub.create_reader('/apollo/localization/pose',
LocalizationEstimate,
plotter.callback_localization)
chassis_sub = cyber.Node("chassis_sub")
chassis_sub.create_reader('/apollo/canbus/chassis', Chassis,
plotter.callback_chassis)
while not cyber.is_shutdown():
ax1.draw_artist(ax1.patch)
ax2.draw_artist(ax2.patch)
ax3.draw_artist(ax3.patch)
ax4.draw_artist(ax4.patch)
with plotter.lock:
item1.draw_lines()
item2.draw_lines()
item3.draw_lines()
item4.draw_lines()
fig.canvas.blit(ax1.bbox)
fig.canvas.blit(ax2.bbox)
fig.canvas.blit(ax3.bbox)
fig.canvas.blit(ax4.bbox)
fig.canvas.flush_events()
seq = "%05d" % header.sequence_num
name = header.module_name
file_path = os.path.join(g_args.out_dir, seq + "_" + name + ".pb.txt")
print file_path
proto_utils.write_pb_to_text_file(proto_msg, file_path)
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="A tool to transcribe received protobuf messages into text files")
parser.add_argument(
"topic", action="store", help="the topic that you want to transcribe.")
parser.add_argument(
"--out_dir",
action="store",
default='.',
help="the output directory for the dumped file, the default value is current directory"
)
g_args = parser.parse_args()
if not os.path.exists(g_args.out_dir):
os.makedirs(g_args.out_dir)
meta_msg = g_message_manager.get_msg_meta_by_topic(g_args.topic)
if not meta_msg:
print "Unknown topic name: %s" % (g_args.topic)
sys.exit(0)
cyber.init()
node = cyber.Node("transcribe_node")
node.create_reader(g_args.topic, meta_msg.msg_type, transcribe)
while not cyber.is_shutdown():
time.sleep(0.005)
def main(argv):
"""Main function"""
argv = FLAGS(argv)
print """
Keyboard Shortcut:
[q]: Quit Tool
[s]: Save Figure
[a]: Auto-adjust x, y axis to display entire plot
[x]: Update Figure to Display last few Planning Trajectory instead of all
[h][r]: Go back Home, Display all Planning Trajectory
[f]: Toggle Full Screen
[n]: Reset all Plots
[b]: Unsubscribe Topics
Legend Description:
Red Line: Current Planning Trajectory
Blue Line: Past Car Status History
Green Line: Past Planning Target History at every Car Status Frame
Cyan Dashed Line: Past Planning Trajectory Frames
"""
cyber.init()
planning_sub = cyber.Node("stat_planning")
fig = plt.figure()
if not FLAGS.show_st_graph:
ax1 = plt.subplot(2, 2, 1)
item1 = Xyitem(ax1, WindowSize, VehicleLength, "Trajectory", "X [m]",
"Y [m]")
ax2 = plt.subplot(2, 2, 2)
item2 = Item(ax2, "Speed", "Time [sec]", "Speed [m/s]", 0, 30)
ax3 = plt.subplot(2, 2, 3, sharex=ax2)
item3 = Item(ax3, "Curvature", "Time [sec]", "Curvature [m-1]", -0.2,
0.2)
ax4 = plt.subplot(2, 2, 4, sharex=ax2)
if not FLAGS.show_heading:
item4 = Item(ax4, "Acceleration", "Time [sec]",
"Acceleration [m/sec^2]", -5, 5)
else:
item4 = Item(ax4, "Heading", "Time [sec]", "Heading [radian]", -4,
4)
else:
ax1 = plt.subplot(2, 2, 1)
item1 = Stitem(ax1, "ST Graph", "Time [sec]", "S [m]")
ax2 = plt.subplot(2, 2, 2)
item2 = Stitem(ax2, "ST Graph", "Time [sec]", "S [m]")
ax3 = plt.subplot(2, 2, 3)
item3 = Stitem(ax3, "ST Graph", "Time [sec]", "S [m]")
ax4 = plt.subplot(2, 2, 4)
item4 = Stitem(ax4, "ST Graph", "Time [sec]", "S [m]")
plt.tight_layout(pad=0.20)
plt.ion()
plt.show()
plotter = Plotter(item1, item2, item3, item4, FLAGS.show_st_graph)
fig.canvas.mpl_connect('key_press_event', plotter.press)
planning_sub.create_reader('/apollo/planning',
ADCTrajectory, plotter.callback_planning)
if not FLAGS.show_st_graph:
localization_sub = cyber.Node("localization_sub")
localization_sub.create_reader('/apollo/localization/pose',
LocalizationEstimate, plotter.callback_localization)
chassis_sub = cyber.Node("chassis_sub")
chassis_sub.create_reader('/apollo/canbus/chassis',
Chassis, plotter.callback_chassis)
while not cyber.is_shutdown():
ax1.draw_artist(ax1.patch)
ax2.draw_artist(ax2.patch)
ax3.draw_artist(ax3.patch)
ax4.draw_artist(ax4.patch)
with plotter.lock:
item1.draw_lines()
item2.draw_lines()
item3.draw_lines()
item4.draw_lines()
fig.canvas.blit(ax1.bbox)
fig.canvas.blit(ax2.bbox)
fig.canvas.blit(ax3.bbox)
fig.canvas.blit(ax4.bbox)
fig.canvas.flush_events()
def callback(self, pos):
global obslist, planning_path, best_trajectory
start_x = int(pos.x * scale)
start_y = int(pos.y * scale)
obslist = []
num_point = 0
minr = float("inf")
for i, point in enumerate(self.pathList):
r = math.sqrt((point[0] - start_x) ** 2 + (point[1] - start_y) ** 2)
if minr >= r:
minr = r
num_point = i
if (num_point - 20) <= 0:
num_point = 20
#规划预瞄点
f_point = self.pathList[num_point - 20]
self.goal = f_point
print("goal:", self.goal)
#将预瞄点坐标从地图坐标系转换到车身坐标系
f_point = [(f_point[0] - start_x) / scale, -((f_point[1] - start_y) / scale)]
yaw = math.pi - pos.yaw
x = f_point[0] * math.cos(yaw) + f_point[1] * math.sin(yaw)
y = f_point[1] * math.cos(yaw) - f_point[0] * math.sin(yaw)
print("trans-goal:", [x, y])
print(" planning start!")
# initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
x = np.array([0.0, 0.0, 0, 0.3, 0.0])
# goal position [x(m), y(m)]
goal = np.array([x, y])
ob = np.matrix(self.obstacleList)
# input [forward speed, yawrate]
u = np.array([0.3, 0.0])
config = Config()
best_trajectory = np.array(x)
u, best_trajectory = dwa_control(x, u, config, goal, ob)
x = motion(x, u, config.dt)
#接近终点
car_r = ((((start_x - self.pathList[0][0])**2 + (start_y - self.pathList[0][1])**2)**0.5) / scale)
if car_r <= 0.3:
u[0] = 0
self.planning_path = Trajectory()
self.speed = Control_Reference()
self.speed.vehicle_speed = u[0]
if not best_trajectory.any():
print("Failed to find a path")
else:
for path_point in best_trajectory:
point_xy.x = path_point[0]
point_xy.y = path_point[1]
self.planning_path.point.append(point_xy)
point_xy.x = self.goal[0]
point_xy.y = self.goal[1]
self.planning_path.point.append(point_xy)
if not cyber.is_shutdown() and self.planning_path:
self.writer.write(self.planning_path)
self.vwriter.write(self.speed)
print("planning done")
trajGenerator = CarTrajectoryGenerator()
if (params.trajectory_type == "8type"):
trajectory = trajGenerator.generate_8type_traj_using_4_clothoids(car,
params.min_turn_radius,
params.max_vel,
params.acc,
params.dec,
frequency=CTRL_FREQUENCY)
else:
trajectory = trajGenerator.generate_0type_traj_using_4_clothoids(car,
params.min_turn_radius,
params.max_vel,
params.acc,
params.dec,
frequency=CTRL_FREQUENCY)
while (not localization_recived and not cyber.is_shutdown()):
time.sleep(0.2)
cyber.shutdown()
if not localization_recived:
sys.exit("Cyber had been shutted down before localization message was recived!\n" + \
"Trajectory was not saved!")
trajectory.transform_to(car.x, car.y, car.yaw, car.z)
if not trajectory_file_handler.save_trajectory(trajectory, params.rtk_player_compatible):
sys.exit("Trajectory was not saved!")
