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
