class TopicSubscirber:
seniorcar_state = SeniorcarState()
seniorcar_command = SeniorcarState()
devision = Int8()
arduino_str = String()
recive_str = String()
def __init__(self):
rospy.init_node('output_to_console')
def subscribe_tpics(self):
rospy.Subscriber("seniorcar_command", SeniorcarState,
self.commandCallback)
rospy.Subscriber("seniorcar_state", SeniorcarState, self.stateCallback)
rospy.Subscriber("motor_controller_input", String,
self.arduinoCallback)
def reciveCallback(self, data):
self.recive_str = data
def commandCallback(self, data):
self.seniorcar_command = data
def stateCallback(self, data):
self.seniorcar_state = data
def devisionCallback(self, data):
self.devision = data
def arduinoCallback(self, data):
self.arduino_str = data
def console_print(self):
rate = rospy.Rate(50)
start_time = rospy.get_rostime()
print "time,command_angle_deg,state_angle_deg,motor_contoller_command,state_vel"
while not rospy.is_shutdown():
now = rospy.get_rostime()
print "%d.%09d,%f,%f,%s,%f,%f" % (
now.secs - start_time.secs, now.nsecs,
self.seniorcar_command.steer_angle,
self.seniorcar_state.steer_angle, self.arduino_str.data,
self.seniorcar_command.vehicle_velocity,
self.seniorcar_state.vehicle_velocity)
rate.sleep()
class Right_Left:
seniorcar_command = SeniorcarState()
def __init__(self):
rospy.init_node('right_or_left', anonymous=True)
rospy.Subscriber("scan", LaserScan, self.callback)
self.pub = rospy.Publisher('seniorcar_command',
SeniorcarState,
queue_size=1000)
self.pub.publish(self.seniorcar_command)
def callback(self, data):
data_num = len(data.ranges)
r_count = 0
l_count = 0
#LRF極座標データを直行座標データに変換(x,y)
for i in range(0, data_num - 1):
if data.ranges[i] < 4.0 and data.ranges[i] > 0.2:
x = data.ranges[i] * math.sin(data.angle_min +
data.angle_increment * i)
y = data.ranges[i] * math.cos(data.angle_min +
data.angle_increment * i)
if 0.0 < y and y < 2.0:
if 0.0 < x and x < 1.0:
l_count += 1
elif -1.0 < x and x < 0:
r_count += 1
print r_count, l_count
if r_count > 3:
self.seniorcar_command.right_winker = 1
else:
self.seniorcar_command.right_winker = 0
if l_count > 3:
self.seniorcar_command.left_winker = 1
else:
self.seniorcar_command.left_winker = 0
def publish_loop(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.pub.publish(self.seniorcar_command)
rate.sleep()
class AvoidPredictedAccident:
accident_predict_result = AccidentPredictResult()
seniorcar_odometry = Odometry()
seniorcar_state = SeniorcarState()
seniorcar_command = SeniorcarState()
seniorcar_command.accel_opening = 0
seniorcar_state.accel_opening = 0
seniorcar_command.steer_angle = 0
seniorcar_command.max_velocity = 2.0
enable_steer_motor = Bool()
enable_steer_motor.data = False #for initialize 11/2
enable_program = Bool() #11/6
enable_program.data = True #11/6 the seniorcar avoids a step then it turns False
start_odometry = Odometry()
odom_recive_flag = True
time_counter = 0 #one way to keep time. 11/7
time_counter_flag = False
t1 = rospy.Time(0) #another way to keep time.11/7
t2 = rospy.Time(0)
#avoid_index1 = 0
old_steer_command_angle = 0.0
def __init__(self):
rospy.init_node('generate_seniorca_comand_to_avoid_predicted_accident')
self.pub = rospy.Publisher('seniorcar_command',
SeniorcarState,
queue_size=10)
self.pub2 = rospy.Publisher('enable_motor', Bool, queue_size=10)
self.pub3 = rospy.Publisher(
'enable_brake', Bool,
queue_size=10) #for avoid program colision 11/6
def subscribe_predicted_result(self):
rospy.Subscriber("accident_predict", AccidentPredictResult,
self.accidentsubCallback)
rospy.Subscriber("seniorcar_state", SeniorcarState, self.stateCallback)
rospy.Subscriber("seniorcar_odometry", Odometry, self.odomCallback)
def stateCallback(self, msg):
self.seniorcar_state = msg
self.seniorcar_command.max_velocity = msg.max_velocity
def odomCallback(self, msg):
if self.odom_recive_flag:
self.start_odometry = msg
print "start_odometry"
print "x:" + str(
self.start_odometry.pose.pose.position.x) + "y:" + str(
self.start_odometry.pose.pose.position.y)
self.odom_recive_flag = False
self.seniorcar_odometry = msg
def accidentsubCallback(self, msg):
self.accident_predict_result = msg
self.min_predict_angle = msg.steer_angle[0]
self.predict_angle_devision = abs(msg.steer_angle[1] -
msg.steer_angle[0])
def calcSteerAngleToPredictedAngleIndex(self, steer_angle):
# radに直す
steer_angle *= 3.14 / 180
# まだ判定結果を受信していない場合は0を返す
if len(self.accident_predict_result.steer_angle) < 1:
return 0
# 範囲外の処理
elif steer_angle < self.accident_predict_result.steer_angle[0]:
return 0
elif steer_angle > self.accident_predict_result.steer_angle[-1]:
return int(len(self.accident_predict_result.steer_angle) - 1)
else:
return int(
(steer_angle - self.accident_predict_result.steer_angle[0]) /
(self.accident_predict_result.steer_angle[1] -
self.accident_predict_result.steer_angle[0]))
def update_seniorcar_command(self):
data_num = len(self.accident_predict_result.steer_angle)
if data_num < 1:
return
current_steer_index = self.calcSteerAngleToPredictedAngleIndex(
self.seniorcar_state.steer_angle) # 現在の操舵角度0度の番号
if current_steer_index == 0:
current_steer_index += 1
elif current_steer_index == data_num - 1:
current_steer_index -= 1
if self.accident_predict_result.max_distance[
current_steer_index] > APPROACH_THRESHOLD and self.accident_predict_result.max_distance[
current_steer_index +
1] > APPROACH_THRESHOLD and self.accident_predict_result.max_distance[
current_steer_index - 1] > APPROACH_THRESHOLD:
# 前方が安全なら操舵角度そのまま
self.seniorcar_command.steer_angle = self.seniorcar_state.steer_angle
self.enable_steer_motor.data = False
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening, MAX_ACCEL_OPENING)
else:
# 安全に走行できる経路の中で現在の角度に近い場所を探す。そもそも無ければ停止する
avoid_index = -1
for i in range(0, data_num - 1):
if self.accident_predict_result.max_distance[
i] > AVOID_THRESHOLD and self.accident_predict_result.max_distance[
i +
1] > APPROACH_THRESHOLD and self.accident_predict_result.max_distance[
i - 1] > APPROACH_THRESHOLD:
if abs(current_steer_index - i) < abs(current_steer_index -
avoid_index):
avoid_index = i
if avoid_index == -1:
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening * DECELERATION_CONSTANT,
MAX_ACCEL_OPENING * DECELERATION_CONSTANT)
self.enable_steer_motor.data = False
self.t1 = rospy.Time.now()
else:
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening, MAX_ACCEL_OPENING)
self.enable_steer_motor.data = True
if avoid_index < 5 and avoid_index > 2: #to adjust offset.11/6
self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[
avoid_index - 2] * 180.0 / 3.14
#elif avoid_index == 7:
#self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[avoid_index] * 180.0 /3.14
elif avoid_index > 9 and avoid_index < 12:
self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[
avoid_index + 2] * 180.0 / 3.14
else:
self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[
avoid_index] * 180.0 / 3.14 #11/6
# 急激に変化しないようにする処理
if abs(self.seniorcar_command.steer_angle -
self.old_steer_command_angle
) > MAX_CHANGE_STEER_ANGLE_BY_STEP:
if self.seniorcar_command.steer_angle > self.old_steer_command_angle:
self.seniorcar_command.steer_angle = self.old_steer_command_angle + MAX_CHANGE_STEER_ANGLE_BY_STEP
elif self.seniorcar_command.steer_angle < self.old_steer_command_angle:
self.seniorcar_command.steer_angle = self.old_steer_command_angle - MAX_CHANGE_STEER_ANGLE_BY_STEP
self.old_steer_command_angle = self.seniorcar_command.steer_angle
def calculate_and_publish_command(self):
rate = rospy.Rate(PUBLISH_RATE)
while not rospy.is_shutdown():
# 最初は足元の情報がないので介入なし
if pow(
self.seniorcar_odometry.pose.pose.position.x -
self.start_odometry.pose.pose.position.x, 2) + pow(
self.seniorcar_odometry.pose.pose.position.y -
self.start_odometry.pose.pose.position.y, 2) < pow(
NO_INTERVATION_DISTANCE, 2):
self.enable_steer_motor.data = False
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening, MAX_ACCEL_OPENING)
self.seniorcar_command.steer_angle = self.seniorcar_state.steer_angle
#if self.t1_flag.data == True:
# self.t2 = rospy.Time.now()
# self.t3 = self.t2.secs-self.t1.secs
#if self.t3 < DELAY_TIME and self.t3 > 0:
# self.pub.publish(self.seniorcar_command)
# self.pub2.publish(self.enable_steer_motor)
#elif self.t3 >DELAY_TIME and self.t3 < DELAY_TIME*2:
# avoid_index2 = 7
# self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[avoid_index2]
# self.pub.publish(self.seniorcar_command)
# self.pub2.publish(self.enable_steer_motor)
#else:
# if self.t1_flag == True:
# self.t1_flag = False
# self.update_seniorcar_command()
# self.pub.publish(self.seniorcar_command)
# self.pub2.publish(self.enable_steer_motor)
else:
self.update_seniorcar_command()
self.pub.publish(self.seniorcar_command)
self.pub2.publish(self.enable_steer_motor)
rate.sleep()
class AvoidPredictedAccident:
accident_predict_result = AccidentPredictResult()
seniorcar_odometry = Odometry()
seniorcar_state = SeniorcarState()
seniorcar_command = SeniorcarState()
seniorcar_command.accel_opening = 0
seniorcar_state.accel_opening = 0
seniorcar_command.steer_angle = 0
seniorcar_command.max_velocity = 6.0
enable_steer_motor = Bool()
enable_steer_motor.data = False #for initialize 11/2
enable_program = Bool() #11/6
enable_program.data = True #11/6 the seniorcar avoids a step then it turns False
start_odometry = Odometry()
odom_recive_flag = True
update_flag = Bool()
update_flag.data = True #11/7_23_15
time_counter = 0 #one way to keep time. 11/7
time_counter_flag = False
t1 = rospy.Time() #another way to keep time.11/7
t2 = rospy.Time()
t3 = 0
t1_flag = Bool()
t1_flag.data = True
avoid_index1 = 0
avoid_index2 = 0
counter = 0
old_steer_command_angle = 0.0
def __init__(self):
rospy.init_node('generate_seniorca_comand_to_avoid_predicted_accident')
self.pub = rospy.Publisher('seniorcar_command',
SeniorcarState,
queue_size=10)
self.pub2 = rospy.Publisher('enable_motor', Bool, queue_size=10)
#self.pub3 = rospy.Publisher('enable_brake', Bool, queue_size=10)#for avoid program colision 11/6
def subscribe_predicted_result(self):
rospy.Subscriber("accident_predict", AccidentPredictResult,
self.accidentsubCallback)
rospy.Subscriber("seniorcar_state", SeniorcarState, self.stateCallback)
rospy.Subscriber("seniorcar_odometry", Odometry, self.odomCallback)
def stateCallback(self, msg):
self.seniorcar_state = msg
self.seniorcar_command.max_velocity = msg.max_velocity
def odomCallback(self, msg):
if self.odom_recive_flag:
self.start_odometry = msg
print "start_odometry"
print "x:" + str(
self.start_odometry.pose.pose.position.x) + "y:" + str(
self.start_odometry.pose.pose.position.y)
self.odom_recive_flag = False
self.seniorcar_odometry = msg
def accidentsubCallback(self, msg):
self.accident_predict_result = msg
self.min_predict_angle = msg.steer_angle[0]
self.predict_angle_devision = abs(msg.steer_angle[1] -
msg.steer_angle[0])
def calcSteerAngleToPredictedAngleIndex(self, steer_angle):
# radに直す
steer_angle *= 3.14 / 180
# まだ判定結果を受信していない場合は0を返す
if len(self.accident_predict_result.steer_angle) < 1:
return 0
# 範囲外の処理
elif steer_angle < self.accident_predict_result.steer_angle[0]:
return 0
elif steer_angle > self.accident_predict_result.steer_angle[-1]:
return int(len(self.accident_predict_result.steer_angle) - 1)
else:
return int(
(steer_angle - self.accident_predict_result.steer_angle[0]) /
(self.accident_predict_result.steer_angle[1] -
self.accident_predict_result.steer_angle[0]))
def update_seniorcar_command(self):
if self.update_flag.data == False:
pass
if self.update_flag.data == True:
data_num = len(self.accident_predict_result.steer_angle)
if data_num < 1:
return
current_steer_index = self.calcSteerAngleToPredictedAngleIndex(
self.seniorcar_state.steer_angle) # 現在の操舵角度0度の番号
if current_steer_index == 0:
current_steer_index += 1
elif current_steer_index == data_num - 1:
current_steer_index -= 1
if self.accident_predict_result.max_distance[
current_steer_index] > APPROACH_THRESHOLD and self.accident_predict_result.max_distance[
current_steer_index +
1] > APPROACH_THRESHOLD and self.accident_predict_result.max_distance[
current_steer_index - 1] > APPROACH_THRESHOLD:
# 前方が安全なら操舵角度そのまま
self.seniorcar_command.steer_angle = self.seniorcar_state.steer_angle
self.enable_steer_motor.data = False
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening, MAX_ACCEL_OPENING)
else:
# 安全に走行できる経路の中で現在の角度に近い場所を探す。そもそも無ければ停止する
avoid_index = -1
for i in range(0, data_num - 1):
if self.accident_predict_result.max_distance[
i] > AVOID_THRESHOLD and self.accident_predict_result.max_distance[
i +
1] > APPROACH_THRESHOLD and self.accident_predict_result.max_distance[
i - 1] > APPROACH_THRESHOLD:
if abs(current_steer_index -
i) < abs(current_steer_index - avoid_index):
avoid_index = i
if avoid_index == -1:
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening *
DECELERATION_CONSTANT,
MAX_ACCEL_OPENING * DECELERATION_CONSTANT)
self.enable_steer_motor.data = False
else:
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening, MAX_ACCEL_OPENING)
#self.enable_steer_motor.data = True
if avoid_index < 6 and avoid_index > 1: #to adjust offset.11/6
self.avoid_index1 = avoid_index - 2
self.enable_steer_motor.data = True
self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[
self.avoid_index1] * 180.0 / 3.14
if self.t1_flag.data == True:
self.t1 = rospy.get_time(
) #probably it doesn't wok
self.t1_flag.data = False
#elif avoid_index == 7:
#self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[avoid_index] * 180.0 /3.14
elif avoid_index > 8 and avoid_index < 13:
self.avoid_index1 = avoid_index + 2
self.enable_steer_motor.data = True
self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[
self.avoid_index1] * 180.0 / 3.14
if self.t1_flag.data == True:
self.t1 = rospy.get_time()
self.t1_flag.data = False
else:
if avoid_index > 13 or avoid_index < 1:
self.enable_steer_motor.data = True
self.avoid_index1 = avoid_index
self.seniorcar_command.steer_angle = self.accident_predict_result.steer_angle[
self.avoid_index1] * 180.0 / 3.14 #11/6
# 急激に変化しないようにする処理
if abs(self.seniorcar_command.steer_angle -
self.old_steer_command_angle
) > MAX_CHANGE_STEER_ANGLE_BY_STEP:
if self.seniorcar_command.steer_angle > self.old_steer_command_angle:
self.seniorcar_command.steer_angle = self.old_steer_command_angle + MAX_CHANGE_STEER_ANGLE_BY_STEP
elif self.seniorcar_command.steer_angle < self.old_steer_command_angle:
self.seniorcar_command.steer_angle = self.old_steer_command_angle - MAX_CHANGE_STEER_ANGLE_BY_STEP
self.old_steer_command_angle = self.seniorcar_command.steer_angle
def func(self):
pass
def calculate_and_publish_command(self):
rate = rospy.Rate(PUBLISH_RATE)
while not rospy.is_shutdown():
# 最初は足元の情報がないので介入なし
if pow(
self.seniorcar_odometry.pose.pose.position.x -
self.start_odometry.pose.pose.position.x, 2) + pow(
self.seniorcar_odometry.pose.pose.position.y -
self.start_odometry.pose.pose.position.y, 2) < pow(
NO_INTERVATION_DISTANCE, 2):
self.enable_steer_motor.data = False
self.seniorcar_command.accel_opening = min(
self.seniorcar_state.accel_opening, MAX_ACCEL_OPENING)
self.seniorcar_command.steer_angle = self.seniorcar_state.steer_angle
else:
self.update_seniorcar_command() #11/7_23_15
if self.t1_flag.data == False:
self.t2 = rospy.get_time()
self.t3 = self.t2 - self.t1
print(self.t3) #to check t3 11/8_10_25
#11/8_9_20 I checked once t1_flag turn False . and then turn off coment out
#11/7_23_15
if self.t3 < DELAY_TIME and self.t3 > 0 or self.t3 == DELAY_TIME: # I will check here .11/7_9_28
self.update_seniorcar_command()
print("Hello")
#if self.t1_flag.data == True:
#print ("True")
#self.func()
elif self.t3 > DELAY_TIME and self.t3 < DELAY_TIME * 3:
self.update_seniorcar_command()
#self.update_flag.data = False
#self.seniorcar_command.accel_opening = 0 # later. I retrieve this sentence.
#self.avoid_index2 = 14 - self.avoid_index1
#self.seniorcar_command.steer_angle = np.sin(3.14 * self.t3 / PERIODIC_TIME)*self.accident_predict_result.steer_angle[self.avoid_index1]
#print("avoid_index1")
#print(self.avoid_index1)
#print("avoid_index2")
#print(self.avoid_index2)
elif self.t3 == 0 or self.t3 < 0:
self.func()
print("start")
else:
print("I'll update")
self.t1_flag.data = True
self.update_flag.data = True #11/7_23_15
self.pub.publish(self.seniorcar_command)
self.pub2.publish(self.enable_steer_motor)
rate.sleep()
class CANUSB_Connecter:
seniorcar_state = SeniorcarState()
def __init__(self):
rospy.init_node('canusb_connecter', anonymous=True)
self.pub = rospy.Publisher('seniorcar_state',
SeniorcarState,
queue_size=1000)
time.sleep(1)
self.connect_with_canusb()
self.pub.publish(self.seniorcar_state)
print self.pub
def connect_with_canusb(self):
# シリアル通信開始
port = rospy.get_param('canusb_port', "/dev/ttyUSB0")
try:
self.ser = serial.Serial(port, 9600)
self.ser.setDTR(False)
time.sleep(0.5)
self.ser.setDTR(True)
print "start connection with canusb"
except:
print "cannot start connection with canusb"
# CANUSBのCANポートを開くコマンド
# 改行コマンド×複数回→通信速度設定→ポートオープン
time.sleep(0.5)
self.ser.write("\r")
self.ser.write("\r")
time.sleep(0.5)
self.ser.write("C\r")
time.sleep(0.5)
self.ser.write("S6\r")
time.sleep(0.5)
self.ser.write("O\r")
print "can open"
def write_candata(self):
#A0への信号は0byteの場所にインクリメント(毎回変わる値)を入れてあげる必要がある
#A0の6byteと7byteは同じにする
#t0A0 2 22 33は機器構成認識信号（22 33 の部分は00 00 意外であればなんでもよい）
#t0A1 8 00 00 accel 00 00 00 00 00 はアクセル開度の上書き
#t0A5 8 incliment 01 00 00 max_vel 00 00 1C other_commnad はアクセル開度以外の上書き.1Cは各信号を操作できるようにするしないのフラグ
#other_commandの中身は2byteの長さで 0 0 0 hone r_winker l_winker 0 light
incliment = 11
wait_time = 0
while not rospy.is_shutdown():
if self.can_override_flag.value == 0:
self.ser.write("t0A022233\r")
self.ser.write("t0A58" + str(incliment) + "00000000000000\r")
elif self.can_override_flag.value == 1:
self.ser.write("t0A022233\r")
print self.command_message(incliment)
self.ser.write(self.command_message(incliment))
#self.ser.write("t0A58"+str(incliment)+"01000000000F0F\r")
if wait_time < 1:
wait_time = wait_time + 0.05
else:
#self.ser.write("t0A180000330000000000\r")
0
time.sleep(0.05)
incliment = incliment + 1
if incliment > 90:
incliment = 11
# CANUSBに送るシリアルコマンド文字列を生成
def command_message(self, incliment):
command = ""
command += "t0A5"
command += "8"
command += str(incliment)
command += "010000"
command += "00" #max_vel
command += "00"
command += "1C"
# 7byte目に格納する文字列の2進数表記
hone = int(self.seniorcar_command_array[8])
r_wink = int(self.seniorcar_command_array[7])
l_wink = int(self.seniorcar_command_array[6])
light = int(self.seniorcar_command_array[5])
other_commands = "000" + str(hone) + str(r_wink) + str(
l_wink) + "0" + str(light)
other_commands = '%x' % int(other_commands, 2)
if r_wink == 1:
other_commands = "08"
elif l_wink == 1:
other_commands = "04"
else:
other_commands = "00"
command += other_commands
command += "\r"
return command
# 受信と送信にプロセスを分けて処理
def start_spin(self):
self.can_override_flag = Value('i', 0)
self.seniorcar_state_array = Array(
'd', (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0))
self.seniorcar_command_array = Array(
'd', (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0))
self.read_process = Process(target=self.read_candata, args=(''))
self.write_process = Process(target=self.write_candata, args=(''))
try:
self.read_process.start()
self.write_process.start()
except rospy.ROSInterruptException:
pass
rospy.Subscriber("seniorcar_command", SeniorcarState,
self.command_recive)
while not rospy.is_shutdown():
self.update_vehicle_data()
self.pub.publish(self.seniorcar_state)
time.sleep(0.1)
def command_recive(self, data):
self.seniorcar_command_array[0] = data.accel_opening
self.seniorcar_command_array[1] = data.max_velocity
self.seniorcar_command_array[2] = data.steer_angle
self.seniorcar_command_array[3] = data.vehicle_velocity * 3.6
self.seniorcar_command_array[4] = data.direction_switch
self.seniorcar_command_array[5] = data.light_signal
self.seniorcar_command_array[6] = data.left_winker
self.seniorcar_command_array[7] = data.right_winker
self.seniorcar_command_array[8] = data.hone_signal
def update_vehicle_data(self):
self.seniorcar_state.accel_opening = self.seniorcar_state_array[0]
self.seniorcar_state.max_velocity = self.seniorcar_state_array[1]
self.seniorcar_state.steer_angle = self.seniorcar_state_array[2]
self.seniorcar_state.vehicle_velocity = self.seniorcar_state_array[
3] / 3.6 # [km/h]から[m/s]へ変換
self.seniorcar_state.direction_switch = self.seniorcar_state_array[4]
self.seniorcar_state.light_signal = self.seniorcar_state_array[5]
self.seniorcar_state.left_winker = self.seniorcar_state_array[6]
self.seniorcar_state.right_winker = self.seniorcar_state_array[7]
self.seniorcar_state.hone_signal = self.seniorcar_state_array[8]
def read_candata(self):
line = ""
can_id = ""
can_data = ""
i = 0
while not rospy.is_shutdown():
serial_bit = self.ser.read()
if serial_bit == "\r" and i > 10:
can_id = line[:4]
#can_bit = line[4]
can_data = line[5:]
line = ""
i = 0
if can_id == "t01E": # セニアカー走行データのID
self.update_can_information_array(can_data)
elif can_id == "t010": # システム認識終了フラグの受信
if can_data[6:8] == "84":
if self.can_override_flag.value == 0:
print "change to interupt mode"
self.can_override_flag.value = 1
else:
if self.can_override_flag.value == 1:
print "change to manual mode"
self.can_override_flag.value = 0
elif can_id == "t0A0":
print can_data
else:
line = line + serial_bit
i = i + 1
self.ser.close()
def update_can_information_array(self, can_data):
self.seniorcar_state_array[0] = float(int(can_data[:2],
16)) / 127.0 # アクセル開度
self.seniorcar_state_array[1] = float(int(
can_data[2:4], 16)) / 100.0 * 4.0 + 2.0 # 最大速度
self.seniorcar_state_array[2] = float(
int(can_data[6:8] + can_data[4:6], 16)) / 10.0 - 90.0 # 操舵角度
self.seniorcar_state_array[3] = float(
int(can_data[10:12] + can_data[8:10], 16)) / 100.0 # 車両速度
# 1byteのデータからbit情報を抽出
byte7_data = int(can_data[14:16], 16)
self.seniorcar_state_array[4] = byte7_data & 1
self.seniorcar_state_array[5] = (byte7_data >> 1) & 1
self.seniorcar_state_array[6] = (byte7_data >> 2) & 1
self.seniorcar_state_array[7] = (byte7_data >> 3) & 1
self.seniorcar_state_array[8] = (byte7_data >> 4) & 1
class playRecordedorder:
speed_array = []
steer_array = []
seniorcar_command =SeniorcarState()
enable_motor = Bool()
enable_motor.data = True
seniorcar_command.accel_opening = 0
seniorcar_command.steer_angle = 0
seniorcar_command.max_velocity = 0
index = 0
def __init__(self):
rospy.init_node('play_recorded_order')
self.get_order_from_txt()
self.pub = rospy.Publisher('seniorcar_command', SeniorcarState, queue_size=10)
self.pub2 = rospy.Publisher('enable_motor', Bool, queue_size=10)
def get_order_from_txt(self):
filename = rospy.get_param('command_array_file_path','/home/nishi/catkin_ws/src/test_drive/command_array.txt')
print "open " + str(filename)
f=open(filename,'r')
num = 0
for j in range(10):
speed = 0
steer = 0
self.speed_array.append(speed)
self.steer_array.append(steer)
for i in f.readlines():
#print i
i=i.strip() #末尾の改行を除去
i=i.split("\t") #空白で文字列を区切り、リストを作成
speed = float(i[0])
steer = float(i[1])
self.speed_array.append(speed)
self.steer_array.append(steer)
num += 1
f.close()
print "success to load %d command_array" % num
print(self.steer_array)
def update_order(self):
if len(self.speed_array) -1 > self.index:
if self.speed_array[self.index]*3.6 > 2.0:
self.seniorcar_command.max_velocity = self.speed_array[self.index]*3.6
self.seniorcar_command.accel_opening = 127#127
else:
self.seniorcar_command.max_velocity = 2.0
self.seniorcar_command.accel_opening = 127#self.speed_array[self.index]*3.6/2*127
self.seniorcar_command.steer_angle = self.steer_array[self.index]
self.index += 1
else:
self.seniorcar_command.accel_opening = 0
self.enable_motor.data = False
rospy.on_shutdown(self.print_finish)
def calculate_and_publish_cmd_vel(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.update_order()
self.pub.publish(self.seniorcar_command)
self.pub2.publish(self.enable_motor)
rate.sleep()
def print_finish(self):
print "finish auto driving"
class OdometryCalculator:
odometry = Odometry()
seniorcar_command = SeniorcarState()
t = 0
def __init__(self):
rospy.init_node('seniorcar_odometry', anonymous=True)
rospy.Subscriber("seniorcar_state", SeniorcarState,
self.update_odometry)
self.pub = rospy.Publisher('seniorcar_odometry',
Odometry,
queue_size=1000)
self.current_time = rospy.get_rostime()
self.last_time = rospy.get_rostime()
self.odometry.header.frame_id = "odom"
self.odometry.child_frame_id = "base_link"
self.odometry.pose.covariance = COV_MATRIX
self.odometry.twist.covariance = COV_MATRIX
self.odometry.pose.pose.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, 0))
def update_odometry(self, data):
self.current_time = rospy.get_rostime()
dt = self.current_time.to_sec() - self.last_time.to_sec()
self.odometry.header.stamp = self.current_time
if data.direction_switch == 0:
data.vehicle_velocity = -1.0 * data.vehicle_velocity
data.steer_angle += STEEA_ANGLE_OFFSET
if abs(data.steer_angle) < 2.0:
data.steer_angle = 0
v = data.vehicle_velocity
w = data.vehicle_velocity * math.tan(
data.steer_angle * math.pi / 180.0) / WHEEL_BASE
if data.steer_angle > 0:
w = w / LEFT_TURN_MAGNIFICATION
else:
w = w / RIGHT_TURN_MAGNIFICATION
deltaTheta = w * dt
last_odom = self.odometry
(roll, pitch, yaw) = euler_from_quaternion([
last_odom.pose.pose.orientation.x,
last_odom.pose.pose.orientation.y,
last_odom.pose.pose.orientation.z,
last_odom.pose.pose.orientation.w
])
print yaw
self.odometry.pose.pose.position.x += v * dt * math.cos(
yaw + deltaTheta / 2.0)
self.odometry.pose.pose.position.y += v * dt * math.sin(
yaw + deltaTheta / 2.0)
self.odometry.twist.twist.linear.x = v * math.cos(yaw)
self.odometry.twist.twist.linear.y = v * math.sin(yaw)
self.odometry.twist.twist.angular.z = w
self.odometry.pose.pose.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(0, 0, yaw + deltaTheta))
self.last_time = rospy.get_rostime()
self.t = self.t + dt
def publish_loop(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
self.pub.publish(self.odometry)
rate.sleep()