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 __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 __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
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
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 __init__(self, num_sensors=4):
"""
:param initial_values: a list of tuples (or lists) that
are the initial values for each PID controller
"""
initial_values = [[0.0, 0.0, 0.0]] * num_sensors
self.pids = []
self.num_pids = len(initial_values)
for initial in initial_values:
self.pids.append(PIDController(pid_constants=initial))
def __init__(self):
self.drive_pub = rospy.Publisher(
"/vesc/high_level/ackermann_cmd_mux/input/nav_0",
AckermannDriveStamped,
queue_size=10)
self.line_sub = rospy.Subscriber('/line_error',
Float32,
self.line_callback,
queue_size=10)
self.ar_sub = rospy.Subscriber("/ar_pose_marker",
AlvarMarkers,
self.ar_callback,
queue_size=10)
self.msg = AckermannDriveStamped()
self.steering_controller = PIDController(kP=0.0007)
self.state = State.search
self.prev_angle = 0
self.ar_markers = []
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 __init__(self):
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=2.0, kD=0)
self.drive_msg = AckermannDriveStamped()
self.goal_distance = 0.8
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 __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 pid_controller_test(StudentPIDC):
k_p = 1.5
k_d = 2.0
k_i = 1.2
m = 2.5
z_target = 2.0
z_actual = 3.2
z_dot_target = -2.8
z_dot_actual = -2.7
dt = 0.1
controller = PIDController(k_p, k_d, k_i, m)
scontroller = StudentPIDC(k_p, k_d, k_i, m)
for _ in range(3):
thrust = controller.thrust_control(z_target, z_actual, z_dot_target,
z_dot_actual, dt)
s_thrust = scontroller.thrust_control(z_target, z_actual, z_dot_target,
z_dot_actual, dt)
if close_enough_floats(thrust, s_thrust):
print("Tests pass")
else:
print("Tests fail. Off by %3.3f percent" % pct_diff(thrust, s_thrust))
class StateMachineNode:
def __init__(self):
self.drive_pub = rospy.Publisher(
"/vesc/high_level/ackermann_cmd_mux/input/nav_0",
AckermannDriveStamped,
queue_size=10)
self.line_sub = rospy.Subscriber('/line_error',
Float32,
self.line_callback,
queue_size=10)
self.ar_sub = rospy.Subscriber("/ar_pose_marker",
AlvarMarkers,
self.ar_callback,
queue_size=10)
self.msg = AckermannDriveStamped()
self.steering_controller = PIDController(kP=0.0007)
self.state = State.search
self.prev_angle = 0
self.ar_markers = []
def line_callback(self, msg):
output = 0
if len(self.ar_markers) > 0:
for i in self.ar_markers:
if i.id == 3 and i.pose.pose.position.z < 0.55:
print(i.pose.pose.position.z)
self.msg.drive.speed = 0
else:
output = self.steering_controller.output_error(msg.data)
self.msg.drive.steering_angle = -output
# speed = abs(0.05 / (output + 0.001) + 0.5)
speed = 1.0
#print("Speed: %s" % speed)
self.msg.drive.speed = speed
self.drive_pub.publish(self.msg)
self.prev_angle = output
def ar_callback(self, msg):
self.ar_markers = msg.markers
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)
def __init__(self, extendArea):
self.sim = Simulator('127.0.0.1', 25001)
self.sim.connect()
### -----------
### GET HANDLES
# ROBOT
self.robotHandle = self.sim.getHandle("Pioneer_p3dx")
# ACTUATORS
self.motorHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftMotor')
self.motorHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightMotor')
# SONARS
for x in range(0, len(self.sonarHandle)):
self.sonarHandle[x] = self.sim.getHandle('Pioneer_p3dx_ultrasonicSensor'+str(x+1))
# VISION
#self.visionHandle = self.sim.getHandle('camera')
self.visionHandle = 0
# WHEEL
self.wheelHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftWheel')
self.wheelHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightWheel')
### -----------
### INIT STREAMS
self.sim.initObjectPositionStream(self.robotHandle, -1)
self.sim.initObjectOrientationStream(self.robotHandle, -1)
for x in range(0, len(self.sonarHandle)):
self.sim.initProximitySensor(self.sonarHandle[x])
self.sim.initFloatSignal("gyroZ")
#self.sim.initVisionSensorImage(self.visionHandle)
self.sim.initObjectVelocity(self.robotHandle)
self.sim.initJointPositionStream(self.motorHandle[0])
self.sim.initJointPositionStream(self.motorHandle[1])
self.sim.initLaserSensor("measuredDataAtThisTime")
### -----------
### POSTIONS
# SONAR (em relacao ao frame do robo)
for x in range(0, len(self.sonarHandle)):
self.SONAR_POSITIONS[x] = self.sim.getObjectPositionBlock(
self.sonarHandle[x], self.robotHandle)
self.L = np.linalg.norm(self.sim.getObjectPositionBlock(
self.wheelHandle[0], self.wheelHandle[1]))
self.L2 = -self.sim.getObjectPositionBlock(self.wheelHandle[0], self.robotHandle)[0]
# Robo precisa estar no chão para pegar o raio
self.R = self.sim.getObjectPositionBlock(self.wheelHandle[0], -1)[2]
#self.R = 0.09749898314476013
# ROBO
firstRobotPosOrn = self.getPosOrn()
# Odometry
self.odometryPosOrn = {}
self.odometryPosOrn['raw'] = firstRobotPosOrn
self.odometryPosOrn['encoder'] = firstRobotPosOrn
self.odometryPosOrn['compass'] = firstRobotPosOrn
self.odometryEncoder = {}
self.odometryEncoder['encoder'] = [None] * 2
self.odometryEncoder['encoder'][0] = self.sim.getJointPosition(self.motorHandle[0])
self.odometryEncoder['encoder'][1] = self.sim.getJointPosition(self.motorHandle[1])
self.odometryEncoder['compass'] = [None] * 2
self.odometryEncoder['compass'][0] = self.sim.getJointPosition(self.motorHandle[0])
self.odometryEncoder['compass'][1] = self.sim.getJointPosition(self.motorHandle[1])
self.odometryTimeRaw = time.time()
self.odometryTimeCompass = time.time()
self.odometryTimeEncoderCompass = time.time()
self.motorW = [None] * 2
self.motorW[0] = 0
self.motorW[1] = 0
# Map grid
x, y, _ = self.getPosOrn()
mapImage = self.sim.getVisionSensorImageBlock(self.sim.getHandle('mapSensor'))
self.gridMap = GridMap(mapImage, extendArea)
x, y, _ = self.getPosOrn()
x, y = self.gridMap.convertToMapUnit(x, y)
self.gridMap.removeCluster(x, y)
self.gridMap.show()
# Controllers
self.goToGoalPID = PIDController(0, 50, 0, 15, windowSize=1000)
self.anglePID = PIDController(0, 10, 0, 10, windowSize=1000)
NUM_TICS = 200000 # 120 minutes
TARGET_TEMP = 35.0
AMBIENT_TEMP = 22.0
def simulate(starting_temp, ambient_temp, controller, renderer, t=.036):
sim = Sim(starting_temp, ambient_temp)
for i in range(NUM_TICS):
renderer.render(sim, controller)
controller.before_tick(sim, tictime=t)
sim.tick(tictime=t)
controller.after_tick(sim, tictime=t)
sim.cleanup()
controller = PIDController(TARGET_TEMP)
renderer = UbidotsRenderer()
# simulate(AMBIENT_TEMP, AMBIENT_TEMP, controller, renderer)
def incubate(starting_temp, ambient_temp, controller, renderer):
incubator = Incubator(ambient_temp)
last_time = time.time()
while(True):
try:
current_time = time.time()
t = current_time - last_time
renderer.render(incubator, controller)
controller.before_tick(incubator, tictime=t)
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)
def __init__(self):
self.sim = Simulator('127.0.0.1', 25000)
self.sim.connect()
### -----------
### GET HANDLES
# ROBOT
self.robotHandle = self.sim.getHandle("Pioneer_p3dx")
# ACTUATORS
self.motorHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftMotor')
self.motorHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightMotor')
# SONARS
for x in range(0, len(self.sonarHandle)):
self.sonarHandle[x] = self.sim.getHandle('Pioneer_p3dx_ultrasonicSensor'+str(x+1))
# VISION
#self.visionHandle = self.sim.getHandle('camera')
self.visionHandle = 0
# WHEEL
self.wheelHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftWheel')
self.wheelHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightWheel')
### -----------
### INIT STREAMS
self.sim.initObjectPositionStream(self.robotHandle, -1)
self.sim.initObjectOrientationStream(self.robotHandle, -1)
for x in range(0, len(self.sonarHandle)):
self.sim.initProximitySensor(self.sonarHandle[x])
self.sim.initFloatSignal("gyroZ")
#self.sim.initVisionSensorImage(self.visionHandle)
self.sim.initObjectVelocity(self.robotHandle)
self.sim.initJointPositionStream(self.motorHandle[0])
self.sim.initJointPositionStream(self.motorHandle[1])
self.sim.initLaserSensor("measuredDataAtThisTime")
### -----------
### POSTIONS
# SONAR (em relacao ao frame do robo)
for x in range(0, len(self.sonarHandle)):
self.SONAR_POSITIONS[x] = self.sim.getObjectPositionBlock(
self.sonarHandle[x], self.robotHandle)
self.L = np.linalg.norm(self.sim.getObjectPositionBlock(
self.wheelHandle[0], self.wheelHandle[1]))
self.L2 = -self.sim.getObjectPositionBlock(self.wheelHandle[0], self.robotHandle)[0]
# Robo precisa estar no chão para pegar o raio
self.R = self.sim.getObjectPositionBlock(self.wheelHandle[0], -1)[2]
#self.R = 0.09749898314476013
# ROBO
firstRobotPosOrn = self.getPosOrn()
# Odometry
self.odometryPosOrn = {}
self.odometryPosOrn['raw'] = firstRobotPosOrn
self.odometryPosOrn['encoder'] = firstRobotPosOrn
self.odometryPosOrn['compass'] = firstRobotPosOrn
self.odometryEncoder = {}
self.odometryEncoder['encoder'] = [None] * 2
self.odometryEncoder['encoder'][0] = self.sim.getJointPosition(self.motorHandle[0])
self.odometryEncoder['encoder'][1] = self.sim.getJointPosition(self.motorHandle[1])
self.odometryEncoder['compass'] = [None] * 2
self.odometryEncoder['compass'][0] = self.sim.getJointPosition(self.motorHandle[0])
self.odometryEncoder['compass'][1] = self.sim.getJointPosition(self.motorHandle[1])
self.odometryTimeRaw = time.time()
self.odometryTimeCompass = time.time()
self.odometryTimeEncoderCompass = time.time()
self.motorW = [None] * 2
self.motorW[0] = 0
self.motorW[1] = 0
# Controllers
self.wallFollowPID = PIDController(0.4, 1, 0, 2, windowSize=1000)
self.obstacleAvoidFuzzy = OAFController()
self.GoToGoalPID = PIDController(0.1, 4, 0.2, 0.4, windowSize=1000)
self.AnglePID = PIDController(0.4, 1, 0, 2, windowSize=1000)
self.subsumptionStrategy = SubsumptionStrategy()
self.subsumptionStrategy.add(AvoidObstaclesFuzzy(self))
self.subsumptionStrategy.add(FollowLeftWallPID(self))
self.subsumptionStrategy.add(FollowRightWallPID(self))
class Robot:
# Handles
robotHandle = None
motorHandle = [None] * 2
wheelHandle = [None] * 2
sonarHandle = [None] * 16
# Constants
SONAR_POSITIONS = [None] * 16
PI = np.pi
SONAR_ANGLES = np.array([PI/2, 50/180.0*PI, 30/180.0*PI, 10/180.0*PI,
-10/180.0*PI, -30/180.0*PI, -50/180.0*PI, -PI/2, -PI/2,
-130/180.0*PI, -150/180.0*PI, -170/180.0*PI, 170/180.0*PI,
150/180.0*PI, 130/180.0*PI, PI/2])
L = None
L2 = None
R = None
def __init__(self):
self.sim = Simulator('127.0.0.1', 25000)
self.sim.connect()
### -----------
### GET HANDLES
# ROBOT
self.robotHandle = self.sim.getHandle("Pioneer_p3dx")
# ACTUATORS
self.motorHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftMotor')
self.motorHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightMotor')
# SONARS
for x in range(0, len(self.sonarHandle)):
self.sonarHandle[x] = self.sim.getHandle('Pioneer_p3dx_ultrasonicSensor'+str(x+1))
# VISION
#self.visionHandle = self.sim.getHandle('camera')
self.visionHandle = 0
# WHEEL
self.wheelHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftWheel')
self.wheelHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightWheel')
### -----------
### INIT STREAMS
self.sim.initObjectPositionStream(self.robotHandle, -1)
self.sim.initObjectOrientationStream(self.robotHandle, -1)
for x in range(0, len(self.sonarHandle)):
self.sim.initProximitySensor(self.sonarHandle[x])
self.sim.initFloatSignal("gyroZ")
#self.sim.initVisionSensorImage(self.visionHandle)
self.sim.initObjectVelocity(self.robotHandle)
self.sim.initJointPositionStream(self.motorHandle[0])
self.sim.initJointPositionStream(self.motorHandle[1])
self.sim.initLaserSensor("measuredDataAtThisTime")
### -----------
### POSTIONS
# SONAR (em relacao ao frame do robo)
for x in range(0, len(self.sonarHandle)):
self.SONAR_POSITIONS[x] = self.sim.getObjectPositionBlock(
self.sonarHandle[x], self.robotHandle)
self.L = np.linalg.norm(self.sim.getObjectPositionBlock(
self.wheelHandle[0], self.wheelHandle[1]))
self.L2 = -self.sim.getObjectPositionBlock(self.wheelHandle[0], self.robotHandle)[0]
# Robo precisa estar no chão para pegar o raio
self.R = self.sim.getObjectPositionBlock(self.wheelHandle[0], -1)[2]
#self.R = 0.09749898314476013
# ROBO
firstRobotPosOrn = self.getPosOrn()
# Odometry
self.odometryPosOrn = {}
self.odometryPosOrn['raw'] = firstRobotPosOrn
self.odometryPosOrn['encoder'] = firstRobotPosOrn
self.odometryPosOrn['compass'] = firstRobotPosOrn
self.odometryEncoder = {}
self.odometryEncoder['encoder'] = [None] * 2
self.odometryEncoder['encoder'][0] = self.sim.getJointPosition(self.motorHandle[0])
self.odometryEncoder['encoder'][1] = self.sim.getJointPosition(self.motorHandle[1])
self.odometryEncoder['compass'] = [None] * 2
self.odometryEncoder['compass'][0] = self.sim.getJointPosition(self.motorHandle[0])
self.odometryEncoder['compass'][1] = self.sim.getJointPosition(self.motorHandle[1])
self.odometryTimeRaw = time.time()
self.odometryTimeCompass = time.time()
self.odometryTimeEncoderCompass = time.time()
self.motorW = [None] * 2
self.motorW[0] = 0
self.motorW[1] = 0
# Controllers
self.wallFollowPID = PIDController(0.4, 1, 0, 2, windowSize=1000)
self.obstacleAvoidFuzzy = OAFController()
self.GoToGoalPID = PIDController(0.1, 4, 0.2, 0.4, windowSize=1000)
self.AnglePID = PIDController(0.4, 1, 0, 2, windowSize=1000)
self.subsumptionStrategy = SubsumptionStrategy()
self.subsumptionStrategy.add(AvoidObstaclesFuzzy(self))
self.subsumptionStrategy.add(FollowLeftWallPID(self))
self.subsumptionStrategy.add(FollowRightWallPID(self))
### -----------
### DRIVE
def move(self, left, right):
self.computeOdometry()
self.motorW[0] = left
self.motorW[1] = right
self.sim.setJointTargetVelocity(self.motorHandle[0], left)
self.sim.setJointTargetVelocity(self.motorHandle[1], right)
def stop(self):
self.move(0, 0)
def drive(self, vLinear, vAngular):
self.move(self.vLToDrive(vLinear,vAngular), self.vRToDrive(vLinear,vAngular))
def vRToDrive(self, vLinear, vAngular):
return (((2*vLinear)+(self.L*vAngular))/2*self.R)
def vLToDrive(self, vLinear, vAngular):
return (((2*vLinear)-(self.L*vAngular))/2*self.R)
### -----------
### SONAR
def readSonars(self):
pointCloud = []
distances = []
for i in range(0, len(self.sonarHandle)):
state, distance = self.sim.readProximitySensor(self.sonarHandle[i])
if (state == True):
x = self.SONAR_POSITIONS[i][0] + (distance * np.cos(self.SONAR_ANGLES[i]))
y = self.SONAR_POSITIONS[i][1] + (distance * np.sin(self.SONAR_ANGLES[i]))
pointCloud.append((x, y))
distances.append(distance)
else:
pointCloud.append((np.inf, np.inf))
distances.append(np.inf)
return distances, pointCloud
### -----------
### LASER
def readLaser(self):
pointCloud = self.sim.readLaserSensor("measuredDataAtThisTime")
laser_array = np.reshape(pointCloud, (int(len(pointCloud)/3),3))
return laser_array
### -----------
### IMAGE
def getSensorViewImage(self):
resolution, image_array = self.sim.getVisionSensorImage(self.visionHandle)
image = np.array(image_array, dtype=np.uint8)#Create a numpy array with uint8 type
image.resize([resolution[1], resolution[0],3])
return image
### -----------
### ODOMETRY
def _computeMotorAngularDisplacement(self, i, name):
posF = self.sim.getJointPosition(self.motorHandle[i])
posI = self.odometryEncoder[name][i]
self.odometryEncoder[name][i] = posF
diff = posF - posI
if (diff >= np.pi):
diff = 2*np.pi - diff
elif (diff <= -np.pi):
diff = 2*np.pi + diff
return diff
def _computeOdometry(self, posOrn, thetaL, thetaR, deltaTheta):
deltaS = self.R * (thetaR + thetaL) / 2
if deltaTheta is None:
deltaTheta = self.R * (thetaR - thetaL) / self.L
angle = posOrn[2] + (deltaTheta/2)
return posOrn + np.array(
[deltaS * np.cos(angle) - deltaTheta* self.L2 * np.sin(angle),
deltaS * np.sin(angle) + deltaTheta* self.L2 * np.cos(angle),
deltaTheta])
def computeOdometry(self):
deltaTime = time.time() - self.odometryTimeRaw
self.odometryTimeRaw = time.time()
thetaL = self.motorW[0] * deltaTime
thetaR = self.motorW[1] * deltaTime
self.odometryPosOrn['raw'] = self._computeOdometry(
self.odometryPosOrn['raw'], thetaL, thetaR, None)
def computeOdometryEncoder(self):
thetaL = self._computeMotorAngularDisplacement(0, 'encoder')
thetaR = self._computeMotorAngularDisplacement(1, 'encoder')
self.odometryPosOrn['encoder'] = self._computeOdometry(
self.odometryPosOrn['encoder'], thetaL, thetaR, None)
def computeOdometryCompass(self):
deltaTime = time.time() - self.odometryTimeCompass
self.odometryTimeCompass = time.time()
thetaL = self._computeMotorAngularDisplacement(0, 'compass')
thetaR = self._computeMotorAngularDisplacement(1, 'compass')
deltaTheta = self.sim.getFloatSignal("gyroZ") * deltaTime
self.odometryPosOrn['compass'] = self._computeOdometry(
self.odometryPosOrn['compass'], thetaL, thetaR, deltaTheta)
### -----------
### GETTERS
def localToGlobalGT(self, position):
return localToGlobal(self.getPosOrn(), position)
def localToGlobalOdometry(self, position):
return localToGlobal(self.getPosOrn(), position)
def getPosOrn(self):
x, y = self.sim.getObjectPosition(self.robotHandle, -1)[:2]
orn = self.sim.getObjectOrientation(self.robotHandle, -1)[2]
return x, y, orn
def getPosOrnOdometyRaw(self):
return self.odometryPosOrn['raw']
def getPosOrnOdometyCompass(self):
return self.odometryPosOrn['compass']
def getPosOrnOdometyEncoder(self):
return self.odometryPosOrn['encoder']
def getPosOrnOdometyEncoderCompass(self):
return self.odometryPosOrn['encoder_compass']
def getLinearVelocity(self):
return np.linalg.norm(self.sim.getObjectVelocity(self.robotHandle)[1][2])
### -----------------
### BEHAVIORS ACTIONS
def avoidObstaclesFuzzy(self):
distancesL = []
for i in range(1, 4):
state, distance = self.sim.readProximitySensor(self.sonarHandle[i])
if (state == True):
distancesL.append(np.abs(distance*np.cos(self.SONAR_ANGLES[i])))
else:
distancesL.append(1)
distancesR = []
for i in range(4, 7):
state, distance = self.sim.readProximitySensor(self.sonarHandle[i])
if (state == True):
distancesR.append(np.abs(distance*np.cos(self.SONAR_ANGLES[i])))
else:
distancesR.append(1)
vLinear, vAngular = self.obstacleAvoidFuzzy.compute(10*np.min(distancesL), 10*np.min(distancesR))
self.drive(10*vLinear, 20*vAngular)
def followWallPID(self, left):
if (left):
sonarId = 0
else:
sonarId = 7
state, distance = self.sim.readProximitySensor(self.sonarHandle[sonarId])
if (state == False):
distance = 0.8
value = self.wallFollowPID.compute(distance)
value1 = 2 *( 1+value)
value2 = 2 *( 1-value)
if (left):
self.move(value1, value2)
else:
self.move(value2, value1)
return distance
def GoToGoal(self,x_final,y_final):
# Get actual position
x, y, orn = self.getPosOrn()
#print('x: ',x)
#print('y: ',y)
# Compute angle to objective
orn_final = math.atan2((y_final-y),(x_final-x))
# PID control to adjust velocity to final objective
distance = math.sqrt((x_final-x)**2+(y_final-y)**2)
value = self.GoToGoalPID.compute(distance)
# PID control to adjust angular velocity to correct angle
angle = orn - orn_final
angle_correction = self.AnglePID.compute(angle)
# Limit PID distance values
if(value>+8):
value = 8
elif(value<-8):
value = -8
else:
value = value
# Limit PID angle values
if(angle_correction>+2):
angle_correction = 2
elif(angle_correction<-2):
angle_correction = -2
else:
angle_correction = angle_correction
#print('Distance: ',distance)
print('PID: ',value)
# Orientation control
if(distance>0.05):
if (orn > orn_final - 0.2 and orn < orn_final + 0.2):
#self.move(-value,-value)
if (orn < orn_final):
self.move(-angle_correction-value,+angle_correction-value)
else:
self.move(+angle_correction-value,-angle_correction-value)
else:
if (orn < orn_final):
self.move(-angle_correction,+angle_correction)
else:
self.move(+angle_correction,-angle_correction)
else:
self.stop()
# Delete or modify to plot something interesting
sonarId = 0
state, distance = self.sim.readProximitySensor(self.sonarHandle[sonarId])
return distance, value
### -----------
### BEHAVIORS
def stepSubsumptionStrategy(self):
strategy = self.subsumptionStrategy.step()
if (strategy is None):
self.move(2, 2)
return strategy
class Robot:
# Handles
robotHandle = None
motorHandle = [None] * 2
wheelHandle = [None] * 2
sonarHandle = [None] * 16
# Constants
SONAR_POSITIONS = [None] * 16
PI = np.pi
SONAR_ANGLES = np.array([PI/2, 50/180.0*PI, 30/180.0*PI, 10/180.0*PI,
-10/180.0*PI, -30/180.0*PI, -50/180.0*PI, -PI/2, -PI/2,
-130/180.0*PI, -150/180.0*PI, -170/180.0*PI, 170/180.0*PI,
150/180.0*PI, 130/180.0*PI, PI/2])
L = None
L2 = None
R = None
# Map Ground truth
gridMap = None
def __init__(self, extendArea):
self.sim = Simulator('127.0.0.1', 25001)
self.sim.connect()
### -----------
### GET HANDLES
# ROBOT
self.robotHandle = self.sim.getHandle("Pioneer_p3dx")
# ACTUATORS
self.motorHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftMotor')
self.motorHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightMotor')
# SONARS
for x in range(0, len(self.sonarHandle)):
self.sonarHandle[x] = self.sim.getHandle('Pioneer_p3dx_ultrasonicSensor'+str(x+1))
# VISION
#self.visionHandle = self.sim.getHandle('camera')
self.visionHandle = 0
# WHEEL
self.wheelHandle[0] = self.sim.getHandle('Pioneer_p3dx_leftWheel')
self.wheelHandle[1] = self.sim.getHandle('Pioneer_p3dx_rightWheel')
### -----------
### INIT STREAMS
self.sim.initObjectPositionStream(self.robotHandle, -1)
self.sim.initObjectOrientationStream(self.robotHandle, -1)
for x in range(0, len(self.sonarHandle)):
self.sim.initProximitySensor(self.sonarHandle[x])
self.sim.initFloatSignal("gyroZ")
#self.sim.initVisionSensorImage(self.visionHandle)
self.sim.initObjectVelocity(self.robotHandle)
self.sim.initJointPositionStream(self.motorHandle[0])
self.sim.initJointPositionStream(self.motorHandle[1])
self.sim.initLaserSensor("measuredDataAtThisTime")
### -----------
### POSTIONS
# SONAR (em relacao ao frame do robo)
for x in range(0, len(self.sonarHandle)):
self.SONAR_POSITIONS[x] = self.sim.getObjectPositionBlock(
self.sonarHandle[x], self.robotHandle)
self.L = np.linalg.norm(self.sim.getObjectPositionBlock(
self.wheelHandle[0], self.wheelHandle[1]))
self.L2 = -self.sim.getObjectPositionBlock(self.wheelHandle[0], self.robotHandle)[0]
# Robo precisa estar no chão para pegar o raio
self.R = self.sim.getObjectPositionBlock(self.wheelHandle[0], -1)[2]
#self.R = 0.09749898314476013
# ROBO
firstRobotPosOrn = self.getPosOrn()
# Odometry
self.odometryPosOrn = {}
self.odometryPosOrn['raw'] = firstRobotPosOrn
self.odometryPosOrn['encoder'] = firstRobotPosOrn
self.odometryPosOrn['compass'] = firstRobotPosOrn
self.odometryEncoder = {}
self.odometryEncoder['encoder'] = [None] * 2
self.odometryEncoder['encoder'][0] = self.sim.getJointPosition(self.motorHandle[0])
self.odometryEncoder['encoder'][1] = self.sim.getJointPosition(self.motorHandle[1])
self.odometryEncoder['compass'] = [None] * 2
self.odometryEncoder['compass'][0] = self.sim.getJointPosition(self.motorHandle[0])
self.odometryEncoder['compass'][1] = self.sim.getJointPosition(self.motorHandle[1])
self.odometryTimeRaw = time.time()
self.odometryTimeCompass = time.time()
self.odometryTimeEncoderCompass = time.time()
self.motorW = [None] * 2
self.motorW[0] = 0
self.motorW[1] = 0
# Map grid
x, y, _ = self.getPosOrn()
mapImage = self.sim.getVisionSensorImageBlock(self.sim.getHandle('mapSensor'))
self.gridMap = GridMap(mapImage, extendArea)
x, y, _ = self.getPosOrn()
x, y = self.gridMap.convertToMapUnit(x, y)
self.gridMap.removeCluster(x, y)
self.gridMap.show()
# Controllers
self.goToGoalPID = PIDController(0, 50, 0, 15, windowSize=1000)
self.anglePID = PIDController(0, 10, 0, 10, windowSize=1000)
### -----------
### DRIVE
def move(self, left, right):
self.computeOdometry()
self.motorW[0] = left
self.motorW[1] = right
self.sim.setJointTargetVelocity(self.motorHandle[0], left)
self.sim.setJointTargetVelocity(self.motorHandle[1], right)
def stop(self):
self.move(0, 0)
def drive(self, vLinear, vAngular):
self.move(self.vLToDrive(vLinear,vAngular), self.vRToDrive(vLinear,vAngular))
def vRToDrive(self, vLinear, vAngular):
return (((2*vLinear)+(self.L*vAngular))/2*self.R)
def vLToDrive(self, vLinear, vAngular):
return (((2*vLinear)-(self.L*vAngular))/2*self.R)
### -----------
### SONAR
def readSonars(self):
pointCloud = []
distances = []
for i in range(0, len(self.sonarHandle)):
state, distance = self.sim.readProximitySensor(self.sonarHandle[i])
if (state == True):
x = self.SONAR_POSITIONS[i][0] + (distance * np.cos(self.SONAR_ANGLES[i]))
y = self.SONAR_POSITIONS[i][1] + (distance * np.sin(self.SONAR_ANGLES[i]))
pointCloud.append((x, y))
distances.append(distance)
else:
pointCloud.append((np.inf, np.inf))
distances.append(np.inf)
return distances, pointCloud
### -----------
### LASER
def readLaser(self):
pointCloud = self.sim.readLaserSensor("measuredDataAtThisTime")
laser_array = np.reshape(pointCloud, (int(len(pointCloud)/3),3))
return laser_array
### -----------
### IMAGE
def getSensorViewImage(self):
resolution, image_array = self.sim.getVisionSensorImage(self.visionHandle)
image = np.array(image_array, dtype=np.uint8)#Create a numpy array with uint8 type
image.resize([resolution[1], resolution[0],3])
return image
### -----------
### ODOMETRY
def _computeMotorAngularDisplacement(self, i, name):
posF = self.sim.getJointPosition(self.motorHandle[i])
posI = self.odometryEncoder[name][i]
self.odometryEncoder[name][i] = posF
diff = posF - posI
if (diff >= np.pi):
diff = 2*np.pi - diff
elif (diff <= -np.pi):
diff = 2*np.pi + diff
return diff
def _computeOdometry(self, posOrn, thetaL, thetaR, deltaTheta):
deltaS = self.R * (thetaR + thetaL) / 2
if deltaTheta is None:
deltaTheta = self.R * (thetaR - thetaL) / self.L
angle = posOrn[2] + (deltaTheta/2)
return posOrn + np.array(
[deltaS * np.cos(angle) - deltaTheta* self.L2 * np.sin(angle),
deltaS * np.sin(angle) + deltaTheta* self.L2 * np.cos(angle),
deltaTheta])
def computeOdometry(self):
deltaTime = time.time() - self.odometryTimeRaw
self.odometryTimeRaw = time.time()
thetaL = self.motorW[0] * deltaTime
thetaR = self.motorW[1] * deltaTime
self.odometryPosOrn['raw'] = self._computeOdometry(
self.odometryPosOrn['raw'], thetaL, thetaR, None)
def computeOdometryEncoder(self):
thetaL = self._computeMotorAngularDisplacement(0, 'encoder')
thetaR = self._computeMotorAngularDisplacement(1, 'encoder')
self.odometryPosOrn['encoder'] = self._computeOdometry(
self.odometryPosOrn['encoder'], thetaL, thetaR, None)
def computeOdometryCompass(self):
deltaTime = time.time() - self.odometryTimeCompass
self.odometryTimeCompass = time.time()
thetaL = self._computeMotorAngularDisplacement(0, 'compass')
thetaR = self._computeMotorAngularDisplacement(1, 'compass')
deltaTheta = self.sim.getFloatSignal("gyroZ") * deltaTime
self.odometryPosOrn['compass'] = self._computeOdometry(
self.odometryPosOrn['compass'], thetaL, thetaR, deltaTheta)
### -----------
### GETTERS
def localToGlobalGT(self, position):
return localToGlobal(self.getPosOrn(), position)
def localToGlobalOdometry(self, position):
return localToGlobal(self.getPosOrn(), position)
def getPosOrn(self):
x, y = self.sim.getObjectPosition(self.robotHandle, -1)[:2]
orn = self.sim.getObjectOrientation(self.robotHandle, -1)[2]
return x, y, orn
def getPosOrnOdometyRaw(self):
return self.odometryPosOrn['raw']
def getPosOrnOdometyCompass(self):
return self.odometryPosOrn['compass']
def getPosOrnOdometyEncoder(self):
return self.odometryPosOrn['encoder']
def getPosOrnOdometyEncoderCompass(self):
return self.odometryPosOrn['encoder_compass']
def getLinearVelocity(self):
return np.linalg.norm(self.sim.getObjectVelocity(self.robotHandle)[1][2])
### -----------------
### Path Planning
def potentialPlanningOffline(self, gx, gy):
self.gridMap.calcPotentialMap(gx, gy)
x, y, _ = self.getPosOrn()
x, y = self.gridMap.convertToMapUnit(x, y)
self.pathPlanning = self.gridMap.calcPotentialPath(x, y)
return [self.gridMap.convertToReal(*pos) for pos in self.pathPlanning]
def potentialAStarPlanningOffline(self, gx, gy):
self.gridMap.calcPotentialMap(gx, gy)
x, y, _ = self.getPosOrn()
x, y = self.gridMap.convertToMapUnit(x, y)
self.pathPlanning = self.gridMap.calcPotentialAStarPath(x, y, gx, gy)
return [self.gridMap.convertToReal(*pos) for pos in self.pathPlanning]
def doPlanning(self):
if (len(self.pathPlanning)):
nextStep = self.pathPlanning[0]
x, y = self.gridMap.convertToReal(*nextStep)
distance = self.goToGoal(x, y)
if (distance<0.2):
self.pathPlanning = self.pathPlanning[1:]
return True
else:
self.stop()
return False
### -----------------
### BEHAVIORS ACTIONS
def FollowPath(self,x_final,y_final):
# Get actual position
x, y, orn = self.getPosOrn()
#print('x: ',x)
#print('y: ',y)
# Compute angle to objective
orn_final = math.atan2((y_final-y),(x_final-x))
# PID control to adjust velocity to final objective
distance = math.sqrt((x_final-x)**2+(y_final-y)**2)
value = self.GoToGoalPID.compute(distance)
# PID control to adjust angular velocity to correct angle
angle = orn - orn_final
angle_correction = self.AnglePID.compute(angle)
# Limit PID distance values
if(value>+2):
value = 2
elif(value<-2):
value = -2
else:
value = value
# Limit PID angle values
if(angle_correction>+2):
angle_correction = 2
elif(angle_correction<-2):
angle_correction = -2
else:
angle_correction = angle_correction
# Orientation control
if(distance>0.5): #0.05
if (orn > orn_final - 0.5 and orn < orn_final + 0.5): #0.2
#self.move(-value,-value)
if (orn < orn_final):
self.move(-angle_correction-value,+angle_correction-value)
else:
self.move(+angle_correction-value,-angle_correction-value)
else:
if (orn < orn_final):
self.move(-angle_correction,+angle_correction)
#print("-+")
else:
self.move(+angle_correction,-angle_correction)
#print("+-")
Arrived = 'false'
else:
#self.stop()
Arrived = 'true'
return Arrived
def goToGoal(self,x_final,y_final):
# Get actual position
x, y, orn = self.getPosOrn()
# Compute angle to objective
orn_final = math.atan2((y_final-y),(x_final-x))
# PID control to adjust velocity to final objective
distance = math.sqrt((x_final-x)**2+(y_final-y)**2)
linearVelocity = -self.goToGoalPID.compute(distance)
# PID control to adjust angular velocity to correct angle
angle = orn - orn_final
if angle > np.pi:
angle = angle - 2 * np.pi
elif angle < -np.pi:
angle = angle + 2 * np.pi
angleVelocity = self.anglePID.compute(angle)
# Limit PID
if (linearVelocity > 10):
linearVelocity = 10
# Linear velocity should be smaller if it's not in correct direction
linearVelocity = (np.cos(angle)**15) * linearVelocity
if (linearVelocity < 0):
linearVelocity = 0
# Limit PID
if (angleVelocity > 10):
angleVelocity = 10
elif (angleVelocity < -10):
angleVelocity = -10
self.drive(5 * linearVelocity, 10 * angleVelocity)
return distance
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()