def __init__(self): rospy.init_node("ground_projection_node") # distorted_input is set to False only in simulation mode distorted_input = get_param("~distorted_input", "False") camera_info = get_camera_info() extrinsics_file_name = rospy.get_namespace().strip("/") extrinsics_file_path = get_config_file_path("extrinsics", extrinsics_file_name) with open(extrinsics_file_path, "r") as file_handle: data = yaml.safe_load(file_handle) h_matrix = np.array(data["H"]["data"]).reshape(3, 3) # create instance of the GroundProjector self.projector = ground_projection.GroundProjector( camera_info, h_matrix, distorted_input) self.pub_track_position = rospy.Publisher("~track_position_output", Point32, queue_size=1) self.sub_track_position = rospy.Subscriber( "~track_position_input", Point32, self.track_position_callback, queue_size=1) rospy.loginfo("[{}] Started.".format(rospy.get_name()))
def __init__(self): self._params = {} self.publishers = {} self.services = {} config_file_name = get_param("~config_file_name", "default") config_file_path = get_config_file_path("race_control", config_file_name) if config_file_path is None: rospy.signal_shutdown("Could not find any config file... " "Shutting down!") # read the controller configuration from config file self.setup_params(config_file_path) self.controller = Controller(self._params["p_gain"], self._params["weight_distance"], self._params["weight_angle"]) # register all publishers self.init_publishers() # create all services self.init_services() self.sub_pose = rospy.Subscriber("~pose_input", LanePose, self.pose_cb, queue_size=1)
def __init__(self): self._params = {} self.publishers = {} self.services = {} # import parameters from config yaml files config_file_name = get_param("~config_file_name", "default") config_file_path = get_config_file_path("leader_detection", config_file_name) # shut down node if no config file could be found if config_file_path is None: rospy.signal_shutdown("Could not find any config file... " "Shutting down!") # read the cofig parameters form .yaml file self.setup_params(config_file_path) # create leader detection object self.leader_detector = leader_detection.LeaderGetter(self._params) # subscribe to image and publish position of leader self.sub_img = rospy.Subscriber("~input_image/raw", Image, self.rcv_img_cb) # register all publishers self.init_publishers() # create all services self.init_services() # ros Image has to be bridged to openCV self.bridge = CvBridge()
def __init__(self, freq=200, address=0x40): """ Args: freq (int): The PWM frequency of the servo. The servo gain depends on the frequency, so it needs to be redetermined, if the frequency changes. address (int): I2C address of the PCA9685 PWM module. """ # find config file config_file_name = picar_common.get_param( "~config_file_name", "default") config_file_path = picar_common.get_config_file_path( "motor", config_file_name) if config_file_path is None: rospy.signal_shutdown("Could not find motor config file. " "Shutting down!") return # init motor picar self.params = MotorParams(config_file_path) # create a timeout variable which is set to false, if a message was received. # Once per second it is set to true. So the car won't get crazy of it loses contact # to some motor command publisher self.msg_timeout = False self.timer = rospy.Timer(rospy.Duration(1), self._msg_timeout_cb) # init pwm self.pwm = Adafruit_PCA9685.PCA9685(address=address) self.pwm.set_pwm_freq(freq) self.pwm.set_pwm(self.params.in_1_channel, 0, 4095) self.pwm.set_pwm(self.params.in_2_channel, 0, 4095) self.pwm.set_pwm(self.params.servo_channel, 0, 1228) self.pwm.set_pwm(self.params.motor_forward_channel, 0, 0) self.pwm.set_pwm(self.params.motor_backward_channel, 0, 0) self.services = {} self.init_services() # handle shutdowns properly -> release motor rospy.on_shutdown(self.on_shutdown) # subscribe to motor commands self.sub_car_cmd = rospy.Subscriber("~car_cmd", CarCmd, self.car_cmd_callback, queue_size=1)
def __init__(self): config_file_name = get_param("~config_file_name", "default") config_file_path = get_config_file_path("color_normalization", config_file_name) if config_file_path is None: rospy.signal_shutdown("Could not find any config file... " "Shutting down!") self.bridge = CvBridge() self.normalizer = color_normalization.Normalizer(config_file_path) self.img_sub = rospy.Subscriber("~input_image/compressed", CompressedImage, self.rcv_img_cb, queue_size=1) self.img_pub = rospy.Publisher("~output_image/raw", Image, queue_size=1)
def __init__(self): # TODO init with None and find way to deal with None declaration self.pos_blue_ball = None self.pos_green_ball = None self._params = {} self.services = {} name = get_param("~extrinsics_file_name", "default") path = get_config_file_path("extrinsics", name) camera_info = get_camera_info() if path is None: rospy.logfatal("No extrinsics found!") rospy.signal_shutdown("No extrinsics found!") with open(path, "r") as f: data = yaml.safe_load(f) H = np.array(data["H"]["data"]) H = np.resize(H, (3, 3)) rospy.loginfo("[{}] Waiting for camera_info message..." "".format(rospy.get_name())) camera_info = rospy.wait_for_message("camera_node/camera_info", CameraInfo) rospy.loginfo("[{}] Received camera_info message." "".format(rospy.get_name())) if camera_info.D[0] == 0.0: self.projector = WorldProjector(camera_info, H, False) else: self.projector = WorldProjector(camera_info, H, True) # register all publishers self.publishers = {} self.init_publishers() # register all publishers self.init_subscribers()
def __init__(self): pass self._params = {} self.publishers = {} self.services = {} # initialize timestamp self.timestamp = rospy.get_rostime() self.last_time = 0.0 config_file_name = get_param("~config_file_name", "default") config_file_path = get_config_file_path("leader_control", config_file_name) if config_file_path is None: rospy.signal_shutdown("Could not find any config file... " "Shutting down!") # read the controller configuration from config file self.setup_params(config_file_path) self.controller = Controller(self._params["k_pvel"], self._params["k_psteer"], self._params["k_dvel"], self._params["k_dsteer"]) # register all publishers self.init_publishers() # create all services self.init_services() self.last_values = ControllerValues(0.0,0.0) #TODO set subscriber self.sub_pose = rospy.Subscriber("~leader_relative_pos_input", Point32, self.pose_cb, queue_size=1)
def __init__(self): config_file_name = get_param("~config_file_name", "default") config_file_path = get_config_file_path("line_detection", config_file_name) if config_file_path is None: rospy.signal_shutdown("Could not find any config file... " "Shutting down!") self.line_detector = line_detection.LineGetter(config_file_path) self.sub_img = rospy.Subscriber("~input_image/raw", Image, self.rcv_img_cb, queue_size=1) self.pub_position = rospy.Publisher("~track_position", Point32, queue_size=1) self.bridge = CvBridge()
def main(): rospy.init_node('camera_node', anonymous=False) rate = rospy.Rate(2) config_file_name = get_param("~config_file_name", "default") config_file_path = get_config_file_path("camera", config_file_name) if config_file_path is None: rospy.signal_shutdown("Could not find camera config file. " "Shutting down!") resolution, fps, fisheye = read_config_from_file(config_file_path) intrinsics_file_name = get_param("~intrinsics_file_name", "default") intrinsics_file_path = get_config_file_path("intrinsics", intrinsics_file_name) publish_intrinsics_info = True if intrinsics_file_path is None: camera_info_intrinsics = None publish_intrinsics_info = False else: camera_info_intrinsics = camera_info_from_intrinsics_file( intrinsics_file_path) camera_info = camera_info_scaled(camera_info_intrinsics, resolution) if fisheye: camera_info.distortion_model = "fisheye" output = FrameSplitter(publish_intrinsics_info, camera_info, camera_info_intrinsics) rospy.set_param("~width", resolution[0]) rospy.set_param("~height", resolution[1]) rospy.set_param("~fps", fps) rospy.set_param("~fisheye", fisheye) with PiCamera(resolution=resolution, framerate=fps, sensor_mode=4) as camera: cam_config = CameraParams(camera) srv = Server(cam_paramsConfig, cam_config.reconfigure_cb) if fisheye: w = 0.7 h = 0.7 x = 0.15 y = 0.15 camera.zoom = (x, y, w, h) camera.start_recording(output, format='mjpeg', quality=100) rospy.loginfo("drc_strengt {}".format(camera.drc_strength)) rospy.loginfo("still_stats {}".format(camera.still_stats)) rospy.loginfo("[{}] Camera started".format(rospy.get_name())) while not rospy.is_shutdown(): try: camera.wait_recording(0) rate.sleep() except KeyboardInterrupt: rospy.loginfo("Shutting down by KeyboardInterrupt") break camera.stop_recording() rospy.loginfo("Shutting down camera node")
def __init__(self): # counts how many messages were send self.msg_counter = 0 # dictionaries for easy access self._params = {} self.services = {} self.publishers = {} self.subscribers = {} # import parameters from config yaml files config_file_name = get_param("~config_file_name", "default") config_file_path = get_config_file_path("curved_line_detection", config_file_name) # shut down node if no config file could be found if config_file_path is None: rospy.signal_shutdown("Could not find any config file... " "Shutting down!") # read the config parameters form .yaml file self.setup_params(config_file_path) # get Camera data name = get_param("~extrinsics_file_name", "default") path = get_config_file_path("extrinsics", name) camera_info = get_camera_info() if path is None: rospy.logfatal("No extrinsics found!") rospy.signal_shutdown("No extrinsics found!") with open(path, "r") as f: data = yaml.safe_load(f) H = np.array(data["H"]["data"]) H = np.resize(H, (3, 3)) rospy.loginfo("[{}] Waiting for camera_info message..." "".format(rospy.get_name())) camera_info = rospy.wait_for_message("camera_node/camera_info", CameraInfo) rospy.loginfo("[{}] Received camera_info message." "".format(rospy.get_name())) # publishing rate self.rate = rospy.Rate(self._params["rate"]) # set true for testing self.visualize = self._params["visualization"] # create curve point detector hsv_mask_interval = np.matrix( [self._params["HSV_low"], self._params["HSV_high"]]) self.__curve_point_detector = CurvePointExtractor( hsv_mask_interval, int(self._params["num_points"]), self._params["cropping_factors"], self.visualize) if camera_info.D[0] == 0.0: self.curve_estimator = CurveEstimator(camera_info, H, False) else: self.curve_estimator = CurveEstimator(camera_info, H, True) # register all publishers self.init_publishers() # register all publishers self.init_subscribers() # ros Image has to be bridged to openCV self.bridge = CvBridge()