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 __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 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):
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 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(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 test_timer():
cyber.init()
ct = cyber_timer.Timer(10, fun, 0) # 10ms
ct.start()
time.sleep(1) # 1s
ct.stop()
print("+" * 80, "test set_option")
ct2 = cyber_timer.Timer() # 10ms
ct2.set_option(10, fun, 0)
ct2.start()
time.sleep(1) # 1s
ct2.stop()
cyber.shutdown()
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 test_init(self):
"""
Test cyber.
"""
self.assertTrue(cyber.init())
self.assertTrue(cyber.ok())
cyber.shutdown()
self.assertTrue(cyber.is_shutdown())
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 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)
def setUpClass(cls):
cyber.init()
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()
import sys
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)
if __name__ == '__main__':
cyber.init("talker_sample")
test_talker_class()
cyber.shutdown()
def perception_receiver(perception_channel):
"""publisher"""
cyber.init()
node = cyber.Node("perception")
node.create_reader(perception_channel, PerceptionObstacles, receiver)
node.spin()
def listener():
cyber.init()
test_node = cyber.Node("planning_acc_listener")
test_node.create_reader("/apollo/planning", planning_pb2.ADCTrajectory,
callback)
timer_s = args.t
timer_kind = 'TIMEOUT'
elif args.t == 0 and args.f > 0:
timer_s = args.f
timer_kind = 'FATAL'
else:
if args.t == 0 and args.f == 0:
timer_s = 0
timer_kind = 'OK'
timer_set = False
print("Starting infinite OK message loop")
else:
print("Only one timer can be non-zero value!")
sys.exit()
cyber.init("Dump_IPC_Supervisor")
dump_sv_node = cyber.Node("dump_sv_talker")
writer = dump_sv_node.create_writer("supervisor/general", SV_info, 5)
begin_time = time.time()
while not(cyber.is_shutdown()):
current_time = time.time()
msg = sv_decision()
msg.header.timestamp_sec = current_time
if current_time - begin_time < timer_s or not(timer_set):
msg.status = 1
msg.message = "It's okay, chill"
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 listener():
cyber.init()
test_node = cyber.Node("chassis_acc_listener")
test_node.create_reader("/apollo/canbus/chassis", chassis_pb2.Chassis,
callback)
