def publish(self, throttle, brake, steer):
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
# using brake pedal command type as percentage , same as used for throttle
bcmd.pedal_cmd_type = BrakeCmd.CMD_PERCENT
if self.brake_deadband > 0.1:
brake *= self.percentagefactor
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
"""publish
Publish the control values to the cmd nodes
:param throttle:
:param brake:
:param steer:
"""
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer, _time=0, _state=0):
try:
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
rospy.loginfo(
'{} : PUBLISH_throttle:{} , brake:{} , steer:{}, light:{}'.
format(_time, throttle, brake, steer, _state))
except Exception as err:
rospy.loginfo('v_error: ERROR {} '.format(err))
def publish(self, throttle, brake, steer):
# Create brake command
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
# Create throttle command
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
# Create steering command
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
# Publish
# NOTE: do not publish throttle and brake at the same time,
# unless we switch from one to the other (to prevent the simulator
# from keeping the last value)
action = 'brake' if brake > 0.0 else 'throttle'
if action != self.last_action:
self.brake_pub.publish(bcmd)
self.throttle_pub.publish(tcmd)
elif action == 'brake':
self.brake_pub.publish(bcmd)
elif action == 'throttle':
self.throttle_pub.publish(tcmd)
self.steer_pub.publish(scmd)
self.last_action = action
def __init__(self):
self.steering = SteeringCmd()
self.steering.enable = True
self.steering.steering_wheel_angle_cmd = 0
self.accel = ThrottleCmd()
self.accel.enable = True
self.accel.pedal_cmd = 0
self.accel.pedal_cmd_type = 2
self.brake = BrakeCmd()
self.brake.enable = True
self.brake.pedal_cmd = 0
self.brake.pedal_cmd_type = 2
self.gear = GearCmd()
self.gear.cmd.gear = 1
self.parking_state = 0
#self.turn_signal = TurnSignalCmd()
self.pub_steering = rospy.Publisher('vehicle/steering_cmd',
SteeringCmd,
queue_size=10) # red car : vehicle
self.pub_brake = rospy.Publisher('vehicle/brake_cmd',
BrakeCmd,
queue_size=10)
self.pub_accel = rospy.Publisher('vehicle/throttle_cmd',
ThrottleCmd,
queue_size=10)
self.pub_gear = rospy.Publisher('vehicle/gear_cmd',
GearCmd,
queue_size=10)
#pub_turn_signal = rospy.Publisher('vehicle/turn_signal_cmd', TurnSignalCmd, queue_size=10)
rospy.init_node('controller', anonymous=True)
self.sub = rospy.Subscriber('/joy', Joy, self.callback, queue_size=10)
def publish(self, throttle, brake, steer):
# Either publish throttle or brake commands, but never both.
# This is not quite correct, as there are situation where we might want
# to simultaneously break and speed up, such as when starting on a hill.
# For the purpose of the simulator though, only one of them makes sense.
if brake > 0.:
throttle = 0
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
def __init__(self, vehicle = 'mkz'):
self.gen_files_path = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/course_txt.txt'
if vehicle == 'mkz':
self.trkr_out_node = 'mkz_ptracker'
self.trkr_out_topic = '/mkz/ptracker'
self.feedback_out_topic = '/mkz/odom'
self.thrtl_topic = '/mkz/throttle_cmd'
self.brake_topic = '/mkz/brake_cmd'
self.steer_topic = '/mkz/steering_cmd'
self.gear_topic = '/mkz/gear_cmd'
self.cmd_vel_topic = '/mkz/cmd_vel'
self.turn_signal_topic = '/mkz/turn_signal_cmd'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif vehicle == 'blue':
self.trkr_out_node = 'blue_ptracker'
self.trkr_out_topic = '/blue/ptracker'
self.feedback_out_topic = '/blue/odom'
self.thrtl_topic = '/blue/throttle_cmd'
self.brake_topic = '/blue/brake_cmd'
self.steer_topic = '/blue/steering_cmd'
self.cmd_vel_topic = '/blue/cmd_vel'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = Float64()
self.brake_msg_t = Float64()
self.steer_msg_t = Float64()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
def publish(self, throttle, brake, steer):
"""
Publishes the ROS topics.
:param throttle: throttle in percentage.
:param brake: brake in torque.
:param steer: steering angle.
"""
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
"""Publishes throttle, brake, and steering to their respective topics
Args:
throttle (float): updated throttle value for vehicle control
brake (float): updated brake value for vehicle control
steer (float): updated steering value for vehicle control
"""
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
"""Publishes control commands (throttle, brake, steering)
to be executed by the vehicle"""
# rospy.logwarn("Publish throttle: {0}".format(throttle))
# rospy.logwarn("Publish brake: {0}".format(brake))
# rospy.logwarn("Publish steer: {0}".format(steer))
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def publish(self, throttle, brake, steer):
#if we want to force the car just move forward,
# set throttle to 1, others are 0
#throttle 0~1
tcmd = ThrottleCmd()
tcmd.enable = True
tcmd.pedal_cmd_type = ThrottleCmd.CMD_PERCENT
tcmd.pedal_cmd = throttle
self.throttle_pub.publish(tcmd)
scmd = SteeringCmd()
scmd.enable = True
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
#brake in units of torque (N*m)
# can be computed using the desired acceleration, weight of the vehicle, and wheel radius.
bcmd = BrakeCmd()
bcmd.enable = True
bcmd.pedal_cmd_type = BrakeCmd.CMD_TORQUE
bcmd.pedal_cmd = brake
self.brake_pub.publish(bcmd)
def __init__(self, vehicle_ns='fusion'):
self.ns = vehicle_ns
if vehicle_ns == 'fusion':
self.llc_out_node = 'fusion_llc'
self.llc_out_topic = '/fusion/llc'
self.trkr_out_topic = '/fusion/ptracker'
self.feedback_out_topic = '/fusion/odom'
self.thrtl_topic = '/fusion/throttle_cmd'
self.brake_topic = '/fusion/brake_cmd'
self.steer_topic = '/fusion/steering_cmd'
self.gear_topic = '/fusion/gear_cmd'
self.cmd_vel_topic = '/fusion/cmd_vel'
self.turn_signal_topic = '/fusion/turn_signal_cmd'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif vehicle_ns == 'mkz':
self.llc_out_node = 'mkz_llc'
self.llc_out_topic = '/mkz/llc'
self.trkr_out_topic = '/mkz/ptracker'
self.feedback_out_topic = '/mkz/odom'
self.thrtl_topic = '/mkz/throttle_cmd'
self.brake_topic = '/mkz/brake_cmd'
self.steer_topic = '/mkz/steering_cmd'
self.gear_topic = '/mkz/gear_cmd'
self.cmd_vel_topic = '/mkz/cmd_vel'
self.turn_signal_topic = '/mkz/turn_signal_cmd'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif vehicle_ns == 'blue':
self.llc_out_node = 'blue_llc'
self.llc_out_topic = '/blue/llc'
self.trkr_out_topic = '/blue/ptracker'
self.feedback_out_topic = '/blue/odom'
self.thrtl_topic = '/blue/throttle_cmd'
self.brake_topic = '/blue/brake_cmd'
self.steer_topic = '/blue/steering_cmd'
self.cmd_vel_topic = '/blue/cmd_vel'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = Float64()
self.brake_msg_t = Float64()
self.steer_msg_t = Float64()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif vehicle_ns == 'orange':
self.llc_out_node = 'orange_llc'
self.llc_out_topic = '/orange/llc'
self.trkr_out_topic = '/orange/ptracker'
self.feedback_out_topic = '/orange/odom'
self.thrtl_topic = '/orange/throttle_cmd'
self.brake_topic = '/orange/brake_cmd'
self.steer_topic = '/orange/steering_cmd'
self.cmd_vel_topic = '/orange/cmd_vel'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = Float64()
self.brake_msg_t = Float64()
self.steer_msg_t = Float64()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
def main(dt, t_vel, ref_index, p_k, p_d, p_i):
global X_ref, Y_ref, psi_ref, vx_ref, vy_ref, Waypoints_received, stateEstimate_mark, state_received
global error_d, error_i, error_p
rospy.init_node('pid', anonymous=True)
rospy.Subscriber('/waypoints', Waypoints, WaypointsCallback)
rospy.Subscriber('state_estimate', state_Dynamic, stateEstimateCallback)
pub = rospy.Publisher('/vehicle/cmd_vel_stamped',
TwistStamped,
queue_size=1)
pub2 = rospy.Publisher('vehicle/steering_cmd', SteeringCmd, queue_size=1)
rate = rospy.Rate(1 / dt)
error_p = 0
error_i = 0
error_d = 0
while (rospy.is_shutdown() != 1):
if stateEstimate_mark and Waypoints_mark:
#print(len(Waypoints_received.points), "numwaypints")
num_steps_received = len(Waypoints_received.points) - 1
dt_received = Waypoints_received.dt
horizon = dt_received * num_steps_received
points = np.zeros((2, num_steps_received))
# ToDO
''' wp = waypoints[6] # 0.8 seconds in the future
theta = math.atan2(max(wp[0], 0.01), wp[1]) - math.pi / 2 # range from -pi/2 to pi/2
if waypoints[-2][0] < 0.5:
theta = 0.0
'''
if Waypoints_received.points[-2].x < 0.5:
error_theta = 0.0
else:
theta_tot = 0.0
weighting = np.array([2, 4, 6, 6, 3, 2, 1]) # 7 values
weighting = weighting * 1.0 / np.sum(
weighting) * weighting.size
factor = 2
for i in range(num_steps_received // factor):
wp = Waypoints_received.points[
i] # 0.8 seconds in the future
wp = [wp.x, wp.y]
theta = math.atan2(
max(wp[0], 0.01),
wp[1]) - math.pi / 2 # range from -pi/2 to pi/2
theta_tot += theta * weighting[i]
error_theta = theta_tot / (num_steps_received // factor)
for i in range(num_steps_received):
points[0, i] = Waypoints_received.points[i].x * cos(
psi_ref) - Waypoints_received.points[i].y * sin(
psi_ref) + X_ref
points[1, i] = Waypoints_received.points[i].x * sin(
psi_ref) + Waypoints_received.points[i].y * cos(
psi_ref) + Y_ref
t_received = np.linspace(dt_received, horizon, num_steps_received)
spl_x = UnivariateSpline(t_received, points[0, :], k=3)
spl_y = UnivariateSpline(t_received, points[1, :], k=3)
spl_x_dot = spl_x.derivative()
spl_y_dot = spl_y.derivative()
spl_x_dot_val = spl_x_dot(t_vel)
spl_y_dot_val = spl_y_dot(t_vel)
#spl_v_val = 1.0 * np.sqrt(spl_x_dot_val**2 + spl_y_dot_val**2)
#spl_v_val = 3.0 #this is for left turn
spl_v_val = 2.0 # for the right turns
twist_cmd = TwistStamped()
twist_cmd.twist.linear.x = spl_v_val
pub.publish(twist_cmd)
ref_index_act = min(ref_index, num_steps_received) - 1
ref_point = points[:, ref_index_act]
lateral_error = (ref_point[0] - X) * sin(psi) - (ref_point[1] -
Y) * cos(psi)
lateral_error = error_theta
print("error theta is ", error_theta)
error_i += dt * (lateral_error + error_p) / 2.
error_d = (lateral_error - error_p) / dt
error_p = lateral_error
steering_angle = -(p_k * error_p + error_i * p_i + error_d * p_d)
steering_cmd = SteeringCmd()
steering_cmd.enable = True
steering_cmd.steering_wheel_angle_cmd = steering_angle * steering_ratio
pub2.publish(steering_cmd)
rospy.loginfo('Lateral error: %f m, pk %f, id %d', lateral_error,
p_k, ref_index_act)
rate.sleep()
def steering_callback(self, steering_model_data):
#print("CALLBACK")
'''
The dbw_mkz SteeringCmd message looks like this:
# Steering Wheel
float32 steering_wheel_angle_cmd # rad, range -8.2 to 8.2
float32 steering_wheel_angle_velocity # rad/s, range 0 to 8.7, 0 = maximum
# Enable
bool enable
# Ignore driver overrides
bool ignore
# Disable the driver override audible warning
bool quiet
# Watchdog counter (optional)
uint8 count
'''
# Sometimes the neural net is faster, sometimes slower. Every time it processes an input
# image it will publish a steering command and that would affect the smoothing of the
# values. To produce a more stable behavior, a timer is used.
time_now = timer()
max_update_frequency = 15 #hz
time_passed = (time_now - self.last_execution_time) * 100.0
steeringCmd_msg = None
if time_passed * max_update_frequency > 100.0:
self.last_execution_time = time_now
steeringCmd_msg = SteeringCmd()
# The original steering values are in between 0 and 100 where 100 is max left and 0 is max right.
# This is first smoothed over 5 previous values and then converted to the range -8.2 to 8.2
# input from the callback
steering_lilliput = steering_model_data.data
# The queue is emptied on the left
self.previous_steering_commands.popleft()
# The newest value added
self.previous_steering_commands.append(steering_lilliput)
# The newest steering command is the weighted sum of the previous 6 input commands
# with a weighting vector, designed to change the value first by a large, later by a minor margin
steering_lilliput = np.sum(self.previous_steering_commands *
self.smoothing_vector)
# Norm to [0,1]
steering_norm = steering_lilliput / 100.0
# Regard DMZ limits
steer_max_left = -8.2 # rad
steer_max_right = 8.2 # rad
# Observe the range of the DMZ limits ( to make it possible to change those values later )
steer_range = np.abs(steer_max_left - steer_max_right)
# Map the normed steering values from lilliput to the dmz
steer_output_rad = (steering_norm * steer_range) - (steer_range /
2.0)
# Put it into a message
steeringCmd_msg.steering_wheel_angle_cmd = steer_output_rad
# Fill other fields
steeringCmd_msg.steering_wheel_angle_velocity = 1.0
steeringCmd_msg.enable = True
steeringCmd_msg.ignore = False
steeringCmd_msg.quiet = False
# If the watchdog counter should be increased do so
if (self.watchdog_counter_used):
steeringCmd_msg.count = self.counter
# If the watchdog counter rises to quickly there is
# another trick here to make him rise slower
if (self.watchdog_throttle):
if (np.random() > self.throttle_value):
self.counter = self.counter + 1
else:
self.counter = self.counter + 1
self.steeringCmd_msg = steeringCmd_msg
# The message has to be sent at a higher rate than
# the net is working so that rate is realized here
#if not self.thread_started:
# threading.Thread(target=self.sendMessageAtRate).start()
# self.thread_started = True
self.steer_pub.publish(self.steeringCmd_msg)
def main():
global vx, vy, X, Y, psi, wz, stateEstimate_mark, laneChange
# env, base policy, attribute policy and PAL-Net related
rospack = rospkg.RosPack()
model_path = os.path.join(rospack.get_path("planning_policy"), "trained_model")
config = dict()
config['mode'] = 'Imitation'
config['run_type'] = 'train'
config['continue'] = True
# construction configuration:
# driver problem
config['env_type'] = 'driver'
config['update_name'] = 'driver'
config['e_update_type'] = 'regular'
# network config:
network_config(config, model_path)
network = Palnet(config)
network.restore()
# start expert structure.
g1 = tf.Graph()
with g1.as_default():
expert = Expert(model_path)
expert.restore()
env = Driving(story_index=100, track_data='long_straight', lane_deviation=9.5, dt=0.02)
P = np.array([[100, 0], [0, 1]])
solvers.options['show_progress'] = False # don't let cvxopt print iterations
# define the initial states and timestep
stateEstimate_mark = False
# import track file
rospy.init_node('RL_control', anonymous=True)
rospy.Subscriber('state_estimate', state_Dynamic, stateEstimateCallback)
rospy.Subscriber('lane_signal', Int8, laneChangeCallback)
vel_cmd_pub = rospy.Publisher('/vehicle/cmd_vel_stamped', TwistStamped, queue_size=1)
steering_cmd_pub = rospy.Publisher('/vehicle/steering_cmd', SteeringCmd, queue_size=1)
dt = 0.02
rate = rospy.Rate(1 / dt)
# get the sim_env ready
env.reset()
while (rospy.is_shutdown() != 1):
if stateEstimate_mark:
env.ego.state = np.array([X, Y, vx, psi])
# deal with keyboard lane change command input
if laneChange != 0:
env.hand_lanechange(laneChange)
laneChange = 0
# a dirty trick, if lane change then disable future lane change
if env.ego.track_select == 1:
env.hand_lanechange(0)
# get the initial observation and obstacle ref
env.get_all_ref()
env.ego.track_select = env.ego.closest_track
ob = np.append(env.ego.state[2], env.ego_track_ref_list[env.ego.track_select])
obstacle_ref_list = env.ego_obstacle_ref_list
ac0 = expert.choose_action(ob)
dudt0 = np.multiply(ac0[:, np.newaxis], np.array([[5], [0.5]]))
if len(obstacle_ref_list):
# extract parameters from obstacle_ref_list
data = {'state0': np.vstack(obstacle_ref_list)[:, :10]}
feed_data = network.get_feed_dict(data)
ob_param = network.sess.run(network.means[network.index], feed_data)
# None, 3.
M = ob_param[:, :2]
b = -ob_param[:, -1:]
if env.ego.track_select == 1:
M[0,0] = 0
M[0,1] = 0
b[0,0] = 1
try:
sol = solvers.qp(P=matrix(0.5 * P), q=matrix(- np.matmul(P, dudt0)), G=matrix(M), h=matrix(b))
except:
# if dAger is not useful, transfer back.
print("RL_control:Something wrong with dAger run.")
num = len(obstacle_ref_list)
for i in range(num):
obstacle_ref = obstacle_ref_list[i]
A = obstacle_ref[10]
B = obstacle_ref[11]
C = obstacle_ref[12]
M[i, 0] = A
M[i, 1] = B
b[i, 0] = -C
sol = solvers.qp(P=matrix(0.5 * P), q=matrix(- np.matmul(P, dudt0)), G=matrix(M), h=matrix(b))
dudt = sol['x']
else:
dudt = dudt0
ac = np.zeros(2)
ac[0] = dudt[0] / 5
ac[1] = dudt[1] / 0.5
np.clip(ac, -1, 1, out=ac)
cmd_vel_stamped = TwistStamped()
cmd_vel_stamped.twist.linear.x = ac[0] * 5 * dt + vx
vel_cmd_pub.publish(cmd_vel_stamped)
steering_cmd = SteeringCmd()
steering_cmd.enable = True
beta = asin(ac[1] * 0.5 * 1.65 / vx)
delta = atan(tan(beta) * (1.65 + 1.20) / 1.65)
steering_cmd.steering_wheel_angle_cmd = delta * 14.8
steering_cmd_pub.publish(steering_cmd)
rate.sleep()
def run(self):
# Initialize all the publishers.
self.enable_pub = rospy.Publisher(ENABLE_TOPIC, Empty, queue_size=10)
self.disable_pub = rospy.Publisher(DISABLE_TOPIC, Empty, queue_size=10)
self.throttle_pub = rospy.Publisher(THROTTLE_TOPIC,
ThrottleCmd,
queue_size=10)
self.brake_pub = rospy.Publisher(BRAKE_TOPIC, BrakeCmd, queue_size=10)
self.steering_pub = rospy.Publisher(STEERING_TOPIC,
SteeringCmd,
queue_size=10)
rospy.init_node(self.config.name, anonymous=True, disable_signals=True)
# Enable the ADAS.
#self.enable_pub.publish(Empty())
# Pull from the control stream and publish messages continuously.
r = rospy.Rate(ROS_FREQUENCY)
last_control_message = ControlMessage(
steer=0,
throttle=0,
brake=0,
hand_brake=False,
reverse=False,
timestamp=erdos.Timestamp(coordinates=[0]))
while not rospy.is_shutdown():
control_message = self._control_stream.try_read()
if control_message is None or isinstance(control_message,
erdos.WatermarkMessage):
control_message = last_control_message
else:
last_control_message = control_message
# Send all the commands from a single ControlMessage one after
# the other.
steer_angle = control_message.steer * STEERING_ANGLE_MAX
self._logger.debug("The steering angle is {}".format(steer_angle))
steer_message = SteeringCmd(enable=True,
clear=True,
ignore=False,
quiet=False,
count=0,
steering_wheel_angle_cmd=steer_angle,
steering_wheel_angle_velocity=0.0)
self.steering_pub.publish(steer_message)
throttle_message = ThrottleCmd(
enable=True,
ignore=False,
count=0,
pedal_cmd_type=ThrottleCmd.CMD_PERCENT,
pedal_cmd=control_message.throttle)
self.throttle_pub.publish(throttle_message)
brake_message = BrakeCmd(enable=True,
ignore=False,
count=0,
pedal_cmd_type=BrakeCmd.CMD_PERCENT,
pedal_cmd=control_message.brake)
self.brake_pub.publish(brake_message)
# Run at frequency
r.sleep()
def publish_steering(self, steer):
scmd = SteeringCmd()
scmd.enable = True
#rospy.loginfo('Proposed Steering angle command : %f', steer)
scmd.steering_wheel_angle_cmd = steer
self.steer_pub.publish(scmd)
def __init__(self, vehicle_ns):
self.ns = vehicle_ns
self.gen_file_cont = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/controller_output.txt'
self.gen_file_cont_fb = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/cont_fb_out.txt'
self.gen_file_odom = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/odom_data_file.txt'
self.gen_file_course = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/course_txt.txt'
self.gen_file_x_path = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/x_path_txt.txt'
self.gen_file_y_path = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/y_path_txt.txt'
self.gen_file_yaw_path = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/yaw_path_txt.txt'
self.gen_gile_max_speed = '/home/oks/catkin_ws/src/framework_sim/gen_txtfiles/target_max_speed.txt'
if self.ns == 'fusion':
self.llc_out_node = 'fusion_llc'
self.trkr_out_node = 'fusion_ptracker'
self.llc_out_topic = '/fusion/llc'
self.trkr_out_topic = '/fusion/ptracker'
self.feedback_out_topic = '/fusion/odom'
self.thrtl_topic = '/fusion/throttle_cmd'
self.brake_topic = '/fusion/brake_cmd'
self.steer_topic = '/fusion/steering_cmd'
self.gear_topic = '/fusion/gear_cmd'
self.cmd_vel_topic = '/fusion/cmd_vel'
self.turn_signal_topic = '/fusion/turn_signal_cmd'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif self.ns == 'mkz':
self.llc_out_node = 'mkz_llc'
self.llc_out_topic = '/mkz/llc'
self.trkr_out_node = 'mkz_ptracker'
self.trkr_out_topic = '/mkz/ptracker'
self.feedback_out_topic = '/mkz/odom'
self.thrtl_topic = '/mkz/throttle_cmd'
self.brake_topic = '/mkz/brake_cmd'
self.steer_topic = '/mkz/steering_cmd'
self.gear_topic = '/mkz/gear_cmd'
self.cmd_vel_topic = '/mkz/cmd_vel'
self.turn_signal_topic = '/mkz/turn_signal_cmd'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = ThrottleCmd()
self.brake_msg_t = BrakeCmd()
self.steer_msg_t = SteeringCmd()
self.gear_msg_t = GearCmd()
self.turn_signal_msg_t = TurnSignalCmd()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif self.ns == 'blue':
self.llc_out_node = 'blue_llc'
self.trkr_out_node = 'blue_ptracker'
self.llc_out_topic = '/blue/llc'
self.trkr_out_topic = '/blue/ptracker'
self.feedback_out_topic = '/blue/odom'
self.thrtl_topic = '/blue/throttle_cmd'
self.brake_topic = '/blue/brake_cmd'
self.steer_topic = '/blue/steering_cmd'
self.cmd_vel_topic = '/blue/cmd_vel'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = Float64()
self.brake_msg_t = Float64()
self.steer_msg_t = Float64()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
elif self.ns == 'orange':
self.llc_out_node = 'orange_llc'
self.trkr_out_node = 'orange_ptracker'
self.llc_out_topic = '/orange/llc'
self.trkr_out_topic = '/orange/ptracker'
self.feedback_out_topic = '/orange/odom'
self.thrtl_topic = '/orange/throttle_cmd'
self.brake_topic = '/orange/brake_cmd'
self.steer_topic = '/orange/steering_cmd'
self.cmd_vel_topic = '/orange/cmd_vel'
self.feedback_msg_t = Odometry()
self.thrtl_msg_t = Float64()
self.brake_msg_t = Float64()
self.steer_msg_t = Float64()
self.cmd_vel_msg_t = Twist()
self.cmd_list_msg_t = Float64MultiArray()
self.time_step = 0.1
def main(dt):
global Waypoints_received, condition
rospy.init_node('pid', anonymous=True)
rospy.Subscriber('/waypoints', Waypoints, WaypointsCallback)
pub = rospy.Publisher('/vehicle/cmd_vel_stamped',
TwistStamped,
queue_size=1)
pub2 = rospy.Publisher('vehicle/steering_cmd', SteeringCmd, queue_size=1)
rospy.Subscriber("/vehicle/mkz_key_command",
String,
on_key_received,
queue_size=10)
rate = rospy.Rate(1 / dt)
error_p = 0
error_i = 0
error_d = 0
while (rospy.is_shutdown() != 1):
if Waypoints_mark:
# all parameter's default values
spl_v_val = 3.0
p_d = 0.0
p_i = 0.0
p_k = 1.2
use_those_waypoints = range(
len(Waypoints_received.points) // 2 - 1)
if condition == "w":
pass
elif condition == "a":
# left
use_those_waypoints = range(
len(Waypoints_received.points) // 2 - 1)
#p_k = 0.9
spl_v_val = 3.0
elif condition == "d":
# right
#p_k = 1.35
#p_k = 0.6
#p_k = 0.9
spl_v_val = 2.0
use_those_waypoints = range(
len(Waypoints_received.points) // 2 - 1)
elif condition == "s":
pass
else:
print("unknown condition ", condition)
if Waypoints_received.points[-2].x < 0.5:
error_theta = 0.0
else:
theta_tot = 0.0
#weighting = np.array([2, 4, 6, 6, 3, 2, 1]) # 7 values
#weighting = weighting * 1.0 / np.sum(weighting) * weighting.size
for i in use_those_waypoints:
wp = Waypoints_received.points[
i] # 0.8 seconds in the future
wp = [wp.x, wp.y]
theta = math.atan2(
max(wp[0], 0.01),
wp[1]) - math.pi / 2 # range from -pi/2 to pi/2
theta_tot += theta #* weighting[i]
error_theta = theta_tot / len(use_those_waypoints)
lateral_error = error_theta
twist_cmd = TwistStamped()
twist_cmd.twist.linear.x = spl_v_val
pub.publish(twist_cmd)
error_i += dt * (lateral_error + error_p) / 2.
error_d = (lateral_error - error_p) / dt
error_p = lateral_error
steering_angle = -(p_k * error_p + error_i * p_i + error_d * p_d)
steering_cmd = SteeringCmd()
steering_cmd.enable = True
steering_cmd.steering_wheel_angle_cmd = steering_angle * steering_ratio
pub2.publish(steering_cmd)
rospy.loginfo('error: %f m, pk %f, speed %f, mode %s',
lateral_error, p_k, spl_v_val, condition)
print("using those waypoints ", use_those_waypoints)
# collecting useful info and publish to a message topic
message = "P={:.2f} Steer={:.2f} Target Speed={:.2f} m/s \nActiveWP={} \nController Condition={}\n".format(
p_k, steering_cmd.steering_wheel_angle_cmd, spl_v_val,
str(use_those_waypoints), condition)
global controller_message_pub
controller_message_pub.publish(message)
rate.sleep()
def __init__(self):
rospy.init_node("rl_sim_env")
#self.initial_model_state = None
rospy.wait_for_service("/gazebo/set_model_state")
self.set_state_pub = rospy.Publisher("/gazebo/set_model_state",
ModelState,
queue_size=10)
#self.set_state_proxy = rospy.ServiceProxy("/gazebo/set_model_state",SetModelState)
self.car_name = "vehicle"
self.init_pose = Pose(Point(-180, -2.3, 0.1), Quaternion(0, 0, 0, 1))
self.init_state = ModelState()
self.init_state.model_name = self.car_name
self.init_state.pose = self.init_pose
self.steering = SteeringCmd()
self.steering.enable = True
self.steering.steering_wheel_angle_cmd = 0
self.accel = ThrottleCmd()
self.accel.enable = True
self.accel.pedal_cmd = 0
self.accel.pedal_cmd_type = 2
self.brake = BrakeCmd()
self.brake.enable = True
self.brake.pedal_cmd = 0
self.brake.pedal_cmd_type = 2
self.gear = GearCmd()
self.gear.cmd.gear = 1
self.pub_steering = rospy.Publisher('vehicle/steering_cmd',
SteeringCmd,
queue_size=10) # red car : vehicle
self.pub_accel = rospy.Publisher('vehicle/throttle_cmd',
ThrottleCmd,
queue_size=10)
self.pub_brake = rospy.Publisher('vehicle/brake_cmd',
BrakeCmd,
queue_size=10)
self.pub_gear = rospy.Publisher('vehicle/gear_cmd',
GearCmd,
queue_size=10)
self.bridge = CvBridge()
self.feature_state = np.zeros(10)
self.deviation = 0
self.dev_ = 0
self.reach_goal = False
self.out_of_lane = False
self.x_coord = self.init_pose.position.x
self.goal_x = 180
self.state_sub = rospy.Subscriber('gazebo/model_states', ModelStates,
self.state_callback)
self.lane_sub = rospy.Subscriber('/vehicle/lane_number', Int32,
self.lane_callback)
self.deviation_sub = rospy.Subscriber('/vehicle/deviation', Float32,
self.deviation_callback)
self.observation_space = 8
self.action_space = 3
self.time_limit = 100
self.time = 0
