def test_read_write(self):
"""
Unit test of reader.
"""
self.assertTrue(cyber.ok())
# Read.
reader_node = cyber.Node("listener")
reader = reader_node.create_reader("channel/chatter", SimpleMessage,
self.callback)
self.assertEqual(reader.name, "channel/chatter")
self.assertEqual(reader.data_type, SimpleMessage)
self.assertEqual(SimpleMessage.DESCRIPTOR.full_name,
"apollo.common.util.test.SimpleMessage")
# Write.
msg = SimpleMessage()
msg.text = "talker:send Alex!"
msg.integer = 0
self.assertTrue(cyber.ok())
writer_node = cyber.Node("writer")
writer = writer_node.create_writer("channel/chatter", SimpleMessage, 7)
self.assertEqual(writer.name, "channel/chatter")
self.assertEqual(writer.data_type,
"apollo.common.util.test.SimpleMessage")
self.assertTrue(writer.write(msg))
# Wait for data to be processed by callback function.
time.sleep(0.1)
def __init__(self):
cyber.init()
self.api_node = cyber.Node("base_api")
self.loc = {"x": 0.0, "y": 0.0, "z": 0.0, "heading": 0.0}
self.veh = {
"speed": 0.0,
"steering": 0.0,
"throttle": 0.0,
"brake": 0.0
}
self.gps = {"latitude": 0.0, "longitude": 0.0}
self.routing_writer = self.api_node.create_writer(
'/apollo/routing_request', RoutingRequest)
self.control_writer = self.api_node.create_writer(
'/apollo/control', ControlCommand)
self.api_node.create_reader("/apollo/localization/pose",
LocalizationEstimate,
self._localization_callback)
self.api_node.create_reader("/apollo/canbus/chassis", Chassis,
self._canbus_callback)
self.api_node.create_reader("/apollo/sensor/gnss/best_pose",
GnssBestPose, self._gnss_callback)
self.api_node.create_reader("/apollo/routing_response",
RoutingResponse, self._routing_callback)
self.api_node.create_reader("/apollo/hmi/status", HMIStatus,
self._hmi_callback)
self.routing_info = {"length": 0}
self.hmi = {"map": "", "car": "", "mode": ""}
def listener():
cyber.init()
test_node = cyber.Node("control_listener")
test_node.create_reader("/apollo/control", control_cmd_pb2.ControlCommand,
callback)
test_node.spin()
cyber.shutdown()
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 main():
"""
Main rosnode
"""
cyber.init()
node = cyber.Node("mock_routing_requester")
sequence_num = 0
routing_request = routing_pb2.RoutingRequest()
routing_request.header.timestamp_sec = cyber_time.Time.now().to_sec()
routing_request.header.module_name = 'routing_request'
routing_request.header.sequence_num = sequence_num
sequence_num = sequence_num + 1
waypoint = routing_request.waypoint.add()
waypoint.pose.x = 587696.82286
waypoint.pose.y = 4141446.66696
waypoint.id = '1-1'
waypoint.s = 1
waypoint = routing_request.waypoint.add()
waypoint.pose.x = 586948.740120
waypoint.pose.y = 4141171.118641
waypoint.id = '1-1'
waypoint.s = 80
writer = node.create_writer('/apollo/routing_request',
routing_pb2.RoutingRequest)
time.sleep(2.0)
print("routing_request", routing_request)
writer.write(routing_request)
def __init__(self):
self.params = {
'throttle': 0.0,
'brake': 0.0,
'steering_eps': 0.0,
'left_turn_signal': False,
'right_turn_signal': False,
'low_beam': False,
'high_beam': False,
'horn': False,
'emergency_light': False,
'emergency_stop': False,
'gear': 0,
'exit': False
}
self.prepare_joy()
cyber.init()
node = cyber.Node("teleop_gamepad")
canbussub = node.create_reader('/apollo/canbus/chassis',
chassis_pb2.Chassis,
self.callback_canbus)
self.controlpub = node.create_writer('/apollo/control',
control_cmd_pb2.ControlCommand)
#Creating read and publish threads
self.ctl_pub_thread = threading.Thread(target=self.publish_control)
self.joy_read_thread = threading.Thread(target=self.joy_read)
self.ctl_pub_thread.start()
self.joy_read_thread.start()
def test_listener_class():
"""
Reader message.
"""
print("=" * 120)
test_node = cyber.Node("listener")
test_node.create_reader("channel/chatter", ChatterBenchmark, callback)
test_node.spin()
def channel_bw(channel_name, window_size):
rt = CyberChannelBw(window_size)
node_bw = cyber.Node("listener_node_bw")
node_bw.create_rawdata_reader(channel_name, rt.callback_bw)
print("reader to [%s]" % channel_name)
while not cyber.is_shutdown():
time.sleep(1.0)
rt.print_bw()
def test_service_class():
"""
Reader message.
"""
print("=" * 120)
node = cyber.Node("service_node")
r = node.create_service("server_01", ChatterBenchmark, ChatterBenchmark,
callback)
node.spin()
def test_params(self):
param1 = parameter.Parameter("author_name", "WanderingEarth")
param2 = parameter.Parameter("author_age", 5000)
param3 = parameter.Parameter("author_score", 888.88)
test_node = cyber.Node(PARAM_SERVICE_NAME)
srv = parameter.ParameterServer(test_node)
node_handle = cyber.Node("service_client_node")
clt = parameter.ParameterClient(test_node, PARAM_SERVICE_NAME)
clt.set_parameter(param1)
clt.set_parameter(param2)
clt.set_parameter(param3)
param_list = clt.get_paramslist()
self.assertEqual(3, len(param_list))
param_list = srv.get_paramslist()
self.assertEqual(3, len(param_list))
def test_listener_class():
"""
Reader message.
"""
print("=" * 120)
test_node = cyber.Node("listener")
test_node.create_reader("/apollo/sensor/camera/front_6mm/image", Image,
callback)
test_node.spin()
def listener_monitor():
"""
Reader message.
"""
print("=" * 120)
test_node = cyber.Node("listener_monitor")
test_node.create_reader("/apollo/monitor/system_status", SystemStatus,
callback_monitor)
test_node.spin()
def channel_echo(channel_name):
"""
Reader message.
"""
node_echo = cyber.Node("listener_node_echo")
echo_cb = CyberChannelecho(channel_name)
node_echo.create_rawdata_reader(channel_name, echo_cb.callback)
while not cyber.is_shutdown():
pass
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 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 = cyber_time.Time.now().to_sec()
player.publish_planningmsg()
sleep_time = 0.1 - (cyber_time.Time.now().to_sec() - 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 print_param_srv():
param1 = parameter.Parameter("author_name", "WanderingEarth")
param2 = parameter.Parameter("author_age", 5000)
param3 = parameter.Parameter("author_score", 888.88)
test_node = cyber.Node(PARAM_SERVICE_NAME)
srv = parameter.ParameterServer(test_node)
node_handle = cyber.Node("service_client_node")
clt = parameter.ParameterClient(test_node, PARAM_SERVICE_NAME)
clt.set_parameter(param1)
clt.set_parameter(param2)
clt.set_parameter(param3)
param_list = clt.get_paramslist()
print("clt param lst len is ", len(param_list))
for param in param_list:
print(param.debug_string())
print("")
param_list = srv.get_paramslist()
print("srv param lst len is ", len(param_list))
for param in param_list:
print(param.debug_string())
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 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 __init__(self):
#
# Creating Cyber RT Node, publishers and subscribers
#
cyber.init()
self.node = cyber.Node("api_node")
self.CURRENT_GLOBAL_STATUS = 0
self.DEBUG_MESSAGE = "no msg recieved"
self.params = {
"sound_on": False,
"debug_mode": False,
}
self.parameters_flag = False
self._create_callback_subscriber()
self._create_decision_subscriber()
self._create_preferences_publisher()
self._create_gnss_status_subscriber()
self._create_imu_status_subscriber()
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 += 1
req.seq = count
print("-" * 80)
response = client.send_request(req)
print("get Response [ ", response, " ]")
def main():
"""
Main function
"""
node = cyber.Node("data_collector")
data_collector = DataCollector(node)
plotter = Plotter()
node.create_reader('/apollo/localization/pose',
localization_pb2.LocalizationEstimate,
data_collector.callback_localization)
node.create_reader('/apollo/canbus/chassis', chassis_pb2.Chassis,
data_collector.callback_canbus)
print('Enter q to quit.')
print('Enter p to plot result from last run.')
print('Enter x to remove result from last run.')
print('Enter x y z, where x is acceleration command, ' +
'y is speed limit, z is decceleration command.')
print('Positive number for throttle and negative number for brake.')
while True:
cmd = input("Enter commands: ").split()
if len(cmd) == 0:
print('Quiting.')
break
elif len(cmd) == 1:
if cmd[0] == "q":
break
elif cmd[0] == "p":
print('Plotting result.')
if os.path.exists(data_collector.outfile):
plotter.process_data(data_collector.outfile)
plotter.plot_result()
else:
print('File does not exist: %s' % data_collector.outfile)
elif cmd[0] == "x":
print('Removing last result.')
if os.path.exists(data_collector.outfile):
os.remove(data_collector.outfile)
else:
print('File does not exist: %s' % date_collector.outfile)
elif len(cmd) == 3:
data_collector.run(cmd)
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 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():
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)
self.cmd.steer_angle = 57 * math.atan2(
2 * preview_y * 0.313,
(preview_x * preview_x + preview_y * preview_y))
if (abs(self.cmd.steer_angle) < 0.1):
self.cmd.steer_angle = 0
if self.cmd.steer_angle > 60:
self.cmd.steer_angle = 60
if self.cmd.steer_angle < -60:
self.cmd.steer_angle = -60
else:
self.cmd.steer_angle = 0
print(self.cmd.steer_angle)
pass
def longitude_controller(self, target_speed, speed_now):
# you should calculate throttle here
self.sum_error_longi += 0.05 * (0.5 - speed_now)
# + 6 + 8.0 * (target_speed - speed_now) + 5.0 * self.sum_error_longi
self.cmd.throttle = 0.5 * 30 + (
0.5 - speed_now) * 8 + 0.5 * self.sum_error_longi
pass
if __name__ == '__main__':
cyber.init()
# update node to your name
exercise_node = cyber.Node("control_node")
exercise = Control(exercise_node)
def callback(self, data):
# TODO print frame number
# TODO api to reshape image
# TODO publish, write compressed image
pass
def reshape(self, data):
"""api to reshape and encodes image, you can call self.reshape(data)"""
new_image = np.frombuffer(data, dtype=np.uint8)
img_param = [int(cv2.IMWRITE_JPEG_QUALITY), 20]
# https://stackoverflow.com/questions/50306863/valueerror-cannot-reshape-array-of-size-50176-into-shape-1-224-224-3
new_image = new_image.reshape((360, 640, 3))
img_encode = cv2.imencode('.jpeg', new_image, img_param)[1]
data_encode = np.array(img_encode)
return data_encode.tostring()
if __name__ == '__main__':
cyber.init()
# TODO update node to your gourp_name or other thing
exercise_node = cyber.Node("exercise1_node_name")
exercise = Exercise(exercise_node)
exercise_node.spin()
cyber.shutdown()
# TODO I
line_list, mean_x, mean_y = find_line_fit(yellow_line,
midpoint=car_mid_point,
nwindows=10,
margin=100)
self.planning_path = Trajectory()
#print("point size:",str(len(mean_y)))
if len(mean_y) > 0:
mean_x_real, mean_y_real = translation_view(
np.asarray(mean_x), np.asarray(mean_y))
for i, point in enumerate(mean_y_real):
point_xy = Point()
point_xy.x = mean_x_real[i]
point_xy.y = point
self.planning_path.point.append(point_xy)
if __name__ == '__main__':
cyber.init()
# TODO update node to your name
exercise_node = cyber.Node("to_mid_point")
exercise = Exercise(exercise_node)
exercise_node.spin()
cyber.shutdown()
def controlcallback(self, control):
if (control.throttle > 10):
self.accelorator = (control.throttle - 10) * 0.03
elif control.throttle > 0:
self.accelorator = 0
elif control.throttle < 0:
self.accelorator = control.throttle * 0.03
dt = 0.05
self.velocity += (self.accelorator - 2 * self.velocity) * dt
self.yaw += self.velocity * (control.steer_angle * 2 *
3.14159265358979 / 180) * dt
self.x_now -= self.velocity * math.cos(self.yaw) * dt
self.y_now -= self.velocity * math.sin(self.yaw) * dt
self.pos.x = self.x_now
self.pos.y = self.y_now
self.pos.z = 0
self.pos.yaw = self.yaw
self.chassis.speed = self.velocity
if __name__ == '__main__':
cyber.init()
exercise_node = cyber.Node("simulation_node")
exercise = Simulator(exercise_node)
exercise_node.spin()
cyber.shutdown()
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")
if __name__ == '__main__':
cyber.init()
cyber_node = cyber.Node("planning_dwa")
exercise = planning(cyber_node)
cyber_node.spin()
cyber.shutdown()
self.infer_boxs = LBox2DList()
if bboxs.shape[1] != 6:
print("No object found in this")
else:
labels = bboxs[:, 0].astype('int32')
scores = bboxs[:, 1].astype('float32')
boxes = bboxs[:, 2:].astype('int32')
for box, label, score in zip(boxes, labels, scores):
if score > 0.05 and ((box[2]-box[0])>30 and (box[3]-box[1])>30):
box_one = LBox2D()
box_one.xmin = int(box[0])
box_one.ymin = int(box[1])
box_one.xmax = int(box[2])
box_one.ymax = int(box[3])
box_one.label = str(label)
box_one.probability = score
self.infer_boxs.box.append(box_one)
if __name__ == '__main__':
cyber.init()
# TODO update node to your name
exercise_node = cyber.Node("inference_box")
exercise = Exercise(exercise_node)
exercise_node.spin()
cyber.shutdown()
