class DriveForwardNode:
def __init__(self):
self.drive_publisher = rospy.Publisher(
'/vesc/high_level/ackermann_cmd_mux/input/nav_0',
AckermannDriveStamped,
queue_size=10)
self.odom_subscriber = rospy.Subscriber('/vesc/odom',
Odometry,
self.odom_callback,
queue_size=10)
self.drive_controller = PIDController(kP=1)
self.steer_controller = PIDController(kP=0.01)
self.drive_msg = AckermannDriveStamped()
def odom_callback(self, msg):
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
q = msg.pose.pose.orientation
roll, pitch, yaw = tf.transformations.euler_from_quaternion(
(q.x, q.y, q.z, q.w))
#steer_output = self.steer_controller.output(yaw, 0)
steer_output = -1
drive_output = self.drive_controller.output(x, 0)
print("Error: %s" % (x - 0))
self.drive_msg.drive.steering_angle = steer_output
self.drive_msg.drive.speed = drive_output
self.drive_publisher.publish(self.drive_msg)
class StateMachineNode:
def __init__(self):
rospy.init_node('state_machine')
self.drive_publisher = rospy.Publisher(DRIVE_PUBLISHER_TOPIC,
AckermannDriveStamped,
queue_size=10)
self.laser_subscriber = rospy.Subscriber(LASER_SUBSCRIBER_TOPIC,
LaserScan,
self.laser_callback,
queue_size=10)
self.markers_subscriber = rospy.Subscriber(AR_TAG_SUBSCRIBER_TOPIC,
AlvarMarkers,
self.markers_callback,
queue_size=10)
self.pf_controller = PotentialFieldController(gain=0.3)
self.pid_controller = PIDController(kP=1.8)
self.laser_data = []
self.state = State.potential_field
self.drive_msg = AckermannDriveStamped()
self.drive_msg.drive.speed = 0.7
self.goal_dist = 0.3
def laser_callback(self, msg):
self.laser_data = msg.ranges
def markers_callback(self, msg):
self.check_state(msg.markers)
print("State: %s" % self.state)
if self.state == State.potential_field:
self.drive_msg.drive.steering_angle = self.pf_controller.output(
self.laser_data)
else:
end = int(0.7 * len(self.laser_data))
perpendicular_dist = 0.0
if self.state == State.follow_left:
perpendicular_dist = min(self.laser_data[end:])
elif self.state == State.follow_right:
perpendicular_dist = min(self.laser_data[:end])
print("Dist: %s" % perpendicular_dist)
print("Error: %s" % (perpendicular_dist - self.goal_dist))
self.drive_msg.drive.steering_angle = -self.pid_controller.output(
perpendicular_dist, self.goal_dist)
self.drive_publisher.publish(self.drive_msg)
def check_state(self, markers):
ids = [m.id for m in markers]
if 23 in ids:
self.state = State.potential_field
elif 18 in ids or 22 in ids:
self.state = State.follow_left
elif 19 in ids:
self.state = State.follow_right
class WallFollowerNode:
def __init__(self):
rospy.init_node(STATE_MACHINE_NODE_NAME)
self.laser_subscriber = rospy.Subscriber('/scan',
LaserScan,
self.laser_callback,
queue_size=10)
self.drive_publisher = rospy.Publisher(
'/vesc/high_level/ackermann_cmd_mux/input/nav_0',
AckermannDriveStamped,
queue_size=10)
self.steering_controller = PIDController(kP=1.2, kD=0)
self.drive_msg = AckermannDriveStamped()
self.cv_bridge = CvBridge()
self.goal_distance = 0.8
self.drive_speed = 1.2
def laser_callback(self, msg):
cur_dist = min(
msg.ranges[int(0.4 * len(msg.ranges)):int(0.6 * len(msg.ranges))])
front_dist = msg.ranges(len(msg.ranges) / 2)
steering_output = self.steering_controller.output(
cur_dist, self.goal_distance)
if front_dist < 2.0:
self.drive_speed = 0.7
else:
self.drive_speed = 1.2
self.drive_msg.drive.steering_angle = output
self.drive_msg.drive.speed = self.drive_speed
print("Distance from wall: %s" % cur_dist)
print("Error: %s" % (cur_dist - self.goal_distance))
print("State: %s" % self.state)
self.drive_publisher.publish(self.drive_msg)
class Drive:
def __init__(self):
self.drive_publisher = rospy.Publisher(DRIVE_TOPIC,
AckermannDriveStamped,
queue_size=10)
self.laser_subscriber = rospy.Subscriber(LASER_TOPIC,
LaserScan,
self.laser_callback,
queue_size=10)
self.odom_subscriber = rospy.Subscriber(ODOM_TOPIC,
Odometry,
self.odom_callback,
queue_size=10)
self.middle_pid_controller = PIDController(kP=0.58)
self.turn_pid_controller = PIDController(kP=0.92)
self.angle_pid_controller = PIDController(kP=0.001)
self.ptf_controller = PotentialFieldController(steer_gain=4.4,
speed_gain=1.0,
alpha=0.92,
mu=0.06)
self.drive_msg = AckermannDriveStamped()
self.laser_data = []
self.pose = None
self.is_referenced = False
self.reference_dist = 0.0
self.reference_angle = 0.0
self.safety_min_dist = 0.25
self.adjustment = 30
def drive_safety(self):
speed = -0.7
angle = 0.0
if self.safety_check:
angle = -1.0
else:
angle = 1.0
self.drive(angle, speed)
def safety_check(self):
return self.laser_data[300] > self.laser_data[780]
def is_safe(self):
front_dist = self.laser_data[len(self.laser_data) / 2]
print("Front dist: %s " % front_dist)
return front_dist > self.safety_min_dist
def drive_middle(self, speed):
left_dist, right_dist = self.get_dists()
angle = -self.middle_pid_controller.output(left_dist - right_dist, 0)
print("Error: %s" % (left_dist - right_dist))
self.drive(angle, speed)
def drive_left(self, goal_dist, speed):
left_dist, _ = self.get_dists()
angle = -self.turn_pid_controller.output(left_dist, goal_dist)
print("Error: %s" % (goal_dist - left_dist))
self.drive(angle, speed)
def drive_right(self, goal_dist, speed):
_, right_dist = self.get_dists()
angle = self.turn_pid_controller.output(right_dist, goal_dist)
print("Error: %s " % (goal_dist - right_dist))
self.drive(angle, speed)
def drive_potential(self, speed):
#laser_scan = self.laser_data[int(0.1*len(self.laser_data)):int(0.9*len(self.laser_data))]
angle, new_speed = self.ptf_controller.output(self.laser_data)
print("Angle: %s " % angle + " Speed: %s" % new_speed)
self.drive(-angle, speed)
def drive_straight(self, speed, dist):
if self.pose is not None:
if not self.is_referenced:
self.reference_dist = self.pose.x
self.reference_angle = self.pose.angle
self.is_referenced = True
print("Referecenced")
pos_error = self.pose.x - self.reference_dist
print("Change in dist: %s " % pos_error)
if abs(pos_error) < dist:
angular_output = self.angle_pid_controller.output(
self.pose.angle, self.reference_angle)
self.drive(angular_output, speed)
return False
self.is_referenced = False
return True
return False
def drive(self, angle, speed):
self.drive_msg.drive.speed = speed
self.drive_msg.drive.steering_angle = angle
print("Angle: %s " % angle + " Speed: %s" % speed)
self.drive_publisher.publish(self.drive_msg)
def get_dists(self):
offset = len(self.laser_data) / 4
adjustment = 0
left_dist = min(self.laser_data[len(self.laser_data) -
offset:len(self.laser_data) - offset +
self.adjustment])
right_dist = min(self.laser_data[offset:offset + self.adjustment])
#left_dist = min(self.laser_data[int(0.5 * len(self.laser_data)):int(0.9 * len(self.laser_data))])
#right_dist = min(self.laser_data[int(0.1 * len(self.laser_data)):int(0.5 * len(self.laser_data))])
return left_dist, right_dist
def laser_callback(self, msg):
self.laser_data = msg.ranges
def odom_callback(self, msg):
p = msg.pose.pose.position
q = msg.pose.pose.orientation
x, y = p.x, p.y
_, _, yaw = tf.transformations.euler_from_quaternion(
(q.x, q.y, q.z, q.w))
self.pose = Pose(x=x, y=y, angle=yaw)
class StateMachineNode:
def __init__(self):
rospy.init_node(STATE_MACHINE_NODE_NAME)
self.ar_subscriber = rospy.Subscriber('/ar_pose_marker',
AlvarMarkers,
self.ar_callback,
queue_size=10)
self.img_subscriber = rospy.Subscriber('/zed/rgb/image_rect_color',
Image,
self.img_callback,
queue_size=10)
self.laser_subscriber = rospy.Subscriber('/scan',
LaserScan,
self.laser_callback,
queue_size=10)
self.drive_publisher = rospy.Publisher(
'/vesc/high_level/ackermann_cmd_mux/input/nav_0',
AckermannDriveStamped,
queue_size=10)
self.steering_controller = PIDController(kP=1.4, kD=0)
self.drive_msg = AckermannDriveStamped()
self.cv_bridge = CvBridge()
self.goal_tag = -1
self.goal_distance = 0.7
self.drive_speed = 1.2
self.ar_tag_threshold = 0.8
self.is_found = False
self.state = State.search_for_face
def ar_callback(self, msg):
if len(msg.markers) > 0:
for tag in msg.markers:
id = tag.id
print("I see id %s" % id)
if id == self.goal_tag and self.state == State.search_for_tag\
and tag.pose.pose.position.z < self.ar_tag_threshold:
self.state = State.found_tag
self.is_found = True
elif self.is_found:
self.state = State.search_for_face
self.is_found = False
elif self.is_found:
self.state = State.search_for_face
self.is_found = False
def img_callback(self, msg):
global faces_encodings
img_cv = self.cv_bridge.imgmsg_to_cv2(msg, 'bgr8')
rows, cols = img_cv.shape[0], img_cv.shape[1]
# img_left = img_cv[:, :int(0.9*cols)]
img_left = img_cv
# Collect results of face detection
results = face_recognition.compare_faces(faces_encodings, img_left)
distances = face_recognition.face_distance(faces_encodings, img_left)
# Array containing the indices of found faces
found_faces = [i for i in range(len(results)) if results[i] == True]
print(results)
# if len(found_faces) > 0 and self.state == State.search_for_face:
if len(found_faces) > 0:
print("I see faces")
# List of tuples containing the distance AND the found index
found_distances = [(distances[found_faces[j]], j)
for j in range(len(found_faces))]
min_area = float('inf')
index = 0
for area, i in found_distances:
if area < min_area:
min_area = area
index = i
self.goal_tag = index + 1
self.state = State.found_face
# If it no longer sees the face yet it's still in the found state
elif (self.goal_tag -
1) not in found_faces and self.state == State.found_face:
self.state = State.search_for_tag
def laser_callback(self, msg):
output = 0.0
cur_dist = min(
msg.ranges[int(0.4 * len(msg.ranges)):int(0.85 * len(msg.ranges))])
if cur_dist > 1.5:
output = -1.0
else:
output = -self.steering_controller.output(cur_dist,
self.goal_distance)
if self.state == State.search_for_face or self.state == State.found_tag:
self.drive_speed = 1.2
elif self.state == State.search_for_tag or self.state == State.found_face:
self.drive_speed = -1.2
self.drive_msg.drive.steering_angle = output
self.drive_msg.drive.speed = self.drive_speed
print("Distance from wall: %s" % cur_dist)
print("Error: %s" % (cur_dist - self.goal_distance))
print("State: %s" % self.state)
self.drive_publisher.publish(self.drive_msg)
class WallFollowerNode:
def __init__(self):
rospy.init_node('wall_follower')
self.laser_subscriber = rospy.Subscriber('/scan', LaserScan,self.laser_callback, queue_size=10)
self.drive_publisher = rospy.Publisher('/drive', AckermannDriveStamped, queue_size=10)
#flag
self.right = 0
self.left = 0
#inizializzo Pid
self.steering_controller = PIDController(kP=1.2, kD=0.0)
#creo messaggio
self.drive_msg = AckermannDriveStamped()
# self.scan = LaserScan()
self.goal_distance = 1.2
self.color=[]
self.diffr= 0
def laser_callback(self,msg):
#fascio frontale
cur_dist = min(msg.ranges[int(0.45*len(msg.ranges)):int(0.55*len(msg.ranges))])
# laterale a -2.47 dx dietro
side_dist_r1 = min(msg.ranges[int(0*len(msg.ranges)):int(0.01*len(msg.ranges))])
# dx avanti
side_dist_r2 = min(msg.ranges[int(0.33*len(msg.ranges)):int(0.34*len(msg.ranges))])
# sx avanti
side_dist_l1 = min(msg.ranges[int(0.65*len(msg.ranges)):int(0.8*len(msg.ranges))])
# sx dietro
side_dist_l2 = min(msg.ranges[int(0.8*len(msg.ranges)):int(len(msg.ranges))])
# di fronte
front_dist = msg.ranges[int(len(msg.ranges)/2)]
# angolo 0
destra = msg.ranges[int(18)]
#self.color = msg.intensities[int(len(msg.intensities)/2)]
# color = min(ms[int(0.5*len(msg.ranges)):int(0.5*len(msg.ranges))])
steering_output = self.steering_controller.output(cur_dist, self.goal_distance)
if side_dist_r1 > 1.4:
self.right = 0
self.drive_msg.drive.steering_angle = 0
if side_dist_l2 > 1.4:
self.left = 0
self.drive_msg.drive.steering_angle = 0
if side_dist_r1 < 1.4:
self.right = 1
if side_dist_l2 < 1.4:
self.left = 1
# trova corridoio
if destra > side_dist_r2:
if destra > side_dist_r1:
if destra > 3.5 and destra < 4:
#gira a dx per 1 secondo
self.turn()
self.diffr = side_dist_r2-side_dist_r1
# procedi dritto muro lontano
if cur_dist > 1.4 :
self.drive_msg.drive.speed = steering_output
self.drive_msg.drive.steering_angle = 0
#gira a dx
if self.diffr > 0.1:
self.drive_msg.drive.steering_angle = -1.2
# muro avanti gira a sx
if cur_dist < 1.4 :
self.drive_msg.drive.steering_angle = +1.2
self.drive_msg.drive.speed = 0.5
# muro a sinistra vicino gira a dx
if side_dist_l2 < 1.4:
self.drive_msg.drive.steering_angle = -1.2
self.left = 1
################### vari print ################################
#print("angle: %s" % self.steering_angle.output(self.laser_data))
#print("Distance from wall: %s" % front_dist)
#print("Colore: %s" % self.color)
#print("LEFT: %s" % side_dist_l2)
print("RIGHT1: %s" % side_dist_r1)
print("destra: %s" % destra)
print("RIGHT2: %s" % side_dist_r2)
print("ZERO: %s" % msg.ranges[int(0)])
#print("diffr: %s" % self.diffr)
#print("angle_increment: %s" % msg.angle_increment)
#print("min_angle: %s" % msg.angle_min)
##################################################################
#pubblica messaggio
self.drive_publisher.publish(self.drive_msg)
def turn (self):
# gira a dx per 1 secondo
self.drive_msg.drive.steering_angle = -1.8
self.drive_msg.drive.speed = 1
self.drive_publisher.publish(self.drive_msg)
rospy.sleep(1)
cur_dist = min(msg.ranges[int(0.45*len(msg.ranges)):int(0.55*len(msg.ranges))])
if cur_dist < 1:
rospy.spin()
class StateMachineNode:
def __init__(self):
rospy.init_node(STATE_MACHINE_NODE_NAME)
self.drive_publisher = rospy.Publisher(
'/vesc/high_level/ackermann_cmd_mux/input/nav_0',
AckermannDriveStamped,
queue_size=10)
self.ar_subscriber = rospy.Subscriber('/ar_pose_marker',
AlvarMarkers,
self.ar_callback,
queue_size=10)
print "Ar subscriber created"
self.img_subscriber = rospy.Subscriber('/zed/rgb/image_rect_color',
Image,
self.img_callback,
queue_size=10)
print "Img subscriber created"
self.laser_subscriber = rospy.Subscriber('/scan',
LaserScan,
self.laser_callback,
queue_size=10)
# print "Laser subscriber created"
# self.drive_publisher = rospy.Publisher('/vesc/high_level/ackermann_cmd_mux/input/nav_0', AckermannDriveStamped,
# queue_size=10)
print "Drive publisher created"
self.steering_controller = PIDController(kP=0.1)
self.drive_msg = AckermannDriveStamped()
self.cv_bridge = CvBridge()
self.goal_tag = -1
self.goal_distance = 0.7
self.drive_speed = 0.7
self.ar_tag_threshold = 0.8
self.state = State.search_face
self.haar = cv2.CascadeClassifier(
'./haarcascade_frontalface_default.xml')
def ar_callback(self, msg):
if len(msg.markers) > 0:
if self.goal_tag in msg.markers and self.state == State.search_tag:
tagdata = [x for x in msg.markers if x.id == self.goal_tag][0]
if tagdata.pose.pose.position < self.ar_tag_threshold: return
self.state = State.approach_tag
self.drive_speed = 0.7
elif self.goal_tag not in msg.markers and self.state == State.approach_tag:
self.state = State.search_face
def img_callback(self, msg):
global faces_encodings
img_cv = self.cv_bridge.imgmsg_to_cv2(msg, 'bgr8')
rows, cols = img_cv.shape[0], img_cv.shape[1]
# Find face in image
faces = self.haar.detectMultiScale(img_cv,
scaleFactor=1.1,
minNeighbors=5)
if self.state == State.search_face:
for x, y, w, h in faces:
# Get encoding
encoding = face_recognition.face_encodings(
img_cv[x:x + w + 1:, y:y + h + 1:, :],
known_face_locations=None)
# Compare the encoding to the list of faces
results = face_recognition.api.compare_faces(
faces_encodings, encoding)
if any(results):
self.goal_tag = min(
[i for i in range(len(results)) if results[i] == True])
self.state = State._face
elif self.state == State.approach_face and len(faces) == 0:
self.state = State.search_tag
self.drive_speed = -0.7
def laser_callback(self, msg):
# If it's searching for a face or a tag, stay away from walls
if self.state == State.search_face or self.state == State.search_tag:
self.goal_distance = 0.7
# If it's found something, approach it
elif self.state == State.approach_face or self.state == State.approach_tag:
self.goal_distance = 0.2
cur_dist = min(msg.ranges[int(0.5 * len(msg.ranges)):])
output = self.steering_controller.output(cur_dist, self.goal_distance)
self.drive_msg.drive.steering_angle = output
self.drive_msg.drive.speed = self.drive_speed
self.drive_publisher.publish(self.drive_msg)