def publish_control(self): left_turn_state = False right_turn_state = False low_beam_state = False high_beam_state = False while True: controlcmd = control_cmd_pb2.ControlCommand() #Emergency stop if self.params['emergency_stop']: controlcmd.throttle = 0.0 controlcmd.brake = 50.0 controlcmd.signal.emergency_light = True if self.params['horn']: controlcmd.signal.horn = True controlcmd.header.timestamp_sec = cyber_time.Time.now().to_sec( ) self.controlpub.write(controlcmd) else: if self.params['left_turn_signal']: controlcmd.signal.turn_signal = 1 if self.params['right_turn_signal']: controlcmd.signal.turn_signal = 2 if self.params['low_beam']: controlcmd.signal.low_beam = True if self.params['high_beam']: controlcmd.signal.high_beam = True if self.params['emergency_light']: controlcmd.signal.emergency_light = True if self.params['horn']: controlcmd.signal.horn = True controlcmd.gear_location = self.params['gear'] controlcmd.throttle = self.params['throttle'] controlcmd.brake = self.params['brake'] if abs(self.params['steering_eps'] - self.axis_states['x'] * STEERING_MAX_RATIO) < MAX_STEERING_PERCENTAGE: self.params['steering_eps'] -= self.axis_states[ 'x'] * STEERING_MAX_RATIO controlcmd.steering_target = self.params['steering_eps'] #Teleop exit condition if self.params['exit']: controlcmd = control_cmd_pb2.ControlCommand() controlcmd.throttle = 0.0 controlcmd.brake = 0.0 controlcmd.header.timestamp_sec = cyber_time.Time.now( ).to_sec() self.controlpub.write(controlcmd) os.kill(os.getpid(), signal.SIGTERM) controlcmd.header.timestamp_sec = cyber_time.Time.now().to_sec() self.controlpub.write(controlcmd) time.sleep(1 / CONTROL_FRAMERATIO)
def process(control_analyzer, planning_analyzer, lidar_endtoend_analyzer): is_auto_drive = False for msg in reader.read_messages(): if msg.topic == "/apollo/canbus/chassis": chassis = chassis_pb2.Chassis() chassis.ParseFromString(msg.message) if chassis.driving_mode == \ chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE: is_auto_drive = True else: is_auto_drive = False if msg.topic == "/apollo/control": if not is_auto_drive: continue control_cmd = control_cmd_pb2.ControlCommand() control_cmd.ParseFromString(msg.message) control_analyzer.put(control_cmd) lidar_endtoend_analyzer.put_control(control_cmd) if msg.topic == "/apollo/planning": if not is_auto_drive: continue adc_trajectory = planning_pb2.ADCTrajectory() adc_trajectory.ParseFromString(msg.message) planning_analyzer.put(adc_trajectory) lidar_endtoend_analyzer.put_planning(adc_trajectory) if msg.topic == "/apollo/sensor/velodyne64/compensator/PointCloud2": if not is_auto_drive: continue point_cloud = pointcloud_pb2.PointCloud() point_cloud.ParseFromString(msg.message) lidar_endtoend_analyzer.put_lidar(point_cloud)
def __init__(self, node): self.sequence_num = 0 self.control_pub = node.create_writer('/apollo/control', control_cmd_pb2.ControlCommand) time.sleep(0.3) self.controlcmd = control_cmd_pb2.ControlCommand() self.outfile = ""
def process(control_analyzer, planning_analyzer, lidar_endtoend_analyzer, is_simulation, plot_planning_path, plot_planning_refpath, all_data): is_auto_drive = False for msg in reader.read_messages(): if msg.topic == "/apollo/canbus/chassis": chassis = chassis_pb2.Chassis() chassis.ParseFromString(msg.message) if chassis.driving_mode == \ chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE: is_auto_drive = True else: is_auto_drive = False if msg.topic == "/apollo/control": if (not is_auto_drive and not all_data) or \ is_simulation or plot_planning_path or plot_planning_refpath: continue control_cmd = control_cmd_pb2.ControlCommand() control_cmd.ParseFromString(msg.message) control_analyzer.put(control_cmd) lidar_endtoend_analyzer.put_pb('control', control_cmd) if msg.topic == "/apollo/planning": if (not is_auto_drive) and (not all_data): continue adc_trajectory = planning_pb2.ADCTrajectory() adc_trajectory.ParseFromString(msg.message) planning_analyzer.put(adc_trajectory) lidar_endtoend_analyzer.put_pb('planning', adc_trajectory) if plot_planning_path: planning_analyzer.plot_path(plt, adc_trajectory) if plot_planning_refpath: planning_analyzer.plot_refpath(plt, adc_trajectory) if msg.topic == "/apollo/sensor/velodyne64/compensator/PointCloud2" or \ msg.topic == "/apollo/sensor/lidar128/compensator/PointCloud2": if ((not is_auto_drive) and (not all_data)) or is_simulation or \ plot_planning_path or plot_planning_refpath: continue point_cloud = pointcloud_pb2.PointCloud() point_cloud.ParseFromString(msg.message) lidar_endtoend_analyzer.put_lidar(point_cloud) if msg.topic == "/apollo/perception/obstacles": if ((not is_auto_drive) and (not all_data)) or is_simulation or \ plot_planning_path or plot_planning_refpath: continue perception = perception_obstacle_pb2.PerceptionObstacles() perception.ParseFromString(msg.message) lidar_endtoend_analyzer.put_pb('perception', perception) if msg.topic == "/apollo/prediction": if ((not is_auto_drive) and (not all_data)) or is_simulation or \ plot_planning_path or plot_planning_refpath: continue prediction = prediction_obstacle_pb2.PredictionObstacles() prediction.ParseFromString(msg.message) lidar_endtoend_analyzer.put_pb('prediction', prediction)
def __init__(self): self.sequence_num = 0 self.control_pub = rospy.Publisher('/apollo/control', control_cmd_pb2.ControlCommand, queue_size=1) rospy.sleep(0.3) self.controlcmd = control_cmd_pb2.ControlCommand() self.canmsg_received = False self.localization_received = False self.outfile = ""
def __init__(self, node): self.sequence_num = 0 self.control_pub = node.create_writer('/apollo/control', control_cmd_pb2.ControlCommand) time.sleep(0.3) self.controlcmd = control_cmd_pb2.ControlCommand() self.canmsg_received = False self.localization_received = False self.case = 'a' self.in_session = False self.outfile = ""
def __init__(self, node): self.sequence_num = 0 self.control_pub = node.create_writer('/apollo/control', control_cmd_pb2.ControlCommand) time.sleep(0.3) self.controlcmd = control_cmd_pb2.ControlCommand() #print("init test %f" %(control_cmd_pb2.ControlCommand().throttle)) self.canmsg_received = False self.localization_received = True # change by why self.case = 'a' self.in_session = False self.outfile = ""
def callback(data): global STEERING_LINE_DATA global TROTTLE_LINE_DATA, BRAKE_LINE_DATA control_cmd_pb = control_cmd_pb2.ControlCommand() control_cmd_pb.CopyFrom(data) STEERING_LINE_DATA.append(control_cmd_pb.steering_target) if len(STEERING_LINE_DATA) > FLAGS.data_length: STEERING_LINE_DATA = STEERING_LINE_DATA[-FLAGS.data_length:] BRAKE_LINE_DATA.append(control_cmd_pb.brake) if len(BRAKE_LINE_DATA) > FLAGS.data_length: BRAKE_LINE_DATA = BRAKE_LINE_DATA[-FLAGS.data_length:] TROTTLE_LINE_DATA.append(control_cmd_pb.throttle) if len(TROTTLE_LINE_DATA) > FLAGS.data_length: TROTTLE_LINE_DATA = TROTTLE_LINE_DATA[-FLAGS.data_length:]
def __init__(self, file): self.proc = [line.rstrip('\n') for line in open(file)] self.index = 0 outfile = file + '_recorded.csv' i = 0 outfile = file + str(i) + '_recorded.csv' while os.path.exists(outfile): i += 1 outfile = file + str(i) + '_recorded.csv' self.file = open(outfile, 'w') self.file.write( "time,io,ctlmode,ctlbrake,ctlthrottle,ctlgear_location,vehicle_speed," + "engine_rpm,driving_mode,throttle_percentage,brake_percentage,gear_location, imu\n" ) self.sequence_num = 0 self.control_pub = rospy.Publisher( '/apollo/control', control_cmd_pb2.ControlCommand, queue_size=1) rospy.sleep(0.3) self.controlcmd = control_cmd_pb2.ControlCommand() # Send First Reset Message print "Send Reset Command" self.controlcmd.header.module_name = "control" self.controlcmd.header.sequence_num = self.sequence_num self.sequence_num = self.sequence_num + 1 self.controlcmd.header.timestamp_sec = rospy.get_time() self.controlcmd.pad_msg.action = 2 self.control_pub.publish(self.controlcmd) rospy.sleep(0.3) # Set Default Message print "Send Default Command" self.controlcmd.pad_msg.action = 1 self.controlcmd.throttle = 0 self.controlcmd.brake = 0 self.controlcmd.steering_rate = 100 self.controlcmd.steering_target = 0 self.controlcmd.gear_location = chassis_pb2.Chassis.GEAR_NEUTRAL self.printedcondition = False self.runtimer = False self.canmsg_received = False self.localization_received = False
if args.bag: file_path = args.bag # bag = rosbag.Bag(file_path) reader = RecordReader(file_path) for msg in reader.read_messages(): print(msg.timestamp, msg.topic) if msg.topic == "/apollo/localization/pose": localization = localization_pb2.LocalizationEstimate() localization.ParseFromString(msg.message) controlinfo.callback_localization(localization) elif msg.topic == "/apollo/planning": adc_trajectory = planning_pb2.ADCTrajectory() adc_trajectory.ParseFromString(msg.message) controlinfo.callback_planning(adc_trajectory) elif msg.topic == "/apollo/control": control_cmd = control_cmd_pb2.ControlCommand() control_cmd.ParseFromString(msg.message) controlinfo.callback_control(control_cmd) elif msg.topic == "/apollo/canbus/chassis": chassis = chassis_pb2.Chassis() chassis.ParseFromString(msg.message) controlinfo.callback_canbus(chassis) print("Done reading the file") else: cyber.init() # rospy.init_node('control_info', anonymous=True) node = cyber.Node("rtk_recorder") planningsub = node.create_reader('/apollo/planning', planning_pb2.ADCTrajectory, controlinfo.callback_planning)
def __init__(self, bag_path=None, out_bag_path=None): self.pb_msg = control_cmd_pb2.ControlCommand() self.bag_path = bag_path print "input bag", self.bag_path print "output bag", self.out_bag_path