def cmd_callback(self, data):
rospy.logdebug("convert...")
steering = self.convert_trans_rot_vel_to_steering_angle(data.linear.x,
data.angular.z)
if TYPE_ACKERMANN == self.message_type:
msg = AckermannDrive()
msg.steering_angle = steering # in rad
msg.speed = data.linear.x # in m/s
msg.acceleration = data.linear.x # in m/s^2
else:
msg = AckermannDriveStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame_id
msg.drive.steering_angle = steering # in rad
msg.drive.steering_angle_velocity = steering # in rad/s
msg.drive.speed = data.linear.x # in m/s
msg.drive.acceleration = data.linear.x # in m/s^2
self.pub.publish(msg)
def __init__(self):
self.driving = AckermannDriveStamped()
self.driving.header.stamp = rospy.Time.now()
self.driving.drive.speed = 3
self.ddes = 1 #1.0 #1.55
self.prev_times = collections.deque([time.clock() for _ in range(10)])
self.prev_errors = collections.deque([0 for _ in range(4)])
self.kp = .6 #.75--right #.5--OG
self.ki = 0 #.1--right # .05--OG
self.kd = .02 #.05 # .025--right #.1--OG
self.mult = 1 # right
self.start_ind = 80 # right
self.end_ind = 280 #500 # right
#self.side_sub = rospy.Subscriber("/racecar/JoyLRSelec", Bool, self.side_callback)
self.pid_pub = rospy.Publisher(
"vesc/ackermann_cmd_mux/input/navigation",
AckermannDriveStamped,
queue_size=1)
self.scan_sub = rospy.Subscriber("scan", LaserScan, self.pid_callback)
#rospy.Subscriber("blob_color", String, self.color_callback)
rospy.Subscriber("/wall", Bool, self.side_callback)
def auto_drive(pid):
global car_run_speed
w = 0
if -0.065 < pid and pid < 0.065:
w = 1
else:
w = 0.3
if car_run_speed < 1.0 * w:
car_run_speed += 0.002 * 10
else:
car_run_speed -= 0.003 * 10
ack_msg = AckermannDriveStamped()
ack_msg.header.stamp = rospy.Time.now()
ack_msg.header.frame_id = ''
ack_msg.drive.steering_angle = pid
ack_msg.drive.speed = car_run_speed
ack_publisher.publish(ack_msg)
print 'speed: '
print car_run_speed
def safe(self, data):
pushback = self.pathnav_pushback + self.wallnav_pushback
turn = self.pathnav_turn + self.wallnav_turn
self.send = AckermannDriveStamped()
self.send.drive.speed = self.drive_constant * pushback #sets net force equal to backforce - pushback
if self.send.drive.speed > self.max_speed:
self.send.drive.speed = self.max_speed
elif self.send.drive.speed < -self.max_speed:
self.send.drive.speed = -self.max_speed
self.send.drive.steering_angle = self.turn_constant * turn #sets net force = turn* a constant that scales $
if self.send.drive.speed < 0: #Checks if the speed is negative
self.send.drive.steering_angle = -self.send.drive.steering_angle #reverses steering angle to turn around while i$
#positive because left is positive but cos(angl$
#safety controller
if data.data == False:
self.send.drive.speed = -.2
self.pub.publish(self.send)
def __init__(self):
#"""initalize the node"""
rospy.init_node("driveStop")
self.pub = rospy.Publisher(DRIVE_TOPIC,
AckermannDriveStamped,
queue_size=1)
self.sub_scan = rospy.Subscriber(self.SCAN_TOPIC,
LaserScan,
callback=self.scan_callback)
rospy.Subscriber("scan", LaserScan, self.driveStop_car_callback)
#""" initialize the box dimensions"""
self.flag_box = ((0, 0), (0, 0))
#"""driving code"""
self.size = 0
self.cmd = AckermannDriveStamped()
self.cmd.drive.speed = 0
self.cmd.drive.steering_angle = 0
#"""get the camera data from the class Zed_converter in Zed"""
self.camera_data = Zed_converter(False, save_image=False)
self.min_value = 0
def publishAckermann(self, speed, angle):
angleScaled = self._map_range(angle, -1.0, 1.0, -1.0, 1.0)
speedScaled = self._map_range(speed, -1.0, 1.0, -MAX_SPEED, MAX_SPEED)
self.current_angle = angleScaled
self.current_speed = speedScaled
self.current_ackermann_timestamp = rospy.Time.now().to_nsec()
if self.noise_enabled:
if self.current_noise_peak == 0:
rand = (random.random() * 2 - 1)
pos = 1 if rand > 0 else -1
self.current_noise_peak = (MAX_NOISE * rand) + MIN_NOISE * pos
self.noise_add = self.current_noise_peak / 10
self.current_noise = 0
# print("Current noise peak: " + str(self.current_noise_peak))
self.current_noise += self.noise_add
if abs(self.current_noise - self.current_noise_peak) < abs(
self.current_noise_peak) * 0.01:
self.noise_add *= -1
if 0.005 > self.current_noise > -0.005:
self.current_noise_peak = 0
self.nosie_add = 0
# print("Current noise: " + str(self.current_noise))
self.current_angle_with_noise = angle + self.current_noise
msg = AckermannDriveStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.drive.steering_angle = self.current_angle_with_noise
msg.drive.speed = speedScaled
msg.drive.acceleration = max_accel_x
msg.drive.jerk = max_jerk_x
self.ackermannPub.publish(msg)
def callback(self, msg):
##################################################
# TODO >>>
# Make the robot move, but don't let it get too
# close to obstacles!
##################################################
filtered_ranges = [360 for i in range(180)] + list(msg.ranges[180:900]) + [360 for i in range(900, len(msg.ranges))]
right_dist = sum(filtered_ranges[180:260]) / 80
right_dist = min(right_dist, 1)
left_dist = sum(filtered_ranges[820:900]) / 80
error = right_dist - left_dist
min_dist = min(filtered_ranges[480:600])
print('Min dist:', min_dist, '| Right dist:', right_dist, '| Left dist:', left_dist)
max_angle = 0.34
max_angle_dist = 0.6
k_p = 1 / max_angle_dist
k_d = 5
drive = AckermannDriveStamped()
if min_dist < 0.4:
drive.drive.speed = -1
drive.drive.steering_angle = 0
else:
drive.drive.speed = 1
if left_dist > 2:
drive.drive.steering_angle = max_angle
elif error >= -max_angle_dist and error <= max_angle_dist:
drive.drive.steering_angle = -max_angle*k_p*error-max_angle*k_d*(error-self.prev_error)
self.prev_error = error
elif error > max_angle_dist:
drive.drive.steering_angle = -max_angle-max_angle*k_d*(error-self.prev_error)
elif error < -max_angle_dist:
drive.drive.steering_angle = max_angle-max_angle*k_d*(error-self.prev_error)
'''
#drive.drive.steering_angle = 0.5*math.sin(time.time())
#print(drive.drive.steering_angle)
self.drive_pub.publish(drive)
def ackermann_cmd_input_callback(self, msg):
self.currentSpeed = msg.drive.speed
if self.SAFE:
self.FRONT_DISTANCE = 0.5 * (1.794144**self.currentOdom.twist.twist.linear.x) + 0.1
elif self.currentOdom.twist.twist.linear.x < 0.01:
self.FRONT_DISTANCE = 0.5 * (1.794144**self.currentOdom.twist.twist.linear.x) + 0.1
if self.currentOdom.twist.twist.linear.x < 0 :
self.FRONT_DISTANCE = .5
#rospy.loginfo(self.currentSpeed)
#rospy.loginfo(self.currentOdom.twist.twist.linear.x)
#self.currentAngleOffset = msg.drive.steering_angle
#rospy.loginfo(self.FRONT_DISTANCE)
#print("ackermann_callback")
#print(self.SAFE)
if self.SAFE :
self.cmd_pub.publish(msg)
else:
back_up_msg = AckermannDriveStamped()
back_up_msg.drive.speed = 0
if msg.drive.speed != 0 and self.currentOdom.twist.twist.linear.x < 0.01 :
back_up_msg.drive.speed = -0.5
if self.currentStep > 540 :
back_up_msg.drive.steering_angle = 3
else :
back_up_msg.drive.steering_angle = -3
if self.currentOdom.twist.twist.linear.x > 0:
if self.currentStep > 540 :
back_up_msg.drive.steering_angle = -3
else :
back_up_msg.drive.steering_angle = 3
#back_up_msg.drive.steering_angle = 0
back_up_msg.header.stamp = rospy.Time.now()
self.cmd_pub.publish(back_up_msg)
def disparity_extender_callback(data):
dis = []
# 获取前向180°的测量距离
for angle in range(-90, 91):
dis.append(get_range(data, angle))
disparities = []
for i in range(len(dis)):
if i == len(dis) - 1:
continue
# 超过阈值则认为可能发生碰撞
if abs(dis[i] - dis[i + 1]) > DISPARITY_DIF:
min_dis = min(dis[i], dis[i + 1]) # 计算可能碰撞最小距离
angle_range = math.ceil(
math.degrees(math.atan(CAR_WIDTH / 2 / min_dis))) # 根据小车宽度计算不会碰撞的最小角度
angle_range += 15 # 添加容差
# 需要裁剪的激光数据范围
side_range = range(int(i - angle_range + 1), i + 1) if dis[i + 1] == min_dis else range(i + 1, int(i + 1 + angle_range))
disparities.append((min_dis, side_range))
# 裁剪激光数据
for min_dis, side_range in disparities:
for i in side_range:
if i >= 0 and i < len(dis):
dis[i] = min(dis[i], min_dis)
max_index = np.argmax(dis) # 得到最可行距离的角度
max_dis = dis[max_index]
# 对角度进行限制，主要是避免激光数据抖动产生的影响
angle = max_index - 90 if abs(max_index - 90) > 15 else 0
angle = angle * np.pi / 180
# speed = 3.5
speed = speed_param - P_param * abs(angle)
drive_msg = AckermannDriveStamped()
drive_msg.drive.steering_angle=angle
drive_msg.drive.speed=speed
drive_pub.publish(drive_msg)
def __init__(self):
"""initalize the node"""
rospy.init_node("driveStop")
self.pub = rospy.Publisher(DRIVE_TOPIC,
AckermannDriveStamped,
queue_size=1)
rospy.Subscriber("scan", LaserScan, self.driveStop_car_callback)
self.joy_sub = rospy.Subscriber("vesc/joy", Joy, self.joy_callback)
self.img_pub = rospy.Subscriber
""" initialize the box dimensions"""
"""driving code"""
self.cmd = AckermannDriveStamped()
self.cmd.drive.speed = 0
self.cmd.drive.steering_angle = 0
"""get the camera data from the class Zed_converter in Zed"""
self.bridge = CvBridge()
self.camera_data = Zed_converter(False, save_image=False)
self.min_value = 0
self.img_pub = rospy.Publisher('/zed/zed_node/boxes', Image)
self.msg = Image
self.factor = 0
self.cone_width = 0
self.input_data = []
self.label_data = []
self.map = None
self.l_data = None
self.lower_ind = 180
self.upper_ind = 900
self.joy_data = None
def pursuitToWaypoint(waypoint):
print waypoint
global rear_axle_center, rear_axle_theta, rear_axle_velocity, cmd_pub
rospy.wait_for_message("/ackermann_vehicle/ground_truth/state", Odometry,
5)
dx = waypoint[0] - rear_axle_center.position.x
dy = waypoint[1] - rear_axle_center.position.y
target_distance = math.sqrt(dx * dx + dy * dy)
cmd = AckermannDriveStamped()
cmd.header.stamp = rospy.Time.now()
cmd.header.frame_id = "base_link"
cmd.drive.speed = rear_axle_velocity.linear.x
cmd.drive.acceleration = max_acc
while target_distance > waypoint_tol:
dx = waypoint[0] - rear_axle_center.position.x
dy = waypoint[1] - rear_axle_center.position.y
lookahead_dist = np.sqrt(dx * dx + dy * dy)
lookahead_theta = math.atan2(dy, dx)
alpha = shortest_angular_distance(rear_axle_theta, lookahead_theta)
cmd.header.stamp = rospy.Time.now()
cmd.header.frame_id = "base_link"
# Publishing constant speed of 1m/s
cmd.drive.speed = 1
# Reactive steering
if alpha < 0:
st_ang = max(-max_steering_angle, alpha)
else:
st_ang = min(max_steering_angle, alpha)
cmd.drive.steering_angle = st_ang
target_distance = math.sqrt(dx * dx + dy * dy)
cmd_pub.publish(cmd)
rospy.wait_for_message("/ackermann_vehicle/ground_truth/state",
Odometry, 5)
def PID(self, maxSpeed, kp, ki, kd, mode):
dir = 1
if mode == 'Right':
error = self.averageR - self.idealDis
dir = -1
elif mode == 'Left':
error = self.averageL - self.idealDis
print(error)
elif mode == 'LR':
error = (((self.averageL + self.averageR) / 2) - self.idealDis)
self.maxSpeed = maxSpeed
# Improve and future
self.current_time = time.time()
#P
prop = kp * error
#I
integ = ki * ((error + self.prev_error) * (self.current_time - self.prev_time))
#D
deriv = kd * ((error - self.prev_error) / (self.current_time - self.prev_time))
self.prev_error = error
self.prev_time = self.current_time
self.output = (prop + integ + deriv ) * dir
if abs(self.output) >= 0.34:
self.velCoeff = 0.8
elif abs(self.output) >= 0.25:
self.velCoeff = 0.9
else:
self.velCoeff = 1
self.ackermann_cmd_input_callback(AckermannDriveStamped())
def callback(stuff):
global count
if count == 1:
global x
global y
input1 = int(
input(
"Choose the starting angle to take into account in degrees "))
input2 = int(
input("Choose the ending angle to take into account in degrees "))
x = int(((input1 * np.pi) / 180) / stuff.angle_increment)
y = int(((input2 * np.pi) / 180) / stuff.angle_increment)
##choose 80
global z
msg = AckermannDriveStamped()
msg.drive.speed = z
pub.publish(msg)
count = 0
distance = .8
space = sum(stuff.ranges[x:y]) / (y - x)
error = space - distance
integral.append(error)
u = -error
a = stuff.ranges[135]
b = 10
c = sqrt(a**2 + b**2)
theta = np.arccos((a**2 + b**2 - c**2) / (2 * a * b))
if sum(integral) < 1800:
nice = sum(integral)
print nice
if error < 0:
msg.drive.steering_angle = msg.drive.steering_angle - (
1 / 10) * theta + ((u / z) * np.absolute(error)) - (1 / 50) * nice
pub.publish(msg)
if error > 0:
msg.drive.steering_angle = msg.drive.steering_angle - (
(1 / 10) * theta * np.absolute(error)) + (
(u / z) * np.absolute(error)) - (
(1 / 50) * nice * np.absolute(error))
pub.publish(msg)
def callback(stuff):
global count
if count == 1:
global x
global y
input1 = int(
input(
"Choose the starting angle to take into account in degrees "))
input2 = int(
input("Choose the ending angle to take into account in degrees "))
x = int(((input1 * np.pi) / 180) / stuff.angle_increment)
y = int(((input2 * np.pi) / 180) / stuff.angle_increment)
count = 0
safe = 0.5
safespace = sum(stuff.ranges[x:y]) / (y - x)
print safespace
print stuff.ranges[x:y]
if safespace < safe:
msg = AckermannDriveStamped()
msg.drive.speed = 0
msg.drive.acceleration = 0
pub.publish(msg)
def pid_control(self, error, velocity):
# update PID parameters
now = rospy.Time.now()
if self.prev_time is not None:
# Differential update
self.diff = error - self.prev_error
# Integral update
interval_err = error * (now - self.prev_time).to_sec()
self.integral = self.integral + interval_err
self.cumulative_squared_error += interval_err * interval_err
# calculate the angle to respond to the measured error -- simple first implementation
# just multiply the error by the combination of P + I + D
angle = clamp(
self.p[kp] * error + self.p[ki] * self.integral +
self.p[kd] * self.diff, min_steering_angle, max_steering_angle)
if abs(angle) <= high_speed_limit:
velocity = high_speed
elif abs(angle) <= med_speed_limit:
velocity = med_speed
else:
velocity = low_speed
# if self.count % 5 == 0:
# rospy.loginfo( "Error: %f, Velocity: %f, Angle: %f", error, velocity, angle )
drive_msg = AckermannDriveStamped()
drive_msg.header.stamp = rospy.Time.now()
drive_msg.header.frame_id = "laser"
drive_msg.drive.steering_angle = angle * self.direction
drive_msg.drive.speed = velocity * self.direction
self.drive_pub.publish(drive_msg)
# update the internal state variables
self.prev_error = error
self.prev_time = now
def camera_callback(self, data):
start = datetime.now()
try:
cv_image = self.bridge_object.imgmsg_to_cv2(
data, desired_encoding="bgr8")
except CvBridgeError as e:
print(e)
h = cv_image.shape[0]
w = cv_image.shape[1]
cropped_bgr_img = cv_image[int(h * 0.5):, int(w * 0.2):int(w * 0.8)]
lane_img = lane_utils.find_yellow_lanes(cropped_bgr_img)
centroid_img, (cX, cY) = lane_utils.find_n_largest_contours(lane_img)
cv2.imshow("Image", cropped_bgr_img)
cv2.imshow("Centroid", centroid_img)
cv2.waitKey(1)
h = cropped_bgr_img.shape[0]
w = cropped_bgr_img.shape[1]
# if cX > h*0.6:
# my_steering_angle = -np.arctan(cY-w/0.3)
# temp_percent = cX/h
# elif cX > h*0.45:
# my_steering_angle = -np.arctan((cY-w/2)/2)
if cX > w / 2:
my_steering_angle = -0.7 #-np.arctan((cY-w/2) / (h-cX+1e-20))
else:
my_steering_angle = 0.7
acker_object = AckermannDriveStamped()
acker_object.drive.steering_angle = my_steering_angle
print(cY, w / 2)
acker_object.drive.steering_angle_velocity = my_steering_angle / 200
acker_object.drive.speed = 0.5
self.drivecar_object.move_robot(acker_object)
def __init__(self):
#Topics & Subs, Pubs
lidarscan_topic = '/scan'
drive_topic = '/drive'
teleop_topic = '/turtle1/cmd_vel'
self.lidar_sub = rospy.Subscriber(lidarscan_topic, LaserScan, self.lidar_callback) #TODO: Subscribe to LIDAR
self.drive_pub = rospy.Publisher(drive_topic, AckermannDriveStamped) #TODO: Publish to drive
self.teleop_sub = rospy.Subscriber(teleop_topic, Twist, self.teleop_callback) # Subscribe to /keyboard from teleop_key
# Parameters
self.D = 1.0 # default D
self.window_size = 10 # to compute a, b as mean values
self.theta = math.radians(30.0) # theta, your choice
self.theta_comp = math.pi/2 - self.theta # the complement angle of theta
# PID CONTROL PARAMS
self.kp = 10.0
self.ki = 0.0
self.kd = 0.5
self.prev_error = 0.0
self.error = 0.0
self.integral = 0.0
self.min_angle = -0.4
self.max_angle = 0.4
self.servo_offset = 0.0 # offset of the car
self.angle = 0 # initial steering angle
self.velocity = 1 # initial velocity
self.freq = 10 # control frequency
## Driving msg
self.drive_msg = AckermannDriveStamped()
self.drive_msg.header.stamp = rospy.Time.now()
self.drive_msg.header.frame_id = "laser"
self.drive_msg.drive.steering_angle = 0.0
self.drive_msg.drive.speed = 0.0
self.start = 0
def callback(data):
global speed
global steering_angle
global distance_set
global acceleration
distance_set = [get_range(data, i) for i in range(90, -91, -1)]
steering_angle = 0.0
acceleration = 0.0
perception(data)
if simulator_mode == True:
drive_msg = AckermannDrive(steering_angle=steering_angle,
speed=speed,
acceleration=acceleration)
drive_st_msg = AckermannDriveStamped(drive=drive_msg)
drive_pub_simulator.publish(drive_st_msg)
else:
drive_msg = AckermannDrive(steering_angle=steering_angle,
speed=speed,
acceleration=acceleration)
drive_pub.publish(drive_msg)
def __init__(self):
# node setup
rospy.init_node("person_follower", anonymous=True)
# state from subscribers
self.tracker = PersonTracker()
self.right_bumper = False
self.last_bounding_box_timestamp = 0
self.last_speed = 0
self.last_angle = 0
# subscriber setup
sub = message_filters.Subscriber('/darknet_ros/bounding_boxes',
BoundingBoxes)
depth_sub = rospy.Subscriber('/camera/depth/image_rect_raw', Image,
self.tracker.depth_cb)
joy_sub = rospy.Subscriber('/vesc/joy', Joy, self.right_bump)
img_sub = message_filters.Subscriber('/camera/color/image_raw', Image)
ts = message_filters.ApproximateTimeSynchronizer([sub, img_sub], 10,
0.1)
sub.registerCallback(self.tracker.bounding_box_cb)
ts.registerCallback(self.tracker.img_cb)
# publisher setup
pub = rospy.Publisher('/vesc/low_level/ackermann_cmd_mux/input/teleop',
AckermannDriveStamped,
queue_size=10)
seq = 0
rate = rospy.Rate(30)
while not rospy.is_shutdown():
seq += 1
header = Header(seq=seq, stamp=rospy.Time.now())
angle, speed = self.get_steering_direction()
if self.right_bumper and angle and speed:
pub.publish(
AckermannDriveStamped(
header,
AckermannDrive(steering_angle=angle, speed=speed)))
rate.sleep()
rospy.spin()
def cmd_callback(data):
global wheelbase
global ackermann_cmd_topic
global frame_id
global pub
global cmd_angle_instead_rotvel
v = data.linear.x
# if cmd_angle_instead_rotvel is true, the rotational velocity is already converted in the C++ node
# in this case this script only needs to do the msg conversion from twist to Ackermann drive
if cmd_angle_instead_rotvel:
steering = data.angular.z
else:
steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase)
msg = AckermannDriveStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.drive.steering_angle = steering
msg.drive.speed = v
pub.publish(msg)
def servo_commands():
#print("Car is at :",pos[0],pos[1])
#print("Enter Waypoints:")
#time.sleep(2)
global x_des
global y_des
global sub
global count
#count +=1
df.columns = df.columns.str.strip()
#print("Car is at :",pos[0],pos[1])
#x_des = float(input())
#y_des = float(input())
#ix_des = (df.X[count])
#y_des = (df.Y[count])
#print("Navigating to:",x_des, y_des)
count += 1
#rospy.init_node('servo_commands', anonymous=True)
msg = AckermannDriveStamped()
#rospy.Subscriber("/progress", Progress, set_throttle_steer)
sub = rospy.Subscriber("/gazebo/model_states", ModelStates, set_position)
while not (rospy.is_shutdown()):
"""
print("====position=====",pos[0],pos[1])
speed_control = PID_control.PID(0.000001,0,0.000001)
err = math.sqrt((x_des-pos[0])**2+(y_des-pos[1])**2)
throttle = speed_control.Update(err)
print("distance:", err)
print("throttle:",throttle)
"""
#msg.drive.speed = 0.0
#x_pub.publish(msg)
time.sleep(0.1)
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
def cmd_callback(data):
global wheelbase
global ackermann_cmd_topic
global frame_id
global pub
# v = data.linear.x
# steering = convert_trans_rot_vel_to_steering_angle(v, data.angular.z, wheelbase)
v = math.sqrt(data.linear.x**2 + data.linear.y**2)
steering = math.atan2(wheelbase * data.angular.z, v)
msg = AckermannDriveStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = frame_id
msg.drive.steering_angle = data.angular.z
msg.drive.speed = v
msg.drive.acceleration = 1
msg.drive.jerk = 1
msg.drive.steering_angle_velocity = 1
print msg
pub.publish(msg)
def controller(self, error, pub_cmd):
"""
Applies prorpotional steering gain (self.Kp)
Caps steering angle at +/- self.steering_saturation
Issues drive message with arbitrary drive speed and calculated
steering angle.
"""
cmd_msg = AckermannDriveStamped()
cmd_msg.drive.speed = self.drive_speed
# apply proportional controller gain (self.Kp)
raw_steering_angle = error * self.Kp
# cap steering angle at self.steering_saturation
if abs(raw_steering_angle) > self.steering_saturation:
steering_angle = math.copysign(self.steering_saturation,
raw_steering_angle)
# insert steering angle and issue drive command
cmd_msg.drive.steering_angle = steering_angle
pub_cmd.publish(cmd_msg)
def pose_cb(self, msg):
cur_pose = np.array([msg.pose.position.x,
msg.pose.position.y,
utils.quaternion_to_angle(msg.pose.orientation)])
success, error = self.compute_error(cur_pose)
if not success:
# We have reached our goal
self.pose_sub = None # Kill the subscriber
self.speed = 0.0 # Set speed to zero so car stops
delta = self.compute_steering_angle(error)
# Setup the control message
ads = AckermannDriveStamped()
ads.header.frame_id = '/map'
ads.header.stamp = rospy.Time.now()
ads.drive.steering_angle = delta
ads.drive.speed = self.speed
# Send the control message
self.cmd_pub.publish(ads)
def __init__(self):
"""initalize the node"""
rospy.init_node("driveStop")
self.pub = rospy.Publisher(DRIVE_TOPIC, AckermannDriveStamped, queue_size = 1)
rospy.Subscriber("scan", LaserScan, self.driveStop_car_callback)
""" initialize the box dimensions"""
"""driving code"""
self.cmd = AckermannDriveStamped()
self.cmd.drive.speed = 0.5
self.cmd.drive.steering_angle = 0
self.center1 = (309, 158)
self.center2 = (347, 219)
self.signTrack = True
"""get the camera data from the class Zed_converter in Zed"""
self.camera_data = Zed_converter(False, save_image = False)
self.min_value=0
def __init__(self):
"""initalize the node"""
rospy.init_node("driveStop")
self.pub = rospy.Publisher(DRIVE_TOPIC,
AckermannDriveStamped,
queue_size=1)
self.image_pub = rospy.Publisher(IMAGE_TOPIC, Image, queue_size=2)
rospy.Subscriber("scan", LaserScan, self.driveStop_car_callback)
""" initialize the box dimensions"""
self.flag_box = ((0, 0), (0, 0))
self.flag_center = (0, 0)
self.flag_size = 0
"""driving stuff"""
self.cmd = AckermannDriveStamped()
self.cmd.drive.speed = 0
self.cmd.drive.steering_angle = 0
"""get the camera data from the class Zed_converter in Zed"""
self.camera_data = Zed_converter(False, save_image=False)
self.imagePush = None
self.bridge = CvBridge()
self.min_value = 0
def drive_callback(self, data):
# Get point and transform into
data.header.stamp = self.listener.getLatestCommonTime(
self.base_frame, data.header.frame_id)
dest = self.listener.transformPoint(self.base_frame, data)
x = dest.point.x
y = dest.point.y
# Computer dist and theta
distance = math.pow(math.pow(x, 2) + math.pow(y, 2), 0.5)
theta = math.atan2(y, x)
# Prepare to Publish Drive Msg
msg = AckermannDriveStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "base_link"
# if obj too close, drive backwards, else calc speed and theta
# tested for points ahead/left, ahead/right, ahead, behind, and current loc
if x <= 0.2:
msg.drive.steering_angle = theta
msg.drive.speed = -0.3
else:
self.distanceI = self.distanceI + distance
dp = self.kp * distance
di = self.kd * (distance - self.lastDistance)
dd = self.ki * (self.distanceI)
self.lastDistance = distance
msg.drive.speed = max(0.1, min(self.max_speed, dp + di + dd))
self.thetaI = self.thetaI + theta
tp = self.k_steer * theta
ti = self.ki * (self.thetaI)
td = self.kd * (theta - self.lastTheta)
msg.drive.steering_angle = max(min(self.max_steering_angle, theta),
-1 * self.max_steering_angle)
self.lastTheta = theta
print(msg.drive.speed, msg.drive.steering_angle)
self.drive_pub.publish(msg)
def control_car(t1, pos, yaw, prev_err, prev_head):
msg = AckermannDriveStamped()
### PID for throttle control ###
speed_control = PID_control.PID(0.5, 0, 0.8)
err = math.sqrt((x_des - pos[0])**2 + (y_des - pos[1])**2)
dt = time.time() - t1
throttle = speed_control.Update(dt, prev_err, err)
### PID for steer control ###
steer_control = PID_control.PID(0.5, 0, 0)
head = math.atan((y_des - pos[1]) / (x_des - pos[0] + 0.01))
steer = steer_control.Update(dt, prev_head - yaw, head - yaw)
### Publish the control signals to Ackermann control topic ###
msg.drive.speed = throttle
msg.drive.steering_angle = steer
x_pub.publish(msg)
# Store previous error
prev_err = err
def __init__(self):
#Topics & Subs, Pubs
lidarscan_topic = '/scan'
drive_topic = '/vesc/high_level/ackermann_cmd_mux/input/nav_0'
self.lidar_sub = rospy.Subscriber(lidarscan_topic, LaserScan, self.lidar_callback) # Subscribe to LIDAR
self.drive_pub = rospy.Publisher(drive_topic, AckermannDriveStamped) # Publish to drive
# Parameters
self.D = 1.0 # default D
self.window_size = 5 # to compute a, b as mean values
self.theta = math.radians(10.0) # theta, your choice
self.theta_comp = math.pi/2 - self.theta # the complement angle of theta
self.a_l = 0.0
self.b_l = 0.0
self.a_r = 0.0
self.b_r = 0.0
# PID CONTROL PARAMS
self.kp = 5.0
self.ki = 0.
self.kd = 0.1
self.prev_error = 0.0
self.error = 0.0
self.integral = 0.0
self.min_angle = -0.4
self.max_angle = 0.4
self.servo_offset = 0.0 # offset of the car
self.angle = 0 # initial steering angle
self.velocity = 1 # initial velocity
self.freq = 5 # control frequency
## Driving msg
self.drive_msg = AckermannDriveStamped()
self.drive_msg.header.stamp = rospy.Time.now()
self.drive_msg.header.frame_id = "laser"
self.drive_msg.drive.steering_angle = 0.0
self.drive_msg.drive.speed = 0.0
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)
