def go(self): vehicle_name = dtu.get_current_robot_name() # noinspection PyUnresolvedReferences output = self.options.output if output is None: output = "out/pipeline" # + dtu.get_md5(self.options.image)[:6] self.info(f"No --output given, using {output}") # noinspection PyUnresolvedReferences opt_image = self.options.image if opt_image is not None: image_filename = opt_image if image_filename.startswith("http"): image_filename = dtu.get_file_from_url(image_filename) bgr = dtu.bgr_from_jpg_fn(image_filename) else: self.info("Validating using the ROS image stream...") import rospy from sensor_msgs.msg import CompressedImage topic_name = os.path.join("/", vehicle_name, "camera_node/image/compressed") self.info("Let's wait for an image. Say cheese!") # Dummy to get ROS message rospy.init_node("single_image") try: img_msg = cast( CompressedImage, rospy.wait_for_message(topic_name, CompressedImage, timeout=10)) self.info("Image captured") except rospy.ROSException as e: self.info( f"\n\n\nDidn't get any message: {e}\n MAKE SURE YOU USE DT SHELL COMMANDS OF VERSION " "4.1.9 OR HIGHER!\n\n\n") raise bgr = dtu.bgr_from_rgb(dru.rgb_from_ros(img_msg)) self.info(f"Picture taken: {str(bgr.shape)} ") dtu.DuckietownConstants.show_timeit_benchmarks = True res, _stats = run_pipeline(bgr) self.info("Resizing images..") res = dtu.resize_small_images(res) self.info("Writing images..") dtu.write_bgr_images_as_jpgs(res, output)
def go(self): robot_name = dtu.get_current_robot_name() # noinspection PyUnresolvedReferences output = self.options.output # noinspection PyUnresolvedReferences the_input = self.options.input if output is None: output = "out/calibrate-extrinsics" # + dtu.get_md5(self.options.image)[:6] self.info(f"No --output given, using {output}") if the_input is None: self.info( ("{}\nCalibrating using the ROS image stream...\n".format("*" * 20))) import rospy from sensor_msgs.msg import CompressedImage topic_name = os.path.join("/", robot_name, "camera_node/image/compressed") logger.info(f"Topic to listen to is: {topic_name}") self.info("Let's wait for an image. Say cheese!") # Dummy for getting a ROS message rospy.init_node("calibrate_extrinsics") try: img_msg = rospy.wait_for_message(topic_name, CompressedImage, timeout=10) self.info("Image captured") except rospy.ROSException as e: print(( "\n\n\n" f"Didn't get any message: {e}\n " "MAKE SURE YOU USE DT SHELL COMMANDS OF VERSION 4.1.9 OR HIGHER." )) raise img_msg = cast(CompressedImage, img_msg) bgr = dtu.bgr_from_rgb(dru.rgb_from_ros(img_msg)) self.info(f"Picture taken: {str(bgr.shape)} ") else: self.info(f"Loading input image {the_input}") bgr = dtu.bgr_from_jpg_fn(the_input) if bgr.shape[1] != 640: interpolation = cv2.INTER_CUBIC bgr = dtu.d8_image_resize_fit(bgr, 640, interpolation) self.info(f"Resized to: {str(bgr.shape)} ") # Disable the old calibration file self.info("Disableing old homography") disable_old_homography(robot_name) self.info("Obtaining camera info") try: camera_info = get_camera_info_for_robot(robot_name) except Exception as e: msg = "Error on obtaining camera info." raise Exception(msg) from e self.info("Get default homography") try: homography_dummy = get_homography_default() except Exception as e: msg = "Error on getting homography." raise Exception(msg) from e self.info("Rectify image") try: rect = Rectify(camera_info) except Exception as e: msg = "Error rectifying image." raise Exception(msg) from e self.info("Calculate GPG") try: gpg = GroundProjectionGeometry(camera_info.width, camera_info.height, homography_dummy.reshape((3, 3))) except Exception as e: msg = "Error calculating GroundProjectionGeometry." raise Exception(msg) from e self.info("Ordered Dict") res = {} try: bgr_rectified = rect.rectify(bgr, interpolation=cv2.INTER_CUBIC) res["bgr"] = bgr res["bgr_rectified"] = bgr_rectified _new_matrix, res["rectified_full_ratio_auto"] = rect.rectify_full( bgr, ratio=1.65) (result_gpg, status) = gpg.estimate_homography(bgr_rectified) if status is not None: raise Exception(status) save_homography(result_gpg.H, robot_name) msg = """ To check that this worked well, place the robot on the road, and run: rosrun complete_image_pipeline single_image Look at the produced jpgs. """ self.info(msg) except Exception as E: self.error(E) finally: dtu.write_bgr_images_as_jpgs(res, output)
def process_log( self, bag_in: dbu.BagReadProxy, prefix_in: str, bag_out, prefix_out: str, utils: ProcessorUtilsInterface, ): log_name = utils.get_log().log_name vehicle_name = dbu.which_robot(bag_in) logger.info(f"Vehicle name: {vehicle_name}") topic = dbu.get_image_topic(bag_in) rcg = get_robot_camera_geometry_from_log(bag_in) bgcolor = dtu.ColorConstants.BGR_DUCKIETOWN_YELLOW sequence = bag_in.read_messages_plus(topics=[topic]) for _i, mp in enumerate(sequence): bgr = dtu.bgr_from_rgb(dru.rgb_from_ros(mp.msg)) res, stats = run_pipeline( bgr, gpg=rcg.gpg, rectifier=rcg.rectifier, line_detector_name=self.line_detector, image_prep_name=self.image_prep, lane_filter_name=self.lane_filter, anti_instagram_name=self.anti_instagram, all_details=self.all_details, ) rect = (480, 640) if not self.all_details else (240, 320) res = dtu.resize_images_to_fit_in_rect(res, rect, bgcolor=bgcolor) msg = ( f"abs: {mp.time_absolute} window: {mp.time_window} from log: " f"{mp.time_from_physical_log_start}") dtu.logger.info(msg) headers = [ f"Robot: {vehicle_name} log: {log_name} time: {mp.time_from_physical_log_start:.2f} s", f"Algorithms | color correction: {self.anti_instagram} | preparation: {self.image_prep} | " f"detector: {self.line_detector} | filter: {self.lane_filter}", ] res = dtu.write_bgr_images_as_jpgs(res, dirname=None, bgcolor=bgcolor) cv_image = res["all"] for head in reversed(headers): max_height = 35 cv_image = dtu.add_header_to_bgr(cv_image, head, max_height=max_height) otopic = "all" omsg = dru.d8_compressed_image_from_cv_image( cv_image, same_timestamp_as=mp.msg) t = rospy.Time.from_sec(mp.time_absolute) dtu.logger.info(f"written {otopic!r} at t = {t.to_sec()}") bag_out.write(prefix_out + "/" + otopic, omsg, t=t) for name, value in list(stats.items()): utils.write_stat(prefix_out + "/" + name, value, t=t)