def drive_straight(self):
while not rospy.is_shutdown():
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 2.0
drive_msg.steering_angle = 0.0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
if (SPEED == 0):
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.0
drive_msg.steering_angle = 0.0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
rospy.sleep(5000)
# don't spin too fast
rospy.sleep(1.0 / PUBLISH_RATE)
def stop_vehicle(self):
msg = AckermannDrive()
msg.speed = 0
msg.acceleration = 0
msg.jerk = 0
msg.steering_angle = 0
msg.steering_angle_velocity = 0
self.pub_ackermann_cmd.publish(msg)
rospy.signal_shutdown('Made it to goal')
def stop(self):
drive_msg_stamped = AckermannDriveStamped()
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.drive_pub.publish(drive_msg_stamped)
def drive(self, speed, angle):
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = speed
drive_msg.steering_angle = angle
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
def apply_control(self, speed, steering_angle):
self.actual_speed = speed
drive_msg_stamped = AckermannDriveStamped()
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 tape_present(self, msg):
frame = self.bridge.imgmsg_to_cv2(msg, "mono8")
white_pixels = cv2.countNonZero(frame)
if white_pixels < MIN_PIXELS_TO_DRIVE:
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.0
drive_msg.steering_angle = 0.0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
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 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 callback(data):
speed = data.linear.x
turn = data.angular.z
msg = AckermannDrive()
msg.speed = speed
msg.acceleration = 1
msg.jerk = 1
msg.steering_angle = turn
msg.steering_angle_velocity = 1
pub.publish(msg)
def drive(self, tags_tan):
turning_factor = self.get_turning_factor(tags_tan)
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = MAX_SPEED - abs(turning_factor / 2)
drive_msg.steering_angle = self.get_steering_angle(turning_factor)
drive_msg.acceleration = 0.0
drive_msg.jerk = 0.0
drive_msg.steering_angle_velocity = 0.1
drive_msg_stamped.drive = drive_msg
self.previous_steering_angle = drive_msg.steering_angle
self.pub.publish(drive_msg_stamped)
def driveTest():
rp.init_node("pleaseWork",anonymous = False)
pub=rp.Publisher("/vesc/ackermann_cmd_mux/input/navigation",AckermannDriveStamped,queue_size=10)
rate=rp.Rate(60)
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.8
drive_msg.steering_angle = 1.0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
while True:
pub.publish(drive_msg_stamped)
rate.sleep()
def path_tracking(self, event):
if self.path and self.state:
if self.path.x_states[0] == -100: # shutdown procedure
self.stop_vehicle()
else:
cx = self.path.x_states
cy = self.path.y_states
cyaw = self.path.yaw_states
# for i in range(len(cyaw)):
# cyaw[i] -= math.radians(270)
#self.target_index = pure_pursuit.calculate_target_index(self.state, cx, cy)
target_index, mind = stanley_controller.calc_target_index(
self.state, cx, cy)
ai = pure_pursuit.pid_control(self.target_speed, self.state.v)
#di, self.target_index = pure_pursuit.pure_pursuit_control(self.state,
# cx,
# cy,
# self.target_index)
di, target_index = stanley_controller.stanley_control(
self.state, cx, cy, cyaw, target_index)
print(self.state.v, di)
self.state.v = self.state.v + ai * self.dt
if di > self.max_steering_angle:
di = self.max_steering_angle
if di < -self.max_steering_angle:
di = -self.max_steering_angle
if self.state.v > self.target_speed:
self.state.v = self.target_speed
rospy.loginfo("path_tracking_node: v: " + str(self.state.v) +
" steering: " + str(di))
msg = AckermannDrive()
msg.speed = 0.50
msg.acceleration = 0.5
msg.jerk = 1.0
msg.steering_angle = di
msg.steering_angle_velocity = 1
self.pub_ackermann_cmd.publish(msg)
def drive(self):
while not rospy.is_shutdown():
if self.received_data is None or self.parsed_data is None:
rospy.sleep(0.5)
continue
if np.any(self.parsed_data['front'][:, 0] < MIN_FRONT_DIST):
rospy.loginfo("stoping!")
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.0
drive_msg.steering_angle = 0.0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
rospy.sleep(.1)
def drive(self):
while not rospy.is_shutdown():
if self.received_data is None or self.parsed_data is None:
rospy.sleep(0.5)
continue
if np.any(self.parsed_data['front'][:, 0] < MIN_FRONT_DIST):
rospy.loginfo("stoping!")
# this is overkill to specify the message, but it doesn't hurt
# to be overly explicit
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.0
drive_msg.steering_angle = 0.0
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
# don't spin too fast
rospy.sleep(.1)
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 apply_control(self):
while not rospy.is_shutdown():
if self.control is None:
print "No control data"
rospy.sleep(0.5)
else:
self.steering_hist.append(self.control[0])
# smoothed_steering = self.steering_hist.mean()
smoothed_steering = self.steering_hist.median()
# print smoothed_steering, self.control[0]
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = self.control[1]
drive_msg.steering_angle = smoothed_steering
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
rospy.sleep(1.0/PUBLISH_RATE)
def talker():
angle_new = 0.5
angle_old = 0.4
acceleration = 0.5
velocity = 0
deltat = 0.1
pub = rospy.Publisher('/carla/ego_vehicle/ackermann_cmd', AckermannDrive, queue_size=10)
rospy.init_node('CAV_talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
angle_vel = 2
speed = 10
jerk = 1
while not rospy.is_shutdown():
# control_cmd = "{steering_angle: %f, steering_angle_velocity: %f, speed: %f, acceleration: %f, jerk: 0.0}" % (angle_new,angle_vel,speed,acceleration)
#control_cmd = ['%f'%(angle_new),'%f'%(angle_vel),'%f'%(speed),'%f'%(acceleration),'%f'%(jerk)]
control_cmd = AckermannDrive()
control_cmd.steering_angle = angle_new
control_cmd.steering_angle_velocity = angle_vel
control_cmd.speed = speed
control_cmd.acceleration = acceleration
control_cmd.jerk = jerk
rospy.loginfo(control_cmd)
pub.publish(control_cmd)
rate.sleep()
def drive(self):
while not rospy.is_shutdown():
if self.received_data is None or self.parsed_data is None:
rospy.sleep(0.5)
continue
if np.any(self.parsed_data['front'][:, 0] < MIN_FRONT_DIST):
rospy.loginfo("stoping!")
msg = String()
msg.data = 'stop_event'
self.failsafe_pub.publish(msg)
# just in case
drive_msg_stamped = AckermannDriveStamped()
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.pub.publish(drive_msg_stamped)
# don't spin too fast
rospy.sleep(.1)
def main_fonk(data, a):
global x, y, paylas
global cur_pos, q_goal, angular_speed, linear_speed, linear_unit, angular_unit
global cur_action, next_action_time
linear_speed = 0.5
angular_speed = 0.5
linear_unit = 0.5
rate = rospy.Rate(20)
angular_unit = math.pi / 6.
# Initialize points
cur_pos.x = x
cur_pos.y = y
start_position = cur_pos
q_goal.z = 0.0
# Bayraklar
cur_action = 'gf' # 'tl': sola dön; 'tr': sağa dön; 'fw': düz ilerle
# Set parameters for actions
next_action_time = rospy.Time.now() # zaman hareketi başlıyor
safe_distance = 2.0 # bir engele yakalaşılacak min nokta
goal_tolarence = 1.5 # hedefe olan tolerans
if a == True:
print("\nAraç hedefe yöneliyor!")
i = 0
for p in data:
q_goal.x = p[0]
q_goal.y = p[1]
if paylas == False:
break
# eğer hedefe uzaksam ve ros kapanmadı ise döngüye başla (kontrol döngüsü)
while get_distance_between(
cur_pos,
q_goal) > goal_tolarence and not rospy.is_shutdown():
global regions_
get_goal_direction() # yön bul
ackerman = AckermannDrive()
ackerman.steering_angle_velocity = 0.0
ackerman.acceleration = 0.5
ackerman.jerk = 0.0
if cur_action == 'tr':
ackerman.speed = linear_speed
ackerman.steering_angle = -angular_speed
elif cur_action == 'tl':
ackerman.speed = linear_speed
ackerman.steering_angle = angular_speed
elif cur_action == 'gf':
ackerman.steering_angle = 0.0
ackerman.speed = linear_speed
elif cur_action == 'lgf':
ackerman.steering_angle = 0.0
ackerman.steering_angle = angular_speed + 0.25 #geriye dönerken yönünü daha fazla çevirir
ackerman.speed = -linear_speed
elif cur_action == 'rgf':
ackerman.steering_angle = 0.0
ackerman.steering_angle = -angular_speed - 0.25 #geriye dönerken yönünü daha fazla çevirir
ackerman.speed = -linear_speed
else:
print("[Hata Durumu]")
break
cmd_vel_pub.publish(ackerman)
rate.sleep()
# pozisyon güncellendi
cur_pos.x = x
cur_pos.y = y
# hedefe ulaşmak kontrol ediliyor
if get_distance_between(cur_pos, q_goal) <= goal_tolarence:
i = i + 1
print i, ". hedefe ulaşıldı!!!"
break
elif paylas == False:
a = False
print("Araç yeni hedef bekliyor!")
cur_action = 'stop'
ackerman = AckermannDrive()
ackerman.steering_angle_velocity = 0.0
ackerman.acceleration = 0.0
ackerman.jerk = 0.0
if cur_action == 'stop':
ackerman.speed = 0.0
ackerman.steering_angle = 0.0
cmd_vel_pub.publish(ackerman)
def timer_cb(self, event):
cmd_msg = AckermannDrive()
current_time = rospy.get_time()
# delta_t = current_time - self.time
delta_t = self.time_step
# delta_t = 0.05
self.time = current_time
jerk = 2.0
# cmd_msg.header.stamp = rospy.Time.now()
cmd_msg.steering_angle = 0.0
cmd_msg.steering_angle_velocity = 0.0
cmd_msg.speed = 0.0
cmd_msg.acceleration = 0.0
cmd_msg.jerk = jerk
if self.pathReady and self.stateReady:
pos_x, pos_y = self.state.get_position()
# pick target w/o collision avoidance, closest point on the traj and one point ahead
_, idx = self.path_tree.query([pos_x, pos_y])
# Target point for steering
if idx < self.traj_steps - 3:
target_pt = self.path_tree.data[idx + 3, :]
# str_idx = idx + 2
else:
target_pt = self.path_tree.data[-1, :]
# str_idx = self.vel_path.shape[0] - 1
print("CONTROLLER: at the end of the desired waypoits!!!")
# Target point for velocity
if idx < self.traj_steps:
target_vel = self.vel_path[idx, :]
# str_idx = idx + 2
else:
target_vel = self.vel_path[-1, :]
# str_idx = self.vel_path.shape[0] - 1
target_speed = np.linalg.norm(target_vel)
speed_diff = target_speed - self.state.get_speed()
# pos_diff = target_pt - self.state.get_position()
# acceleration = cmd_msg.acceleration + 2.0*(
# pos_diff/self.time_step**2.0 - speed_diff/self.time_step
# )
acceleration = abs(speed_diff) / (2.0 * delta_t)
cmd_msg.acceleration = np.min([1.5, acceleration])
steer = self.compute_ackermann_steer(target_pt)
steer_diff = abs(steer - self.steer_prev)
if steer_diff >= 0.3:
print(" 0.3")
steer = self.steer_prev
elif steer_diff >= 0.05:
print(" 0.05")
acceleration = 0.0
target_speed = target_speed / 5.0
self.steer_prev = steer
if self.state.get_speed() - target_speed > 0.0:
# cmd_msg.acceleration = acceleration - 5
acceleration = -100.0
cmd_msg.speed = target_speed / 5.0
jerk = 1000.0
elif target_speed - self.state.get_speed() > 0.2:
acceleration = np.min([3.0, acceleration])
else:
acceleration = 0.0
cmd_msg.steering_angle = steer
cmd_msg.speed = target_speed
cmd_msg.acceleration
cmd_msg.jerk = jerk
# Print out some debugging information
if abs(target_speed - self.max_speed) > 2.0:
# print("Posit diff:", pos_diff)
print("Speed diff:", speed_diff)
print("Current speed:", self.state.get_speed())
print("Desired speed:", target_speed)
print("Desired accel:", acceleration)
print("Desired jerk:", jerk)
print("Speed (sent):", cmd_msg.speed)
print("Accel (sent):", cmd_msg.acceleration)
print("Jerk (sent):", cmd_msg.jerk)
print("delta t:", delta_t)
# for visualization purposes and debuging control node
mk_msg = Marker()
mk_msg.header.stamp = rospy.Time.now()
mk_msg.header.frame_id = 'map'
mk_msg.pose.position.x = target_pt[0]
mk_msg.pose.position.y = target_pt[1]
mk_msg.type = Marker.CUBE
mk_msg.scale.x = 3
mk_msg.scale.y = 3
mk_msg.scale.z = 3
mk_msg.color.a = 1.0
mk_msg.color.r = 1
mk_msg.color.b = 1
self.tracking_pt_viz_pub.publish(mk_msg)
# self.vehicle_cmd_pub.publish(vehicle_cmd_msg)
self.command_pub.publish(cmd_msg)
def convert_to_ackermann(self, joy_msg):
axes = joy_msg.axes
buttons = joy_msg.buttons
if buttons[7] == 1: # button a clicked
self.start_autonomous = not self.start_autonomous
bool_msg = Bool()
bool_msg = self.start_autonomous
self.pub_start_autonomous.publish(bool_msg)
if self.start_autonomous:
rospy.loginfo("joy_to_ackermann: start autonomous mode")
self.timer.shutdown()
else:
rospy.loginfo("joy_to_ackermann: start manual mode")
self.timer = rospy.Timer(rospy.Duration(0.1), self.publish_cmd)
if buttons[3] == 1: # button y clicked
self.front_camera = not self.front_camera
bool_msg = Bool()
bool_msg = self.front_camera
self.pub_switch_cameras.publish(bool_msg)
if axes[-1] == 1: # up on the D pad
self.max_motor_vel += 0.05
rospy.loginfo(
"joy_to_ackermann: max motor velocity increased to: " +
str(self.max_motor_vel))
msg = Float32()
msg.data = self.max_motor_vel
self.pub_max_vel.publish(msg)
elif axes[-1] == -1: # down on the D pad
self.max_motor_vel -= 0.05
rospy.loginfo(
"joy_to_ackermann: max motor velocity increased to: " +
str(self.max_motor_vel))
msg = Float32()
msg.data = self.max_motor_vel
self.pub_max_vel.publish(msg)
if buttons[2] == 1: # button x clicked
motor_velocity = 0
steering_angle = 0
else:
steering_axis = axes[0]
reverse_trigger = axes[2]
gas_trigger = axes[5]
# convert to speed
if gas_trigger == 1 and reverse_trigger == 1:
motor_velocity = 0
elif reverse_trigger != 1:
motor_velocity = -1 * abs(reverse_trigger * self.max_motor_vel)
else:
motor_velocity = abs(gas_trigger * self.max_motor_vel)
# convert to steering angle and reverse it since
# the xbox controller returns right in the negatives
steering_angle = steering_axis * self.max_steering_angle
steering_angle *= -1
msg = AckermannDrive()
msg.speed = motor_velocity
msg.acceleration = 1
msg.jerk = 1
msg.steering_angle = -1 * steering_angle
msg.steering_angle_velocity = 1
self.current_cmd_msg = msg
print(msg)
from ackermann_msgs.msg import AckermannDrive
car_ctrl = e2o_ctrl()
car_ctrl.Accel = 0
car_ctrl.Brake = 0
car_ctrl.Steer = 0
car_ctrl.RNDB = 'N'
ackermann_ctrl = AckermannDrive()
ackermann_ctrl.steering_angle_velocity = 1
ackermann_ctrl.speed = 0
ackermann_ctrl.steering_angle = 0
ackermann_ctrl.acceleration = 0
ackermann_ctrl.jerk = 0
last_speed = 0
a1 = 0.2 # if input acceleration is "'a' then top speed that can be reached is a*a1
a2 = 0.02/50 # increment speed by a2*a
a3 = 0.005/50 # decrement speed by a3*a
b1 = 1.0/1500 # decrement speed by b1*brake %
c1 = 1.0/1000 # when gear is neutral, decrement speed by c1
print("a1 ", a1)
print("a2 ", a2)
print("a3 ", a3)
print("b1 ", b1)
print("c1 ", c1)
#INITIALISE VARIABLES
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
from ackermann_msgs.msg import AckermannDrive
import numpy as np
t = AckermannDrive()
t.steering_angle_velocity = 1
t.speed = 0
t.steering_angle = 0
t.acceleration = 0
t.jerk = 0
key_mapping = {
'w': [0, 1],
'x': [0, -1],
'a': [-1, 0],
'd': [1, 0],
's': [0, 0]
}
def keys_cb(msg, twist_pub):
if len(msg.data) == 0 or not key_mapping.has_key(msg.data[0]):
return # unknown key
vels = key_mapping[msg.data[0]]
if msg.data[0] == 'w':
t.speed += 0.05
def drive(self):
while not rospy.is_shutdown():
if self.received_data is None or self.parsed_data is None:
rospy.sleep(0.5)
continue
print "front", self.range_hist_front.mean()#, self.parsed_data["front"][:,0]
print "left", self.range_hist_left.mean()#, self.parsed_data["left"][:,0]
print "right", self.range_hist_right.mean()#, self.parsed_data["right"][:,0]
if (np.any(self.parsed_data['front'][:,0] < MIN_FRONT_DIST) or (not self.status)):
#print "stop!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
rospy.loginfo("stoping!")
# this is overkill to specify the message, but it doesn't hurt
# to be overly explicit
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
self.steer = 0.0
if(self.right_is_long):
self.steer = LEFT_STEER
else:
self.steer = RIGHT_STEER
drive_msg.steering_angle = self.steer
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
#if GEAR:
# print "GEAR>>>>>"
# for i in range(390, 500):
# print "BACK>>>>>>>>>"
# drive_msg.speed = 403
# self.pub.publish(drive_msg_stamped)
GEAR = 0
drive_msg.speed = BACKWARD_SPEED
self.pub.publish(drive_msg_stamped)
rospy.sleep(.4)
elif np.any(self.parsed_data['front'][:,0] < CHANGE_FRON_DIST):
rospy.loginfo("change!")
GEAR = 1
if np.sum(self.range_hist_left.mean() > self.range_hist_right.mean()):
self.right_is_long = False
self.left_is_long = True
#print "turn left"
#print np.sum(self.parsed_data['left'][:,0]), np.sum(self.parsed_data['right'][:,0])
#print self.parsed_data['right'][:,0]
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = DEFAULT_SPEED
#PID CONTROL
ERROR_CURRENT = LEFT_STEER - self.steer # 0 - 260
E_DIFF = ERROR_CURRENT - self.error_last
self.error_last = ERROR_CURRENT
result = KP*ERROR_CURRENT - KD*E_DIFF
self.steer = result
#print "steer : ", self.steer
#print "error_last : ", self.error_last
#print "error_current : ", ERROR_CURRENT
#print "e_diff : ", E_DIFF
#print "error_last : ", self.error_last
print "turn left PID result>>>> ", result
#self.steering_hist.append(result)
self.steer = result
drive_msg.steering_angle = self.steer
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
else:
self.right_is_long = True
self.left_is_long = False
#print "turn right"
#print np.sum(self.parsed_data['left'][:,0]), np.sum(self.parsed_data['right'][:,0])
#print self.parsed_data['right'][:,0]
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = DEFAULT_SPEED
#PID CONTROL
ERROR_CURRENT = RIGHT_STEER - self.steer
E_DIFF = ERROR_CURRENT - self.error_last
self.error_last = ERROR_CURRENT
result = KP*ERROR_CURRENT - KD*E_DIFF
#print "steer : ", self.steer
#print "error_current : ", ERROR_CURRENT
#print "e_diff : ", E_DIFF
#print "error_last : ", self.error_last
print "turn right PID result>>>> ", result
self.steer = result
drive_msg.steering_angle = self.steer
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
else:
#print "drive~!!>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"
self.right_is_long = False
self.left_is_long = False
GEAR = 1
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = DEFAULT_SPEED
#PID CONTROL
ERROR_CURRENT = FRONT_STEER - self.steer
E_DIFF = ERROR_CURRENT - self.error_last
self.error_last = ERROR_CURRENT
result = KP*ERROR_CURRENT - KD*E_DIFF
#print "steer : ", self.steer
#print "error_current : ", ERROR_CURRENT
#print "e_diff : ", E_DIFF
#print "error_last : ", self.error_last
print "go straight PID result>>>> ", result
self.steer = result
drive_msg.steering_angle = self.steer
drive_msg.acceleration = 0
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
self.pub.publish(drive_msg_stamped)
# don't spin too fast
rospy.sleep(.1)
key = getKey()
if key in keyBindings.keys():
x = keyBindings[key][0]
th = keyBindings[key][1]
else:
x = 0
th = 0
if (key == '\x03'):
break
msg = AckermannDrive()
# msg.header.stamp = rospy.Time.now();
# msg.header.frame_id = "base_link";
msg.speed = x * speed
msg.acceleration = 1
msg.jerk = 1
msg.steering_angle = th * turn
msg.steering_angle_velocity = 1
pub.publish(msg)
except:
print 'error'
finally:
msg = AckermannDrive()
# msg.header.stamp = rospy.Time.now();
# msg.header.frame_id = "base_link";
msg.speed = 0
msg.acceleration = 1
import rospy as rp
import numpy as np
import cv2
import random
from sensor_msgs.msg import LaserScan, Image
from cv_bridge import CvBridge, CvBridgeError
from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive
bridge = CvBridge()
drive_msg_stamped = AckermannDriveStamped()
drive_msg = AckermannDrive()
drive_msg.speed = 0.4
drive_msg.steering_angle = -1.0
drive_msg.acceleration = 1
drive_msg.jerk = 0
drive_msg.steering_angle_velocity = 0
drive_msg_stamped.drive = drive_msg
global proportional_error
global integral_error
global derivative_error
global active_lidar_in
global inactive_lidar_in
global left_lidar_pts
global right_lidar_pts
global previous_error
def callback2(data):
global prepSwitch
