def send_motor_command(self, velocity, steer_angle):
'''publish motor commands to drive and steering servo'''
ackermann_cmd = AckermannDriveStamped()
ackermann_cmd.header = self.create_header(self.car_frame)
ackermann_cmd.drive.steering_angle = steer_angle
self.current_steering = steer_angle
ackermann_cmd.drive.speed = velocity
# ackermann_cmd.drive.acceleration = 1.0
self.cmd_vel_pub.publish(ackermann_cmd)
def stop(self):
print "Stopping"
drive_msg_stamped = AckermannDriveStamped()
drive_msg_stamped.header = utils.make_header("/base_link")
drive_msg = AckermannDrive()
drive_msg.speed = 0
drive_msg.steering_angle = 0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.control_pub.publish(drive_msg_stamped)
def con_input():
# Starts a new node
rospy.init_node('con_in_node', anonymous=True)
rate = rospy.Rate(20) # 20hz
con_in_pub = rospy.Publisher(
'/vesc/high_level/ackermann_cmd_mux/input/nav_0',
AckermannDriveStamped,
queue_size=10)
#Setting the current time for distance calculus
# let's build a 3x3 matrix:
drive_in = AckermannDriveStamped()
drive_in.header = Header()
drive_in.header.frame_id = 'base_link'
drive_in.header.stamp = rospy.Time.now()
drive_in.header.seq = 0
drive_in.drive = AckermannDrive()
drive_in.drive.steering_angle = 0
drive_in.drive.steering_angle_velocity = 0
drive_in.drive.speed = 0
drive_in.drive.acceleration = 0
drive_in.drive.jerk = 0
os.system("read -p 'Press Enter to continue...' var")
#print('Any key to start')
while not rospy.is_shutdown():
t0 = rospy.Time.now().to_sec()
t1 = t0
i = 0
while (t1 - t0 < 200):
#rospy.loginfo("t:%f\n",t1 - t0)
con_in_pub.publish(drive_in)
if t1 - t0 < 3:
drive_in.drive.steering_angle = 0
drive_in.drive.steering_angle_velocity = 0
drive_in.drive.speed = 0
drive_in.drive.acceleration = 0
drive_in.header.seq = i
else:
if i < 50:
drive_in.drive.speed = 2 * state_sig[i + 1, 3]
drive_in.drive.acceleration = 2 * con_sig[i, 0]
drive_in.drive.steering_angle = state_sig[i + 1, 4]
drive_in.drive.steering_angle_velocity = con_sig[i, 1]
i = i + 1
else:
drive_in.drive.steering_angle = 0
drive_in.drive.steering_angle_velocity = 0
drive_in.drive.speed = 0
drive_in.drive.acceleration = 0
drive_in.header.seq = i
t1 = rospy.Time.now().to_sec()
rate.sleep()
rate.sleep()
def apply_control(self, speed, steering_angle):
self.actual_speed = speed
drive_msg_stamped = AckermannDriveStamped()
drive_msg_stamped.header = utils.make_header("/base_link")
drive_msg = AckermannDrive()
drive_msg.speed = speed
# 正为左转，负为右转
drive_msg.steering_angle = steering_angle
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.control_pub.publish(drive_msg_stamped)
def filter_command(data):
global flag_move
global flag_stop
pub_movement = rospy.Publisher('/vesc/ackermann_cmd_mux/input/navigation', AckermannDriveStamped, queue_size=1)
newData = AckermannDriveStamped()
newData.header = data.header
#print "flag is ", flag_stop
#if len(data.header.frame_id) > 0 or not flag_stop:
newData.drive = data.drive
pub_movement.publish(newData)
def callback(data):
rospy.loginfo(str(data.steer) + " - " + str(data.throttle))
ackermsg = AckermannDriveStamped()
ackermsg.header = Header()
ackermsg.header.stamp = rospy.Time.now()
ackermsg.drive = AckermannDrive()
ackermsg.drive.steering_angle = data.steer
ackermsg.drive.steering_angle_velocity = steering_angle_velocity
ackermsg.drive.speed = data.throttle
ackermsg.drive.acceleration = acceleration
ackermsg.drive.jerk = jerk
publisher.publish(ackermsg)
def __init__(self):
self.pub = rospy.Publisher("ackermann_cmd",\
AckermannDriveStamped, queue_size =1 )
while not rospy.is_shutdown():
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
header_msg = Header()
drive_msg.speed = 1
drive_msg.steering_angle = 0.1
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
header_msg.stamp = rospy.Time.now()
header_msg.frame_id = '1'
drive_msg_stamped.header = header_msg
pub = rospy.Publisher('/ackermann_cmd',
AckermannDriveStamped,
queue_size=1)
pub.publish(drive_msg_stamped)
def controlLoopCB(self, event):
'''Control loop for car MPC'''
if self.goal_received and not self.goal_reached:
control_loop_start_time = time.time()
# Update system states: X=[x, y, psi, v]
px = self.current_pos_x
py = self.current_pos_y
psi = self.current_yaw
v = self.current_vel_odom
# Update system inputs: U=[steering, throttle]
steering = self.steering_angle # radian
dt = self.mpc.dT
L = self.mpc.L
# Waypoints related parameters
self.get_nearby_waypoints()
self.mpc_reference_pub.publish(self.local_path)
NW = len(self.local_path.poses) # Number of waypoints
cospsi = cos(psi)
sinpsi = sin(psi)
print("Control loop time1=:",
time.time() - control_loop_start_time)
# Convert to the vehicle coordinate system
x_veh = np.zeros(NW)
y_veh = np.zeros(NW)
for i in range(NW):
dx = self.local_path.poses[i].pose.position.x - px
dy = self.local_path.poses[i].pose.position.y - py
x_veh[i] = dx * cospsi + dy * sinpsi
y_veh[i] = dy * cospsi - dx * sinpsi
# Fit waypoints
self.mpc.coeffs = np.polyfit(x_veh, y_veh, 3)
if self.DEBUG_MODE:
print("poly coeffs=", self.mpc.coeffs)
self.publish_path_from_coeffs(self.mpc.coeffs)
print("Control loop time2=:",
time.time() - control_loop_start_time)
# cte = self.polyeval(coeffs, 0.0)
# epsi = atan(coeffs[1])
current_state = np.array([0.0, 0.0, 0.0, v])
print("Control loop time_mpc=:",
time.time() - control_loop_start_time)
# Solve MPC Problem
first_control, trajectory = self.mpc.solve(current_state)
print("Control loop time3=:",
time.time() - control_loop_start_time)
# MPC result (all described in car frame)
steering = first_control[1] #radian
speed = v + first_control[0] * dt # speed
if (speed >= self.param['v_max']):
speed = self.param['v_max']
elif (speed <= (-self.param['v_max'] / 2.0)):
speed = -self.param['v_max'] / 2.0
# Display the MPC predicted trajectory
mpc_traj = Path()
mpc_traj.header = self.create_header(self.car_frame)
mpc_traj.poses = []
for i in range(trajectory.shape[0]):
tempPose = PoseStamped()
tempPose.header = mpc_traj.header
tempPose.pose.position.x = trajectory[i, 0]
tempPose.pose.position.y = trajectory[i, 1]
tempPose.pose.orientation.w = 1.0
mpc_traj.poses.append(tempPose)
print("Control loop time4=:",
time.time() - control_loop_start_time)
# publish the mpc trajectory
self.mpc_trajectory_pub.publish(mpc_traj)
if self.DEBUG_MODE:
print("DEBUG")
print("psi: ", psi)
print("accel: ", first_control[0])
print("V: ", v)
print("coeffs: ", self.mpc.coeffs)
print("_steering:", steering)
# print("_throttle: \n",throttle)
print("_speed: ", speed)
# print("mpc_speed: \n",mpc_results[2])
print("Control loop time=:",
time.time() - control_loop_start_time)
else:
steering = 0.0
speed = 0.0
if (self.goal_reached and self.goal_received):
print("Goal Reached !")
# publish cmd
ackermann_cmd = AckermannDriveStamped()
ackermann_cmd.header = self.create_header(self.car_frame)
ackermann_cmd.drive.steering_angle = steering
ackermann_cmd.drive.speed = speed
# ackermann_cmd.drive.acceleration = throttle
self.ackermann_pub.publish(ackermann_cmd)
def controlLoopCB(self, event):
'''Control loop for car MPC'''
if self.goal_received and not self.goal_reached:
control_loop_start_time = time.time()
# Update system states: X=[x, y, psi]
px = self.current_pos_x
py = self.current_pos_y
psi = self.current_yaw
# Update system inputs: U=[speed(v), steering]
v = self.current_vel_odom
steering = self.steering_angle # radian
dt = 1.0/self.CONTROLLER_FREQ
L = self.mpc.L
# Waypoints related parameters
self.get_nearby_waypoints()
self.mpc_reference_pub.publish(self.local_path)
NW = len(self.local_path.poses) # Number of waypoints
cospsi = cos(psi)
sinpsi = sin(psi)
print("Control loop time1=:", time.time() - control_loop_start_time)
# Convert to the vehicle coordinate system
x_veh = np.zeros(NW)
y_veh = np.zeros(NW)
for i in range(NW):
dx = self.local_path.poses[i].pose.position.x - px
dy = self.local_path.poses[i].pose.position.y - py
x_veh[i] = dx * cospsi + dy * sinpsi
y_veh[i] = dy * cospsi - dx * sinpsi
# Fit waypoints to a n order spline
self.mpc.coeffs = np.polyfit(x_veh, y_veh, self.param['spline_poly_order'])
if self.DEBUG_MODE:
print("poly coeffs=",self.mpc.coeffs)
self.publish_path_from_coeffs(self.mpc.coeffs)
print("Control loop time2=:", time.time() - control_loop_start_time)
cte = np.polyval(self.mpc.coeffs, 0.0)
# epsi = atan(self.mpc.coeffs[2])
if self.DELAY_MODE:
# Predicting vehicle state at the actual moment of control (current time + delay dt)
px_act = v * dt
py_act = 0
psi_act = v * steering * dt / L
current_state= np.array([px_act,py_act,psi_act])
else:
current_state= np.array([0.0,0.0,0.0])
# Solve MPC Problem
mpc_time=time.time()
first_control,trajectory = self.mpc.solve(current_state)
print("Control loop time mpc=:", time.time() - mpc_time)
# MPC result (all described in car frame)
steering = float(first_control[1]) #radian
speed = float(first_control[0]) # speed
if (speed >= self.param['v_max']):
speed = self.param['v_max']
elif (speed <= (- self.param['v_max'] / 2.0)):
speed = - self.param['v_max'] / 2.0
# Display the MPC predicted trajectory
mpc_traj = Path()
mpc_traj.header= self.create_header(self.car_frame)
mpc_traj.poses=[]
for i in range(trajectory.shape[0]):
tempPose = PoseStamped()
tempPose.header = mpc_traj.header
tempPose.pose.position.x = trajectory[i,0]
tempPose.pose.position.y = trajectory[i,1]
tempPose.pose.orientation.w = 1.0
mpc_traj.poses.append(tempPose)
print("Control loop time4=:", time.time() - control_loop_start_time)
# publish the mpc trajectory
self.mpc_trajectory_pub.publish(mpc_traj)
if self.DEBUG_MODE:
print("DEBUG")
print("psi: ",psi)
print("V: ",v)
print("coeffs: ",self.mpc.coeffs)
print("_steering:",steering)
print("_speed: ",speed)
print("Control loop time=:",time.time()-control_loop_start_time)
self.current_time += 1.0 / self.CONTROLLER_FREQ
self.cte_plot.append(cte)
self.t_plot.append(self.current_time)
self.v_plot.append(speed)
self.steering_plot.append(np.rad2deg(steering))
else:
steering = 0.0
speed = 0.0
# publish cmd
ackermann_cmd = AckermannDriveStamped()
ackermann_cmd.header = self.create_header(self.car_frame)
ackermann_cmd.drive.steering_angle = steering
ackermann_cmd.drive.speed = speed
# ackermann_cmd.drive.acceleration = throttle
self.ackermann_pub.publish(ackermann_cmd)
def controlLoopCB(self, event):
'''Control loop for car MPC'''
if self.goal_received and not self.goal_reached:
control_loop_start_time = time.time()
# Update system states: X=[x, y, psi]
px = self.current_pos_x
py = self.current_pos_y
car_pos = np.array([self.current_pos_x, self.current_pos_y])
psi = self.current_yaw
# Update system inputs: U=[speed(v), steering]
v = self.current_vel_odom
steering = self.steering_angle # radian
dt = 1.0 / self.CONTROLLER_FREQ
L = self.mpc.L
current_s, near_idx = self.find_current_arc_length(car_pos)
current_state = np.array([px, py, psi, current_s])
centerPose = PoseStamped()
centerPose.header = self.create_header('map')
centerPose.pose.position.x = float(self.center_lane[near_idx, 0])
centerPose.pose.position.y = float(self.center_lane[near_idx, 1])
centerPose.pose.orientation = self.heading(0.0)
self.center_tangent_pub.publish(centerPose)
print(car_pos,v,near_idx,current_s,psi)
# input('str')
# Solve MPC Problem
mpc_time = time.time()
first_control, trajectory = self.mpc.solve(current_state)
mpc_compute_time = time.time() - mpc_time
print("Control loop time mpc=:", mpc_compute_time)
# MPC result (all described in car frame)
steering = float(first_control[1]) # radian
speed = float(first_control[0]) # speed
if (speed >= self.param['v_max']):
speed = self.param['v_max']
elif (speed <= (- self.param['v_max'] / 2.0)):
speed = - self.param['v_max'] / 2.0
# Display the MPC predicted trajectory
mpc_traj = Path()
mpc_traj.header = self.create_header('map')
mpc_traj.poses = []
for i in range(trajectory.shape[0]):
tempPose = PoseStamped()
tempPose.header = mpc_traj.header
tempPose.pose.position.x = trajectory[i, 0]
tempPose.pose.position.y = trajectory[i, 1]
tempPose.pose.orientation = self.heading(trajectory[i, 2])
mpc_traj.poses.append(tempPose)
print("Control loop time4=:", time.time() - control_loop_start_time)
# publish the mpc trajectory
self.mpc_trajectory_pub.publish(mpc_traj)
total_time = time.time() - control_loop_start_time
if self.DEBUG_MODE:
print("DEBUG")
print("psi: ", psi)
print("V: ", v)
print("coeffs: ", self.mpc.coeffs)
print("_steering:", steering)
print("_speed: ", speed)
print("Control loop time=:", total_time)
self.current_time += 1.0 / self.CONTROLLER_FREQ
# self.cte_plot.append(cte)
self.t_plot.append(self.current_time)
self.v_plot.append(speed)
self.steering_plot.append(np.rad2deg(steering))
self.time_plot.append(mpc_compute_time * 1000)
# input('str')
else:
steering = 0.0
speed = 0.0
# publish cmd
ackermann_cmd = AckermannDriveStamped()
ackermann_cmd.header = self.create_header(self.car_frame)
ackermann_cmd.drive.steering_angle = steering
ackermann_cmd.drive.speed = speed
# ackermann_cmd.drive.acceleration = throttle
self.ackermann_pub.publish(ackermann_cmd)
def controlLoopCB(self, event):
'''Control loop for car MPC'''
if self.goal_received and not self.goal_reached:
control_loop_start_time = time.time()
# Update system states: X=[x, y, psi]
px = copy.deepcopy(self.current_pos_x)
py = copy.deepcopy(self.current_pos_y)
car_pos = np.array([px, py])
psi = copy.deepcopy(self.current_yaw)
# Update system inputs: U=[speed(v), steering]
v = copy.deepcopy(self.current_vel_odom)
steering = self.steering_angle # radian
L = self.mpc.L
current_s, near_idx = self.find_current_arc_length(car_pos)
print "pre", current_s, near_idx
if self.DELAY_MODE:
dt_lag = self.LAG_TIME
px = px + v * np.cos(psi) * dt_lag
py = py + v * np.sin(psi) * dt_lag
psi = psi + (v / L) * tan(steering) * dt_lag
current_s = current_s + self.projected_vel * dt_lag
current_state = np.array([px, py, psi, current_s])
centerPose = PoseStamped()
centerPose.header = self.create_header('map')
centerPose.pose.position.x = float(self.center_lane[near_idx, 0])
centerPose.pose.position.y = float(self.center_lane[near_idx, 1])
centerPose.pose.orientation = self.heading(
self.center_point_angles[near_idx])
self.center_tangent_pub.publish(centerPose)
# Solve MPC Problem
mpc_time = time.time()
first_control, trajectory, control_inputs = self.mpc.solve(
current_state)
mpc_compute_time = time.time() - mpc_time
# MPC result (all described in car frame)
speed = float(first_control[0]) # speed
steering = float(first_control[1]) # radian
self.projected_vel = speed
#throttle calculation
throttle = 0.03 * (speed - v) / self.param['dT']
if throttle > 1:
throttle = 1
elif throttle < -1:
throttle = -1
if speed == 0:
throttle = 0
if not self.mpc.WARM_START:
speed, steering, throttle = 0, 0, 0
self.mpc.WARM_START = True
if (speed >= self.param['v_max']):
speed = self.param['v_max']
elif (speed <= (-self.param['v_max'] / 2.0)):
speed = -self.param['v_max'] / 2.0
# Display the MPC predicted trajectory
mpc_traj = Path()
mpc_traj.header = self.create_header('map')
mpc_traj.poses = []
for i in range(trajectory.shape[0]):
tempPose = PoseStamped()
tempPose.header = mpc_traj.header
tempPose.pose.position.x = trajectory[i, 0]
tempPose.pose.position.y = trajectory[i, 1]
tempPose.pose.orientation = self.heading(trajectory[i, 2])
mpc_traj.poses.append(tempPose)
self.mpc_trajectory_pub.publish(mpc_traj)
# publish the mpc related metadata to hylaa node
#metadata message
meta = MPC_metadata()
meta.header = self.create_header('map')
meta.dt = self.param['dT']
meta.horizon = self.param['N'] * self.param['dT']
self.meta_pub.publish(meta)
#publish mpc prediction results message
mpc_prediction_results = []
for i in range(control_inputs.shape[0]):
mpc_prediction_states = [
trajectory[i, 0], trajectory[i, 1], trajectory[i, 2], 0.1
]
mpc_prediction_inputs = [
control_inputs[i, 0], control_inputs[i, 1]
]
mpc_prediction_results.append(
(mpc_prediction_states, mpc_prediction_inputs))
trajectory = MPC_trajectory()
trajectory.header = meta.header
trajectory.trajectory = [
MPC_prediction(pred[0], pred[1])
for pred in mpc_prediction_results
]
self.prediction_pub.publish(trajectory)
total_time = time.time() - control_loop_start_time
if self.DEBUG_MODE:
rospy.loginfo("DEBUG")
rospy.loginfo("psi: %s ", psi)
rospy.loginfo("V: %s", v)
rospy.loginfo("Throttle: %s", throttle)
rospy.loginfo("Control loop time mpc= %s:", mpc_compute_time)
rospy.loginfo("Control loop time=: %s", total_time)
self.current_time += 1.0 / self.CONTROLLER_FREQ
# self.cte_plot.append(cte)
self.t_plot.append(self.current_time)
self.v_plot.append(speed)
self.steering_plot.append(np.rad2deg(steering))
self.time_plot.append(mpc_compute_time * 1000)
else:
steering = 0.0
speed = 0.0
throttle = 0.0
# publish cmd
ackermann_cmd = AckermannDriveStamped()
ackermann_cmd.header = self.create_header(self.car_frame)
ackermann_cmd.drive.steering_angle = steering
self.steering_angle = steering
ackermann_cmd.drive.speed = speed
if self.THROTTLE_MODE:
ackermann_cmd.drive.acceleration = throttle
self.ackermann_pub.publish(ackermann_cmd)
def do_pure_pursuit(self):
# print "pose:=", pose_x, pose_y, pose_yaw
if self.pose_read_flag:
if self.FIND_NEAREST_POINT:
self.last_search_index = self.find_closed_point_index(
self.current_pose)
print "Nearest index=", self.last_search_index
self.FIND_NEAREST_POINT = False
if self.LOOP_ENABLE and self.NEAR_END_OF_WAY_POINTS:
self.last_search_index = 0
self.NEAR_END_OF_WAY_POINTS = False
if self.SPEED_CONTROL:
average_x_from_car = self.get_x_deviation_in_path(
self.current_pose[0], self.current_pose[1],
self.current_pose[2])
if self.INTERPOLATION_MODE == 'quadratic':
self.LOOKAHEAD_DISTANCE = self.do_quadratic_interpolation(
average_x_from_car)
else:
self.LOOKAHEAD_DISTANCE = np.interp(
average_x_from_car, [0.0, self.MAX_X_DEVIATION], [
self.STRAIGHT_LOOKAHEAD_DIST,
self.TURNING_LOOKAHEAD_DIST
])
self.VELOCITY = np.interp(
average_x_from_car, [0.0, self.MAX_X_DEVIATION],
[self.STRAIGHT_VEL, self.TURNING_VEL])
pursuit_param = pure_pursuit_param()
pursuit_param.lookahead_dist = self.LOOKAHEAD_DISTANCE
pursuit_param.velocity = self.VELOCITY
self.parameters_pub.publish(pursuit_param)
# 2. Find the path point closest to the vehicle that is >= 1 lookahead distance from vehicle's current location.
distance = 0.00
last_index = len(self.waypoints) - 1
for index, point in enumerate(self.waypoints):
if index < self.last_search_index:
continue
distance = dist(point, self.current_pose)
if distance >= self.LOOKAHEAD_DISTANCE:
self.last_search_index = index
break
if (last_index - 10) <= self.last_search_index <= last_index:
self.NEAR_END_OF_WAY_POINTS = True
goal_point = self.waypoints[self.last_search_index]
# print distance, self.waypoints[self.last_search_index]
# print "goal-point:=", goal_point
# 3. Transform the goal point to vehicle coordinates.
point_x_wrt_car = self.compute_x_wrt_car(
goal_point[0] - self.current_pose[0],
goal_point[1] - self.current_pose[1], self.current_pose[2],
distance)
# 4. Calculate the curvature = 1/r = 2x/l^2
# The curvature is transformed into steering wheel angle by the vehicle on board controller.
# Flipping to negative because for the VESC a right steering angle has a negative value.
curvature = -2.0 * point_x_wrt_car / distance**2
angle = math.atan(self.L * curvature) #rear wheel reference frame
# angle = math.arcsin(self.L*curvature) #front wheel reference frame
angle = constrain(angle, math.radians(-30), math.radians(30))
# publish cmd
ackermann_cmd = AckermannDriveStamped()
ackermann_cmd.header = self.create_header(self.car_frame)
ackermann_cmd.drive.steering_angle = angle
ackermann_cmd.drive.speed = self.VELOCITY
self.ackermann_pub.publish(ackermann_cmd)
