def control(data):
msg = drive_param()
global prev_error
global velocity
global kp
global kd
global flag
global length
global path_y
global path_x
myrobot = robot()
myrobot.set(data.pose.position.x,
data.pose.position.y) #update current pose
''' flag is set when path arrives from planner node'''
if flag == 1:
'''find distance from robot and the desired track (cross track error)'''
for index in range(0, ((length / 2) - 1)):
myrobot.set(data.pose.position.x,
data.pose.position.y) #update current pose
print("curr_y = ", myrobot.y, "curr_x = ", myrobot.x)
print("path_y = ", path_y[index], "path_x = ", path_x[index])
p1 = Point(path_y[index],
path_x[index]) # points from desired path
p2 = Point(path_y[index + 1], path_x[index + 1])
s = Segment(p1, p2)
error = -float(s.distance(
myrobot.curr_pose)) # x in the equation --> -x-Lsin(theta)
if (myrobot.y < path_y[index]):
error = -error #-error = left turn, +ve error = right turn
print("error = ", error)
'''calculate steering angle using pid controller'''
#int_error += error # integral not used
diff_error = error - prev_error # differential
angle = -(kp * error) - (kd * diff_error) # - (ki * int_CTE)
if angle < -0.78:
angle = -0.78
elif angle > 0.78:
angle = 0.78
prev_error = error
msg.angle = angle
velocity = 4.0
msg.velocity = velocity
#print("flag = 1, vel = " , msg.velocity, ", angle = " , msg.angle)
pub.publish(msg)
flag = 0
''' keep vel and steering to 0 if no plan arrives and flag is not set'''
angle = 0
velocity = 0.0
msg.angle = angle
msg.velocity = velocity # constant velocity
#print("flag = 0, vel = ", msg.velocity, ", angle = " , msg.angle)
pub.publish(msg)
def update(u):
if self.listener.canTransform('/map', '/base_link', self.t):
position, quaternion = self.listener.lookupTransform(
'/map', '/base_link', t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
yaw = rpy[2]
msg = drive_param()
msg.velocity = 10
msg.angle = u[0]
rospy.loginfo("velocity:%lf,angle:%lf", msg.velocity, msg.angle)
self.pub.publish(msg)
a = np.array([position[0], position[1], yaw]).astype(float)
a = a.tolist()
return a
def closed_loop_prediction(waypoints):
rospy.init_node("path_tracking")
listener = tf.TransformListener()
listener.waitForTransform('/map', '/base_link', rospy.Time(),
rospy.Duration(20.0))
pub = rospy.Publisher('drive_parameters', drive_param, queue_size=10)
index = 0
pe = 0
pth_e = 0
gazebo_pose = Pose_listener()
while not rospy.is_shutdown():
t = listener.getLatestCommonTime('/map', '/base_link')
if listener.canTransform('/map', '/base_link', t):
position, quaternion = listener.lookupTransform(
'/map', '/base_link', t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
#x = position[0]
#y = position[1]
#yaw = rpy[2]
x = gazebo_pose.pose.x
y = gazebo_pose.pose.y
yaw = gazebo_pose.pose.theta
delta, e, th_e, index = lqr_steering_control(
x, y, yaw, waypoints, index, pe, pth_e)
if delta > max_steer:
delta = max_steer
elif delta < -max_steer:
delta = -max_steer
pe = e
pth_e = th_e
msg = drive_param()
if np.linalg.norm([x - waypoints[-1][0], y - waypoints[-1][1]
]) < 0.3:
msg.velocity = 0
rospy.signal_shutdown("the car has reach the goal")
else:
msg.velocity = velocity
msg.angle = -delta
pub.publish(msg)
stdscr.addstr(2, 20, "Down")
stdscr.addstr(2, 25, '%.2f' % forward)
stdscr.addstr(5, 20, " ")
if key == curses.KEY_LEFT:
left = left + 0.1
if left >= 0.78:
left = 0.78
elif left < -0.78:
left = -0.78
stdscr.addstr(3, 20, "left")
stdscr.addstr(3, 25, '%.2f' % left)
stdscr.addstr(5, 20, " ")
elif key == curses.KEY_RIGHT:
left = left - 0.1
if left >= 0.78:
left = 0.78
elif left < -0.78:
left = -0.78
stdscr.addstr(3, 20, "rgt ")
stdscr.addstr(3, 25, '%.2f' % left)
stdscr.addstr(5, 20, " ")
if key == curses.KEY_DC:
left = 0
forward = 0
stdscr.addstr(5, 20, "Stop")
msg = drive_param()
msg.velocity = forward
msg.angle = left
pub.publish(msg)
curses.endwin()