def on_can_message(self, msg):
if msg.interface == CanMessage.CANTYPE_CONTROL:
if msg.id in self.kia_db._frame_id_to_message:
# Kia CAN messageSTEERING_ANGLE_angle
kia_can_msg = self.kia_db.decode_message(msg.id, bytearray(msg.data))
msg_type = self.kia_db.get_message_by_frame_id(msg.id)
if msg_type.name == "STEERING_ANGLE":
self.steering_angle_pub.publish(Float32(data=float(kia_can_msg["STEERING_ANGLE_angle"])))
elif msg_type.name == "SPEED":
# print(kia_can_msg)
speed = util.mph_to_ms(float(kia_can_msg["SPEED_rear_left"]))
self.on_speed(speed, msg.can_timestamp)
elif msg.id in self.oscc_db._frame_id_to_message:
# OSCC Message. Currently this only publishes 0 or 1 to indicate
# enabled or not. In the future this should be changed to
# throttle/brake/steering values but that requires a firmware change
# to the OSCC
return
oscc_can_msg = self.oscc_db.decode_message(msg.id, bytearray(msg.data))
if oscc_can_msg.name == "BRAKE_REPORT":
self.brake_pedal_pub.publish(oscc_can_msg.brake_report_enabled)
elif oscc_can_msg.name == "STEERING_REPORT":
self.steering_torque.publish(oscc_can_msg.steering_report_enabled)
elif oscc_can_msg.name == "THROTTLE_REPORT":
self.accel_pedal_pub.publish(oscc_can_msg.throttle_report_enabled)
def on_can_message(self, msg):
if msg.interface == self.CANTYPE_CONTROL:
if msg.id in self.kia_db._frame_id_to_message:
# Kia CAN messageSTEERING_ANGLE_angle
kia_can_msg = self.kia_db.decode_message(
msg.id, bytearray(msg.data))
msg_type = self.kia_db.get_message_by_frame_id(msg.id)
if msg_type.name == "STEERING_ANGLE":
steering_wheel_angle = float(
kia_can_msg["STEERING_ANGLE_angle"]) * math.pi / 180.0
steering_wheel_angle_msg = Float32(
data=steering_wheel_angle)
yaw_angle_msg = Float32(data=steering_wheel_angle /
KIA_SOUL_STEERING_RATIO)
self.steering_wheel_angle_raw_pub.publish(
steering_wheel_angle_msg)
self.steering_angle_raw_pub.publish(yaw_angle_msg)
joint_msg = JointState()
joint_msg.header.stamp = rospy.Time.from_seconds(
msg.can_timestamp)
joint_msg.name = [
"steering_joint", "yaw", "front_left_steer_joint",
"front_right_steer_joint"
]
joint_msg.position = [
steering_wheel_angle,
steering_wheel_angle / KIA_SOUL_STEERING_RATIO,
steering_wheel_angle / KIA_SOUL_STEERING_RATIO,
steering_wheel_angle / KIA_SOUL_STEERING_RATIO
]
self.steering_joint_states_pub.publish(joint_msg)
self.calc_steering_accel(steering_wheel_angle,
msg.can_timestamp)
elif msg_type.name == "SPEED":
# print(kia_can_msg)
speed = util.mph_to_ms(
float(kia_can_msg["SPEED_rear_left"]))
self.on_speed(speed, msg.can_timestamp)
elif msg.id in self.oscc_db._frame_id_to_message:
# OSCC Message. Currently this only publishes 0 or 1 to indicate
# enabled or not. In the future this should be changed to
# throttle/brake/steering values but that requires a firmware change
# to the OSCC
oscc_can_msg = self.oscc_db.decode_message(
msg.id, bytearray(msg.data))
msg_type = self.oscc_db.get_message_by_frame_id(msg.id)
# if oscc_can_msg.name == "BRAKE_REPORT":
# # self.brake_pedal_pub.publish(oscc_can_msg.brake_report_enabled)
# elif oscc_can_msg.name == "STEERING_REPORT":
# self.steering_torque.publish(oscc_can_msg.steering_report_enabled)
# el
if msg_type.name == "STEERING_REPORT":
_, _, _, torque = struct.unpack_from("hccf", msg.data)
self.steering_torque.publish(Float32(data=torque / 12.7))
self.last_torque = torque
def __init__(self, distance, velocity, accel):
super().__init__('fake_lead_vehicle')
self.distance = float(distance)
self.velocity = util.mph_to_ms(float(velocity))
self.accel = float(accel)
self.time_since_last_update = None
self.last_ego_speed = None
self.current_ego_speed = 0
self.lead_vehicle_pub = self.create_publisher(LeadVehicle, '/lead_vehicle')
self.lead_vehicle_velocity = self.create_publisher(Float32, '/lead_vehicle_velocity')
self.lead_vehilce_distance = self.create_publisher(Float32, '/lead_vehicle_distance')
self.velocity_sub = self.create_subscription(Float32, 'wheel_speed', self.on_wheel_speed)
self.controls_enabled = self.create_subscription(Bool, 'controls_enable', self.on_controls_enable)
self.tick_timer = self.create_timer(1. /10, self.tick)
self.controls_enabled = False
def __init__(self):
self.T = 20
self.dt = 0.2
self.min_follow_distance = cvx.Parameter(value=6.0)
self.max_acceleration = cvx.Parameter(value=5.0)
self.min_acceleration = cvx.Parameter(value=-5.0)
self.min_max_jerk = cvx.Parameter(value=3.0)
self.cruising_speed = cvx.Parameter(
value=util.mph_to_ms(INITIAL_CRUISING_SPEED))
self.last_v_trajectory = None
self.last_a_trajectory = None
self.last_x_trajectory = None
self.v_ego = cvx.Parameter(value=0.0)
self.a_ego = cvx.Parameter(value=0.0)
self.x_lead = cvx.Parameter(value=INITIAL_DISTANCE_TO_LEAD_CAR)
self.v_lead = cvx.Parameter(value=self.cruising_speed.value)
self.a_lead = cvx.Parameter(value=0.0)
self.v = cvx.Variable(self.T + 1)
self.a = cvx.Variable(self.T + 1)
self.j = cvx.Variable(self.T)
self.x = cvx.Variable(self.T + 1)
self.solver = self.init_mpc_solver()
self.cruising_speed_sub = rospy.Subscriber('cruising_speed', Float32,
self.on_cruising_speed)
self.speed_sub = rospy.Subscriber('wheel_speed', Float32,
self.on_wheel_speed)
self.lead_car_sub = rospy.Subscriber('lead_obstacle', Obstacle,
self.on_lead_obstacle)
self.computed_accel_sub = rospy.Subscriber('computed_accel_filtered',
Float32,
self.on_computed_accel)
self.plan_pub = rospy.Publisher('longitudinal_plan',
LongitudinalPlan,
queue_size=1)
rate = rospy.Rate(RATE)
while not rospy.is_shutdown():
self.make_plan()
rate.sleep()
def on_can_message(self, msg, can_timestamp):
if msg.arbitration_id in self.kia_db._frame_id_to_message:
# Kia CAN messageSTEERING_ANGLE_angle
kia_can_msg = self.kia_db.decode_message(msg.arbitration_id,
bytearray(msg.data))
msg_type = self.kia_db.get_message_by_frame_id(msg.arbitration_id)
if msg_type.name == "STEERING_ANGLE":
steering_wheel_angle = float(
kia_can_msg["STEERING_ANGLE_angle"]) * math.pi / 180.0
steering_wheel_angle_msg = Float32(data=steering_wheel_angle)
yaw_angle_msg = Float32(data=steering_wheel_angle /
KIA_SOUL_STEERING_RATIO)
self.steering_wheel_angle_raw_pub.publish(
steering_wheel_angle_msg)
self.steering_angle_raw_pub.publish(yaw_angle_msg)
joint_msg = JointState()
joint_msg.header.stamp = rospy.Time.from_seconds(can_timestamp)
joint_msg.name = [
"steering_joint", "yaw", "front_left_steer_joint",
"front_right_steer_joint"
]
joint_msg.position = [
steering_wheel_angle,
steering_wheel_angle / KIA_SOUL_STEERING_RATIO,
steering_wheel_angle / KIA_SOUL_STEERING_RATIO,
steering_wheel_angle / KIA_SOUL_STEERING_RATIO
]
self.steering_joint_states_pub.publish(joint_msg)
elif msg_type.name == "SPEED":
# print(kia_can_msg)
speed = util.mph_to_ms(float(kia_can_msg["SPEED_rear_left"]))
self.on_speed(speed, can_timestamp)
elif msg.arbitration_id in self.oscc_db._frame_id_to_message:
# OSCC Message. Currently this only publishes 0 or 1 to indicate
# enabled or not. In the future this should be changed to
# throttle/brake/steering values but that requires a firmware change
# to the OSCC
oscc_can_msg = self.oscc_db.decode_message(msg.arbitration_id,
bytearray(msg.data))
msg_type = self.oscc_db.get_message_by_frame_id(msg.arbitration_id)
def __init__(self):
super(LongitudinalPlanner, self).__init__('longitudinal_planner')
self.T = 20
self.dt = 0.2
self.min_follow_distance = cvx.Parameter(value=6.0)
self.max_acceleration = cvx.Parameter(value=5.0)
self.min_acceleration = cvx.Parameter(value=-5.0)
self.min_max_jerk = cvx.Parameter(value=3.0)
self.cruising_speed = cvx.Parameter(
value=util.mph_to_ms(INITIAL_CRUISING_SPEED))
self.last_v_trajectory = None
self.last_a_trajectory = None
self.last_x_trajectory = None
self.v_ego = cvx.Parameter(value=0.0)
self.a_ego = cvx.Parameter(value=0.0)
self.x_lead = cvx.Parameter(value=INITIAL_DISTANCE_TO_LEAD_CAR)
self.v_lead = cvx.Parameter(value=self.cruising_speed.value)
self.a_lead = cvx.Parameter(value=0.0)
self.v = cvx.Variable(self.T + 1)
self.a = cvx.Variable(self.T + 1)
self.j = cvx.Variable(self.T)
self.x = cvx.Variable(self.T + 1)
self.solver = self.init_mpc_solver()
self.cruising_speed_sub = self.create_subscription(
Float32, 'cruising_speed', self.on_cruising_speed)
self.speed_sub = self.create_subscription(Float32, 'wheel_speed',
self.on_wheel_speed)
self.lead_car_sub = self.create_subscription(LeadVehicle,
'lead_vehicle',
self.on_lead_vehicle)
self.imu_sub = self.create_subscription(Imu, 'imu', self.on_imu)
self.computed_accel_sub = self.create_subscription(
Float32, 'computed_accel_filtered', self.on_computed_accel)
self.plan_pub = self.create_publisher(LongitudinalPlan,
'longitudinal_plan')
self.timer = self.create_timer(1.0 / 4.0, self.make_plan)
