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