Beispiel #1
0
    def main(self):
        import argparse
        parser = argparse.ArgumentParser(description='DWA Demo')
        parser.add_argument('--save', dest='save', action='store_true')
        parser.set_defaults(save=False)
        args = parser.parse_args()
        if args.save:
            import imageio
            writer = imageio.get_writer('./dwa.gif', mode='I', duration=0.05)
        while True:
            prev_time = time.time()
            self.map = np.zeros((600, 600, 3), dtype=np.uint8)
            for point in self.draw_points:
                cv2.circle(self.map, tuple(point), 4, (255, 255, 255), -1)
            if self.goal is not None:
                cv2.circle(self.map, (int(self.goal[0]*10), int(self.goal[1]*10)),
                        4, (0, 255, 0), -1)
                if len(self.point_cloud):
                    # Planning
                    self.vel = dwa.planning(self.pose, self.vel, self.goal,
                            np.array(self.point_cloud, np.float32), self.config)
                    # Simulate motion
                    self.pose = dwa.motion(self.pose, self.vel, self.config.dt)

            pose = np.ndarray((3,))
            pose[0:2] = np.array(self.pose[0:2]) * 10
            pose[2] = self.pose[2]

            base = np.array(self.base) * 10
            base[0:2] += pose[0:2]
            base[2:4] += pose[0:2]

            # Not the correct rectangle but good enough for the demo
            width = base[2] - base[0]
            height = base[3] - base[1]
            rect = ((pose[0], pose[1]), (width, height), np.degrees(pose[2]))
            box = cv2.boxPoints(rect)
            box = np.int0(box)
            cv2.drawContours(self.map,[box],0,(0,0,255),-1)

            fps = int(1.0 / (time.time() - prev_time))
            cv2.putText(self.map, f'FPS: {fps}', (20, 30),
                    cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)

            cv2.putText(self.map, f'Point Cloud Size: {len(self.point_cloud)}',
                    (20, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)

            if args.save:
                writer.append_data(self.map)
            cv2.imshow('cvwindow', self.map)
            key = cv2.waitKey(1)
            if key == 27:
                break
            elif key == ord('r'):
                self.point_cloud = []
                self.draw_points = []
        if args.save:
            writer.close()
Beispiel #2
0
        def generate():
            while True:
                frame = np.ones((600, 600, 3), dtype=np.uint8)
                time.sleep(0.01)
                for point in self.draw_points:
                    cv2.circle(frame, tuple(point), 4, (255, 255, 255), -1)
                if self.goal is not None:
                    cv2.circle(
                        frame,
                        (int(self.goal[0] * 10), int(self.goal[1] * 10)), 4,
                        (0, 255, 0), -1)
                    if len(self.point_cloud):
                        # Planning
                        self.vel = dwa.planning(
                            self.pose, self.vel, self.goal,
                            np.array(self.point_cloud, np.float32),
                            self.config)
                        # Simulate motion
                        self.pose = dwa.motion(self.pose, self.vel,
                                               self.config.dt)

                pose = np.ndarray((3, ))
                pose[0:2] = np.array(self.pose[0:2]) * 10
                pose[2] = self.pose[2]

                base = np.array(self.base) * 10
                base[0:2] += pose[0:2]
                base[2:4] += pose[0:2]

                # Not the correct rectangle but good enough for the demo
                width = base[2] - base[0]
                height = base[3] - base[1]
                rect = ((pose[0], pose[1]), (width, height),
                        np.degrees(pose[2]))
                box = cv2.boxPoints(rect)
                box = np.int0(box)
                cv2.drawContours(frame, [box], 0, (0, 0, 255), -1)

                enc_frame = cv2.imencode('.jpg', frame)[1].tobytes()
                yield (b'\r\n' + b'--frame\r\n'
                       b'Content-Type: image/jpeg\r\n\r\n' + enc_frame +
                       b'\r\n')
    def evaluate(self, point_cloud):

        # self.point_cloud = point_cloud

        if self.goal:
            self.vel = dwa.planning(self.pose, self.vel, self.goal,
                                    np.array(self.point_cloud, np.float32),
                                    self.config)
            self.pose = dwa.motion(self.pose, self.vel, self.config.dt)

            self.count += 1
            print(self.count, 'virtual robot pose:', self.pose)

            distance = math.hypot(self.goal[0] - self.pose[0],
                                  self.goal[1] - self.pose[1])

            if distance < 3:
                print('virtual target got')
                self.goal = None

        return self.pose
Beispiel #4
0
    def main(self):
        image = cv2.imread('sample_0.png')
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        h,w,d = image.shape
        top = int(h*0.6)
        gray[0:top, 0:w] = 0
#        roi = gray[top:h, 0:w]
        ret, thresh = cv2.threshold(gray, 127, 255, 0)
        edges = cv2.Canny(thresh, 10, 150, apertureSize=3)
        contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
        lines = cv2.polylines(image, contours, False, (255, 0, 0), 1)
        
        print("contours len: ", len(contours), contours)
        for contour in contours:
            for c in contour:
                for pt in c:
                    #print("point: ", contour[0][0])
                    self.draw_points.append(pt)
                    self.point_cloud.append(pt)
                    #cv2.circle(image, tuple(contour[0][0]), 5, (255, 255, 255), -1)
            
        #print("drawpoints: ", self.draw_points)
        print("point cloud len:", len(self.point_cloud))
        self.pose = (w/20, 70.0, 0)
        while True:
            prev_time = time.time()
            self.map = np.zeros((720, 1280, 3), dtype=np.uint8)
            for point in self.draw_points:
                cv2.circle(self.map, tuple(point), 5, (255, 255, 255), -1)
                
            if self.goal is not None:
                cv2.circle(self.map, (int(self.goal[0]*10), int(self.goal[1]*10)),
                        4, (0, 255, 0), -1)
                if len(self.point_cloud):
                    # Planning
                    self.vel = dwa.planning(self.pose, self.vel, self.goal,
                            np.array(self.point_cloud, np.float32), self.config)
                    # Simulate motion
                    self.pose = dwa.motion(self.pose, self.vel, self.config.dt)
            
            pose = np.ndarray((3,))
            pose[0:2] = np.array(self.pose[0:2]) * 10
            pose[2] = self.pose[2]
    
            base = np.array(self.base) * 10
            base[0:2] += pose[0:2]
            base[2:4] += pose[0:2]
            
            # Not the correct rectangle but good enough for the demo
            width = base[2] - base[0]
            height = base[3] - base[1]
            rect = ((pose[0], pose[1]), (width, height), np.degrees(pose[2]))
            box = cv2.boxPoints(rect)
            box = np.int0(box)
            cv2.drawContours(self.map,[box],0,(0,0,255),-1)
            
            fps = int(1.0 / (time.time() - prev_time))
            cv2.putText(self.map, f'FPS: {fps}', (20, 30),
                    cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
            
            cv2.putText(self.map, f'Point Cloud Size: {len(self.point_cloud)}',
                        (20, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
        
            cv2.imshow('frame', image)
            cv2.imshow('demo', self.map)
            k = cv2.waitKey(1)
            if k%256 == 27:
                break
        cv2.destroyAllWindows
Beispiel #5
0
    def main(self):
        cap=cv2.VideoCapture('J_shape_wideangle.avi')
        while(cap.isOpened()):
            self.point_cloud = []
            self.draw_points = []
            prev_time = time.time()
            ret, frame=cap.read()
#            if ret != True:
#                break
            self.ctr += 1
            gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
            h,w,d = frame.shape
            top = int(h*0.6)
            gray[0:top, 0:w] = 0 
            ret, thresh = cv2.threshold(gray, 127, 255, 0)
            edges = cv2.Canny(thresh, 10, 150, apertureSize=3)
            contours, hierarchy = cv2.findContours(edges, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
            cv2.polylines(frame, contours, False, (255, 0, 0), 2)
            if self.ctr%10 == 0: 
                for contour in contours:
                    for c in contour:
                        for pt in c:
                    #print("point: ", contour[0][0])
                            self.draw_points.append(pt)
                            self.point_cloud.append(pt)
                #print("drawpoints: ", self.draw_points)
                    
            self.map = np.zeros((720, 1280, 3), dtype=np.uint8)
            for point in self.draw_points:
                cv2.circle(frame, tuple(point), 5, (255, 255, 255), -1)
                
            if self.goal is not None:
                cv2.circle(frame, (int(self.goal[0]*10), int(self.goal[1]*10)),
                        4, (0, 255, 0), -1)
                if len(self.point_cloud):
                    # Planning
                    self.vel = dwa.planning(self.pose, self.vel, self.goal,
                            np.array(self.point_cloud, np.float32), self.config)
                    print(self.vel)
                    # Simulate motion
                    self.pose = dwa.motion(self.pose, self.vel, self.config.dt)
                    
            pose = np.ndarray((3,))
            pose[0:2] = np.array(self.pose[0:2]) * 10
            pose[2] = self.pose[2]

            base = np.array(self.base) * 10
            base[0:2] += pose[0:2]
            base[2:4] += pose[0:2]
            
            # Not the correct rectangle but good enough for the demo
            width = base[2] - base[0]
            height = base[3] - base[1]
            rect = ((pose[0], pose[1]), (width, height), np.degrees(pose[2]))
            box = cv2.boxPoints(rect)
            box = np.int0(box)
            cv2.drawContours(frame,[box],0,(0,0,255),-1)
            
            fps = int(1.0 / (time.time() - prev_time))
            cv2.putText(frame, f'FPS: {fps}', (20, 30),
                    cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)

            cv2.putText(frame, f'Point Cloud Size: {len(self.point_cloud)}',
                    (20, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2)
            
#            cv2.imshow('demo', self.map)
            cv2.imshow('frame', frame)
            k = cv2.waitKey(1)
            if k%256 == 27:
                print("[INFO] ESC hit, closing...")
                break
            
            elif k == ord('r'):
                self.point_cloud = []
                self.draw_points = []