def __init__(self, carla_world, params): """ Constructor :param carla_world: carla world object :type carla_world: carla.World :param params: dict of parameters, see settings.yaml :type params: dict """ cyber.init() self.cyber_node = cyber.Node('carla_cyber_client_node') self.params = params self.use_object_sensor = self.params.get("object_sensor") is not None super(CarlaCyberBridge, self).__init__(carla_id=0, carla_world=carla_world, frame_id='/map') self.timestamp_last_run = 0.0 self.tf_to_publish = [] self.msgs_to_publish = [] self.actor_list = [] self.map = self.carla_world.get_map() # register callback to create/delete actors self.update_child_actors_lock = threading.Lock() self.carla_world.on_tick(self._carla_update_child_actors) # register callback to update actors self.update_lock = threading.Lock() self.carla_world.on_tick(self._carla_time_tick) self.writers = {}
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(): """ Main rosnode """ cyber.init() node = cyber.Node("mock_routing_requester") sequence_num = 0 routing_request = routing_pb2.RoutingRequest() routing_request.header.timestamp_sec = time.time() 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 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(): cyber.init("fault_injector") fault_injector_node = cyber.Node("fault_injector") writer = fault_injector_node.create_writer( "/apollo/sensor/camera/front_6mm/image", Image) for image_file in read_image_list(): image = cv2.imread(DATASET_FOLDER + IMAGES_FOLDER + image_file) image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) #image = cv2.resize(image, (1920, 1080), 1.0, 1.0, cv2.INTER_CUBIC) image = adjust_contrast_brightness(image, 2.0, 50.0) image_frame = np.zeros((1080, 1920, 3), dtype="uint8") image_frame[OFFSET_Y:OFFSET_Y + image.shape[0], OFFSET_X:OFFSET_X + image.shape[1]] = image proto_image = Image() proto_image.header.frame_id = "front_6mm" proto_image.header.timestamp_sec = cyber.time.time() proto_image.measurement_time = cyber.time.time() proto_image.width = image_frame.shape[1] proto_image.height = image_frame.shape[0] proto_image.encoding = "rgb8" proto_image.step = 3 * image_frame.shape[1] proto_image.data = image_frame.tobytes() writer.write(proto_image) time.sleep(0.2) cyber.shutdown()
def main(): print("potential_field_planning start") cyber.init() planning_node = cyber.Node("planning_potential") _ = planning(planning_node) planning_node.spin() cyber.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 subscreber_ctrl_pad(client, q_drivemode, q_drivemode2control): cyber.init() pad = control_pad(client, q_drivemode, q_drivemode2control) #rospy.init_node('simulation_control_pad', anonymous=True) node = cyber.Node("simulation_control_pad") #rospy.Subscriber('/apollo/control/pad', pad_msg_pb2.PadMessage, # pad.sim_control_pad_callback) node.create_reader('/apollo/control/pad', pad_msg_pb2.PadMessage, pad.sim_control_pad_callback) #rospy.spin() node.spin()
def subscreber_ctrl_cmd(client, q_speed, q_drivemode2control, q_gear): cyber.init() control = control_obj(client, q_speed, q_drivemode2control, q_gear) #rospy.init_node('simulation_control', anonymous=True) node = cyber.Node("simulation_control") #rospy.Subscriber('/apollo/control', control_cmd_pb2.ControlCommand, # control.sim_control_callback) node.create_reader('/apollo/control', control_cmd_pb2.ControlCommand, control.sim_control_callback) #rospy.spin() node.spin()
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 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(self): cyber.init() ct = cyber_timer.Timer(100, func, 0) # 100ms ct.start() time.sleep(1) # 1s ct.stop() print('+' * 40 + 'test set_option' + '+' * 40) ct2 = cyber_timer.Timer() # 10ms ct2.set_option(100, func, 0) ct2.start() time.sleep(1) # 1s ct2.stop() cyber.shutdown()
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_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 test_init(self): """ Test cyber. """ self.assertTrue(cyber.init()) self.assertTrue(cyber.ok()) cyber.shutdown() self.assertTrue(cyber.is_shutdown())
def __init__(self): self.car = Lexus() self.car_state_mutex = allocate_lock() self.params = self.pars_arguments() self.trajectory_file_handler = self.create_trajectory_file_handler() self.trajectory = self.load_trajectory() self.nearest_point_id = 0 cyber.init() self.cyber_node = cyber.Node("oscar_tools_trajectory_player") self.cyber_trajectory_pub = self.cyber_node.create_writer( '/apollo/planning', planning_pb2.ADCTrajectory) self.localization_received = False self.planning_msg_count = 0 self.min_along_path_neighbors_dist = 2 * pi * self.car.min_radius * 0.8 self.estop = False atexit.register(self.shutdown)
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 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 function for carla simulator ROS bridge maintaiing the communication client and the CarlaRosBridge objects """ cyber.init('carla_cyber_client') params = yaml.load(open("../../config/settings.yaml"))['carla'] host = params['host'] port = params['port'] carla_client = carla.Client(host=host, port=port) carla_client.set_timeout(2000) carla_world = carla_client.get_world() carla_ros_bridge = CarlaRosBridge(carla_world=carla_client.get_world(), params=params) carla_ros_bridge.run() carla_ros_bridge = None del carla_world del carla_client
def __init__(self, carla_actor, parent): """ Constructor :param carla_actor: carla actor object :type carla_actor: carla.Actor :param parent: the parent of this :type parent: carla_ros_bridge.Parent """ super(EgoVehicle, self).__init__( carla_actor=carla_actor, parent=parent, topic_prefix=carla_actor.attributes.get('role_name'), append_role_name_topic_postfix=False) self.object_sensor = None if parent.use_object_sensor: self.object_sensor = ObjectSensor(self) self.vehicle_info_published = False self.planned_trajectory = None self.control_mode = False # self.enable_autopilot_subscriber = rospy.Subscriber( # self.topic_name() + "/enable_autopilot", # Bool, self.enable_autopilot_updated) cyber.init() self.cyber_node = cyber.Node('carla_ego_node') self.cyber_node.create_reader('/apollo/control', ControlCommand, self.cyber_control_command_updated) self.cyber_node.create_reader('/apollo/planning', ADCTrajectory, self.planning_callback) self.cyber_node.create_reader('/apollo/routing_response', RoutingResponse, self.routing_callback) self.tf_writer = self.cyber_node.create_writer('/tf', TransformStampeds)
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)
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 test_record_writer_read(self): """ unit test of record. """ self.assertTrue(cyber.init()) # writer fwriter = record.RecordWriter() fwriter.set_size_fileseg(200) fwriter.set_intervaltime_fileseg(10) self.assertTrue(fwriter.open(TEST_RECORD_FILE)) fwriter.write_channel(CHAN_1, MSG_TYPE, STR_10B) fwriter.write_message(CHAN_1, STR_10B, TIME) self.assertEqual(1, fwriter.get_messagenumber(CHAN_1)) self.assertEqual(MSG_TYPE, fwriter.get_messagetype(CHAN_1)) self.assertEqual(STR_10B, fwriter.get_protodesc(CHAN_1)) fwriter.close() # reader fread = record.RecordReader(TEST_RECORD_FILE) channel_list = fread.get_channellist() self.assertEqual(1, len(channel_list)) self.assertEqual(CHAN_1, channel_list[0]) for channelname, msg, datatype, timestamp in fread.read_messages(): # print "+++" # print channelname # print msg, datatype, timestamp self.assertEqual(CHAN_1, channelname) self.assertEqual(STR_10B, msg) self.assertEqual(TIME, timestamp) self.assertEqual(1, fread.get_messagenumber(channelname)) self.assertEqual(MSG_TYPE, datatype) self.assertEqual(MSG_TYPE, fread.get_messagetype(channelname)) # print "pbdesc -> %s" % fread.get_protodesc(channelname) msg = record_pb2.Header() header_msg = fread.get_headerstring() msg.ParseFromString(header_msg) self.assertEqual(1, msg.major_version) self.assertEqual(0, msg.minor_version) self.assertEqual(1, msg.chunk_number) self.assertEqual(1, msg.channel_number) self.assertTrue(msg.is_complete) cyber.shutdown()
def setUpClass(cls): cyber.init()
def listener_callback(self, perception_obstacles): #with open(self.__simulation_folder + "/fusion_{}.txt".format(perception_obstacles.header.sequence_num), "w") as log_txt: #with open(self.__scenario.oracle_path + "/fusion_{}.txt".format(perception_obstacles.header.sequence_num), "w") as log_txt: with open(self.__simulation_folder + "/fusion_{}.txt".format(perception_obstacles.header.sequence_num), "w") as log_txt: log_txt.write(MessageToJson(perception_obstacles)) if __name__ == "__main__": scenario_path = sys.argv[1] index = sys.argv[2] filter_name = sys.argv[3] print(scenario_path, index, filter_name) scenario = Scenario(scenario_path) filter = Filter.get_filter(filter_name) print(filter) cyber.init("fault_injector_scenario_{}".format(index)) time.sleep(1) cyber_node = cyber.Node("fault_injector_node_{}".format(index)) #scenario = Scenario(scenario_path) simulation = Simulation(scenario, filter, cyber_node) simulation.run() time.sleep(1) cyber.shutdown() time.sleep(1) #i = 0 # #for scenario in Scenario.get_scenarios(): # cyber.init("ciao_{}".format(i)) # time.sleep(10) # fault_injector_node = cyber.Node("fault_injector_{}".format(i))
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 listener(): cyber.init() test_node = cyber.Node("chassis_acc_listener") test_node.create_reader("/apollo/canbus/chassis", chassis_pb2.Chassis, callback)
def perception_receiver(perception_channel): """publisher""" cyber.init() node = cyber.Node("perception") node.create_reader(perception_channel, PerceptionObstacles, receiver) node.spin()
while True: cmd = raw_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" 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" elif len(cmd) == 3: data_collector.run(cmd) if __name__ == '__main__': cyber.init() main() cyber.shutdown()
import sys sys.path.append("../") 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) if __name__ == '__main__': cyber.init("talker_sample") test_talker_class() cyber.shutdown()
def listener(): cyber.init() test_node = cyber.Node("planning_acc_listener") test_node.create_reader("/apollo/planning", planning_pb2.ADCTrajectory, callback)
#self.logger.info("%s"%insstat) self.insstat_pub.write(insstat) def shutdown(self): """ shutdown rosnode """ self.terminating = True #self.logger.info("Shutting Down...") time.sleep(0.2) 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) if __name__ == '__main__': cyber.init() main() cyber.shutdown()
import sys sys.path.append("../") 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) if __name__ == '__main__': cyber.init("talker_sample") test_talker_class() cyber.shutdown()