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()