def __init__(self, node_name):
# Initialize the DTROS parent class
super(StopLineFilterNode, self).__init__(node_name=node_name, node_type=NodeType.PERCEPTION)
# Initialize the parameters
self.stop_distance = DTParam("~stop_distance", param_type=ParamType.FLOAT)
self.min_segs = DTParam("~min_segs", param_type=ParamType.INT)
self.off_time = DTParam("~off_time", param_type=ParamType.FLOAT)
self.max_y = DTParam("~max_y", param_type=ParamType.FLOAT)
## params
# self.stop_distance = self.setupParam("~stop_distance", 0.22) # distance from the stop line that we should stop
# self.min_segs = self.setupParam("~min_segs", 2) # minimum number of red segments that we should detect to estimate a stop
# self.off_time = self.setupParam("~off_time", 2)
# self.max_y = self.setupParam("~max_y ", 0.2) # If y value of detected red line is smaller than max_y we will not set at_stop_line true.
## state vars
self.lane_pose = LanePose()
self.state = "JOYSTICK_CONTROL"
self.sleep = False
## publishers and subscribers
self.sub_segs = rospy.Subscriber("~segment_list", SegmentList, self.cb_segments)
self.sub_lane = rospy.Subscriber("~lane_pose", LanePose, self.cb_lane_pose)
self.sub_mode = rospy.Subscriber("fsm_node/mode", FSMState, self.cb_state_change)
self.pub_stop_line_reading = rospy.Publisher("~stop_line_reading", StopLineReading, queue_size=1)
self.pub_at_stop_line = rospy.Publisher("~at_stop_line", BoolStamped, queue_size=1)
def __init__(self, node_name):
# Initialize the DTROS parent class
super(VehicleFilterNode, self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Add the node parameters to the parameters dictionary and load their default values
self.distance_between_centers = DTParam('~distance_between_centers',
param_type=ParamType.FLOAT)
self.max_reproj_pixelerror_pose_estimation = DTParam(
'~max_reproj_pixelerror_pose_estimation',
param_type=ParamType.FLOAT)
self.virtual_stop_line_offset = DTParam('~virtual_stop_line_offset',
param_type=ParamType.FLOAT)
self.bridge = CvBridge()
# these will be defined on the first call to calc_circle_pattern
self.last_calc_circle_pattern = None
self.circlepattern_dist = None
self.circlepattern = None
self.state = None
# subscribers
self.sub_centers = rospy.Subscriber("~centers",
VehicleCorners,
self.cb_process_centers,
queue_size=1)
self.sub_info = rospy.Subscriber("~camera_info",
CameraInfo,
self.cb_process_camera_info,
queue_size=1)
""" TODO: REMOVE AFTER ROAD_ANOMALY NODE CONSTRUCTION"""
self.sub_state = rospy.Subscriber("~mode", FSMState, self.cbFSMState)
# publishers
self.pub_virtual_stop_line = rospy.Publisher("~virtual_stop_line",
StopLineReading,
queue_size=1)
self.pub_visualize = rospy.Publisher("~debug/visualization_marker",
Marker,
queue_size=1)
self.pub_stopped_flag = rospy.Publisher("~stopped",
BoolStamped,
queue_size=1)
self.pcm = PinholeCameraModel()
self.changePattern = rospy.ServiceProxy("~set_pattern", ChangePattern)
self.log("Initialization completed")
self.last_led_state = None
def __init__(self, node_name):
super().__init__(node_name, node_type=NodeType.PERCEPTION)
# parameters
self.publish_freq = DTParam("~publish_freq", -1)
# utility objects
self.bridge = CvBridge()
self.reminder = DTReminder(frequency=self.publish_freq.value)
# subscribers
self.sub_img = rospy.Subscriber("~image_in",
CompressedImage,
self.cb_image,
queue_size=1,
buff_size="10MB")
# publishers
self.pub_img = rospy.Publisher(
"~image_out",
Image,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION,
dt_healthy_freq=self.publish_freq.value,
dt_help="Raw image",
)
def _init_dtparam(name):
str_topics = []
param_type = ParamType.STRING if name in str_topics else ParamType.FLOAT
return DTParam(f'~{name}',
param_type=param_type,
min_value=-100.0,
max_value=100.0)
def __init__(self, node_name):
super().__init__(node_name, node_type=NodeType.PERCEPTION)
# parameters
self.publish_freq = DTParam("~publish_freq", -1)
self.alpha = DTParam("~alpha", 0.0)
# utility objects
self.jpeg = TurboJPEG()
self.reminder = DTReminder(frequency=self.publish_freq.value)
self.camera_model = None
self.rect_camera_info = None
self.mapx, self.mapy = None, None
# subscribers
self.sub_img = rospy.Subscriber(
"~image_in",
CompressedImage,
self.cb_image,
queue_size=1,
buff_size='10MB'
)
self.sub_camera_info = rospy.Subscriber(
"~camera_info_in",
CameraInfo,
self.cb_camera_info,
queue_size=1
)
# publishers
self.pub_img = rospy.Publisher(
"~image/compressed",
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION,
dt_healthy_freq=self.publish_freq.value,
dt_help="Rectified image (i.e., image with no distortion effects from the lens)."
)
self.pub_camera_info = rospy.Publisher(
"~camera_info",
CameraInfo,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION,
dt_healthy_freq=self.publish_freq.value,
dt_help="Camera parameters for the (virtual) rectified camera."
)
def __init__(self, node_name):
# Initialize the DTROS parent class
super(VehicleDetectionNode,
self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Initialize the parameters
self.process_frequency = DTParam("~process_frequency",
param_type=ParamType.FLOAT)
self.circlepattern_dims = DTParam("~circlepattern_dims",
param_type=ParamType.LIST)
self.blobdetector_min_area = DTParam("~blobdetector_min_area",
param_type=ParamType.FLOAT)
self.blobdetector_min_dist_between_blobs = DTParam(
"~blobdetector_min_dist_between_blobs", param_type=ParamType.FLOAT)
# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
#
self.cbParametersChanged(
) # TODO: THIS SHOULD BE FIXED IN THE NEW DTROS!!!!!!!!!!
#
# !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
self.bridge = CvBridge()
self.last_stamp = rospy.Time.now()
# Subscriber
self.sub_image = rospy.Subscriber("~image",
CompressedImage,
self.cb_image,
queue_size=1)
# Publishers
self.pub_centers = rospy.Publisher("~centers",
VehicleCorners,
queue_size=1)
self.pub_circlepattern_image = rospy.Publisher(
"~debug/detection_image/compressed", CompressedImage, queue_size=1)
self.pub_detection_flag = rospy.Publisher("~detection",
BoolStamped,
queue_size=1)
self.log("Initialization completed.")
def __init__(self, node_name):
# initialize the DTROS parent class
super(ColorDetectorNode, self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# bridge between opencv and ros
self.bridge = CvBridge()
# construct subscriber for images
self.sub = rospy.Subscriber('~duckie_cam/compressed', CompressedImage,
self.callback)
# construct publisher for debug images
self.pub = rospy.Publisher('~debug_images/compressed',
CompressedImage,
queue_size=10)
self.color = DTParam(
'~color',
param_type=ParamType.STRING,
)
self.downscale = DTParam('~downscale',
param_type=ParamType.FLOAT,
min_value=0.5,
max_value=1.0)
def __init__(self):
super(AprilTagDetector,
self).__init__(node_name='apriltag_detector_node',
node_type=NodeType.PERCEPTION)
# get static parameters
self.family = rospy.get_param('~family', 'tag36h11')
self.nthreads = rospy.get_param('~nthreads', 1)
self.quad_decimate = rospy.get_param('~quad_decimate', 1.0)
self.quad_sigma = rospy.get_param('~quad_sigma', 0.0)
self.refine_edges = rospy.get_param('~refine_edges', 1)
self.decode_sharpening = rospy.get_param('~decode_sharpening', 0.25)
self.tag_size = rospy.get_param('~tag_size', 0.065)
# dynamic parameter
self.detection_freq = DTParam('~detection_freq',
default=-1,
param_type=ParamType.INT,
min_value=-1,
max_value=30)
self._detection_reminder = DTReminder(
frequency=self.detection_freq.value)
# camera info
self._camera_parameters = None
self._camera_frame = None
# create detector object
self._at_detector = Detector(families=self.family,
nthreads=self.nthreads,
quad_decimate=self.quad_decimate,
quad_sigma=self.quad_sigma,
refine_edges=self.refine_edges,
decode_sharpening=self.decode_sharpening)
# create a CV bridge object
self._bridge = CvBridge()
# create subscribers
self._img_sub = rospy.Subscriber('image_rect', Image, self._img_cb)
self._cinfo_sub = rospy.Subscriber('camera_info', CameraInfo,
self._cinfo_cb)
# create publishers
self._img_pub = rospy.Publisher('tag_detections_image/compressed',
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.VISUALIZATION)
self._tag_pub = rospy.Publisher('tag_detections',
AprilTagDetectionArray,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION)
self._img_pub_busy = False
# create TF broadcaster
self._tf_bcaster = tf.TransformBroadcaster()
# spin forever
rospy.spin()
def __init__(self, node_name):
# Initialize the DTROS parent class
super(LaneControllerNode, self).__init__(node_name=node_name,
node_type=NodeType.CONTROL)
# Add the node parameters to the parameters dictionary
# TODO: MAKE TO WORK WITH NEW DTROS PARAMETERS
self.params = dict()
self.params['~v_bar'] = DTParam('~v_bar',
param_type=ParamType.FLOAT,
min_value=0.0,
max_value=5.0)
# Construct publishers
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1,
dt_topic_type=TopicType.CONTROL)
# we will just call the controller at 10Hz to publish constant velocity
rospy.Timer(rospy.Duration(0.1), self.getControlAction)
self.log("Initialized!")
def __init__(self, node_name):
# Initialize the DTROS parent class
super(KinematicsNode, self).__init__(node_name=node_name, node_type=NodeType.CONTROL)
# Get the vehicle name
self.veh_name = rospy.get_namespace().strip("/")
# Read parameters from a robot-specific yaml file if such exists
self.read_params_from_calibration_file()
# Get static parameters
self._k = rospy.get_param("~k")
# Get editable parameters
self._gain = DTParam("~gain", param_type=ParamType.FLOAT, min_value=0.1, max_value=1.0)
self._trim = DTParam("~trim", param_type=ParamType.FLOAT, min_value=0.1, max_value=1.0)
self._limit = DTParam("~limit", param_type=ParamType.FLOAT, min_value=0.1, max_value=1.0)
self._baseline = DTParam("~baseline", param_type=ParamType.FLOAT, min_value=0.05, max_value=0.2)
self._radius = DTParam("~radius", param_type=ParamType.FLOAT, min_value=0.01, max_value=0.1)
self._v_max = DTParam("~v_max", param_type=ParamType.FLOAT, min_value=0.01, max_value=2.0)
self._omega_max = DTParam("~omega_max", param_type=ParamType.FLOAT, min_value=1.0, max_value=10.0)
# Prepare the save calibration service
self.srv_save = rospy.Service("~save_calibration", Empty, self.srv_save_calibration)
# Setup publishers
self.pub_wheels_cmd = rospy.Publisher(
"~wheels_cmd", WheelsCmdStamped, queue_size=1, dt_topic_type=TopicType.CONTROL
)
self.pub_velocity = rospy.Publisher(
"~velocity", Twist2DStamped, queue_size=1, dt_topic_type=TopicType.CONTROL
)
# Setup subscribers
self.sub_car_cmd = rospy.Subscriber("~car_cmd", Twist2DStamped, self.car_cmd_callback)
# ---
self.log("Initialized with: \n%s" % self.get_configuration_as_str())
def __init__(self, node_name):
# Initialize the DTROS parent class
super(LaneControllerNode, self).__init__(node_name=node_name,
node_type=NodeType.PERCEPTION)
# Add the node parameters to the parameters dictionary
# TODO: MAKE TO WORK WITH NEW DTROS PARAMETERS
self.params = dict()
self.params['~v_bar'] = DTParam('~v_bar',
param_type=ParamType.FLOAT,
min_value=0.0,
max_value=5.0)
self.params['~k_d'] = DTParam('~k_d',
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params['~k_theta'] = DTParam('~k_theta',
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params['~k_Id'] = DTParam('~k_Id',
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params['~k_Iphi'] = DTParam('~k_Iphi',
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params['~theta_thres'] = rospy.get_param('~theta_thres', None)
self.params['~d_thres'] = rospy.get_param('~d_thres', None)
self.params['~d_offset'] = rospy.get_param('~d_offset', None)
self.params['~integral_bounds'] = rospy.get_param(
'~integral_bounds', None)
self.params['~d_resolution'] = rospy.get_param('~d_resolution', None)
self.params['~phi_resolution'] = rospy.get_param(
'~phi_resolution', None)
self.params['~omega_ff'] = rospy.get_param('~omega_ff', None)
self.params['~verbose'] = rospy.get_param('~verbose', None)
self.params['~stop_line_slowdown'] = rospy.get_param(
'~stop_line_slowdown', None)
# Need to create controller object before updating parameters, otherwise it will fail
self.controller = LaneController(self.params)
# self.updateParameters() # TODO: This needs be replaced by the new DTROS callback when it is implemented
# Initialize variables
self.fsm_state = None
self.wheels_cmd_executed = WheelsCmdStamped()
self.pose_msg = LanePose()
self.pose_initialized = False
self.pose_msg_dict = dict()
self.last_s = None
self.stop_line_distance = None
self.stop_line_detected = False
self.at_stop_line = False
self.obstacle_stop_line_distance = None
self.obstacle_stop_line_detected = False
self.at_obstacle_stop_line = False
self.current_pose_source = 'lane_filter'
# Construct publishers
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1,
dt_topic_type=TopicType.CONTROL)
# Construct subscribers
self.sub_lane_reading = rospy.Subscriber("~lane_pose",
LanePose,
self.cbAllPoses,
"lane_filter",
queue_size=1)
self.sub_intersection_navigation_pose = rospy.Subscriber(
"~intersection_navigation_pose",
LanePose,
self.cbAllPoses,
"intersection_navigation",
queue_size=1)
self.sub_wheels_cmd_executed = rospy.Subscriber(
"~wheels_cmd",
WheelsCmdStamped,
self.cbWheelsCmdExecuted,
queue_size=1)
self.sub_stop_line = rospy.Subscriber("~stop_line_reading",
StopLineReading,
self.cbStopLineReading,
queue_size=1)
self.sub_obstacle_stop_line = rospy.Subscriber(
"~obstacle_distance_reading",
StopLineReading,
self.cbObstacleStopLineReading,
queue_size=1)
self.log("Initialized!")
def __init__(self, node_name):
# Initialize the DTROS parent class
super(LaneControllerNode, self).__init__(node_name=node_name,
node_type=NodeType.CONTROL)
## Add the node parameters to the parameters dictionary (1) and
## initialize controller (2)
# 1. get parameters
self.params = dict()
self.params["~v_forward"] = DTParam("~v_forward",
default=0.5,
help="forward speed",
param_type=ParamType.FLOAT,
min_value=0.0,
max_value=5.0)
self.params["~k_theta"] = DTParam("~k_theta",
default=5.0,
help="k_theta's value",
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params["~k_d"] = DTParam("~k_d",
help="k_d's value",
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params["~k_dd"] = DTParam("~k_dd",
default=5.0,
help="k_dd's value",
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params["~k_dtheta"] = DTParam("~k_dtheta",
default=5.0,
help="k_dtheta's value",
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params["~k_Id"] = DTParam("~k_Id",
default=0.0,
help="k_Id's value",
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params["~k_Itheta"] = DTParam("~k_Itheta",
default=0.0,
help="k_Id's value",
param_type=ParamType.FLOAT,
min_value=-100.0,
max_value=100.0)
self.params["~theta_threshold"] = rospy.get_param(
"~theta_threshold", np.pi / 6)
self.params["~d_threshold"] = rospy.get_param("~d_threshold", 0.3)
self.params["~look_ahead_k"] = rospy.get_param("~look_ahead_k", 3.0)
self.params["~k_d_critically_damped_flag"] = rospy.get_param(
"~k_d_critically_damped_flag", False)
self.params["~d_resolution"] = rospy.get_param("~d_resolution", 0.005)
self.params["~theta_resolution"] = rospy.get_param(
"~theta_resolution", 0.1)
self.params["~d_threshold_robust_flag"] = rospy.get_param(
"~d_threshold_robust_flag", False)
self.params["~slow_down_multiplier"] = rospy.get_param(
"~slow_down_multiplier", 0.5)
self.params["~slow_down_theta_thres"] = rospy.get_param(
"~slow_down_theta_thres", 0.5)
self.params["~slow_down_d_thres"] = rospy.get_param(
"~slow_down_d_thres", 0.5)
# 2. initialize controller
self.controller_type = rospy.get_param("~controller_type", None)
if self.controller_type == "turn_pid":
self.params["~turning_config"] = rospy.get_param("~turning_config")
assert self.controller_type in \
LaneControllerNode.CONTROLLER_LOOKUP.keys()
self.controller = LaneControllerNode.CONTROLLER_LOOKUP\
[self.controller_type](self.params)
# initialize useful parameters
self.last_time = None
self.fsm_state = None
self.wheels_cmd_executed = WheelsCmdStamped()
self.at_stop_line = False
self.at_obstacle_stop_line = False
# Construct publishers
self.pub_car_cmd = rospy.Publisher("~car_cmd",
Twist2DStamped,
queue_size=1,
dt_topic_type=TopicType.CONTROL)
# Construct subscribers
self.sub_lane_reading = rospy.Subscriber("~lane_pose",
LanePose,
self.cbLanePoses,
queue_size=1)
self.sub_lane_reading = rospy.Subscriber("~lane_filter",
LanePose,
self.cbLaneFilters,
queue_size=1)
self.sub_intersection_navigation_pose = rospy.Subscriber(
"~intersection_navigation_pose",
LanePose,
self.cbIntersectionPoses,
queue_size=1)
self.sub_wheels_cmd_execute = rospy.Subscriber("~wheels_cmd_executed",
WheelsCmdStamped,
self.cbWheelsCmdExecute,
queue_size=1)
self.sub_fsm_node = rospy.Subscriber("~fsm_mode",
FSMState,
self.cbMode,
queue_size=1)
self.sub_stop_line = rospy.Subscriber("~stop_line_reading",
StopLineReading,
self.cbStopLineReading,
queue_size=1)
self.sub_obstacle_stop_line = rospy.Subscriber(
"~obstacle_distance_reading",
StopLineReading,
self.cbObstacleStopLineReading,
queue_size=1)
self.log("Initialized!")
self.log("Lane controller type = {}.".format(self.controller_type))
self.log("Slow down multiplier = {}".format(
self.params["~slow_down_multiplier"]))
self.log("Slow down theta tres = {}".format(
self.params["~slow_down_theta_thres"]))
self.log("Slow down d thres = {}".format(
self.params["~slow_down_d_thres"]))
def __init__(self):
super(AprilTagDetector, self).__init__(
node_name='apriltag_detector_node',
node_type=NodeType.PERCEPTION
)
# get static parameters
self.family = rospy.get_param('~family', 'tag36h11')
self.ndetectors = rospy.get_param('~ndetectors', 1)
self.nthreads = rospy.get_param('~nthreads', 1)
self.quad_decimate = rospy.get_param('~quad_decimate', 1.0)
self.quad_sigma = rospy.get_param('~quad_sigma', 0.0)
self.refine_edges = rospy.get_param('~refine_edges', 1)
self.decode_sharpening = rospy.get_param('~decode_sharpening', 0.25)
self.tag_size = rospy.get_param('~tag_size', 0.065)
self.rectify_alpha = rospy.get_param('~rectify_alpha', 0.0)
# dynamic parameter
self.detection_freq = DTParam(
'~detection_freq',
default=-1,
param_type=ParamType.INT,
min_value=-1,
max_value=30
)
self._detection_reminder = DTReminder(frequency=self.detection_freq.value)
# camera info
self._camera_parameters = None
self._mapx, self._mapy = None, None
# create detector object
self._detectors = [Detector(
families=self.family,
nthreads=self.nthreads,
quad_decimate=self.quad_decimate,
quad_sigma=self.quad_sigma,
refine_edges=self.refine_edges,
decode_sharpening=self.decode_sharpening
) for _ in range(self.ndetectors)]
self._renderer_busy = False
# create a CV bridge object
self._jpeg = TurboJPEG()
# create subscribers
self._img_sub = rospy.Subscriber(
'~image',
CompressedImage,
self._img_cb,
queue_size=1,
buff_size='20MB'
)
self._cinfo_sub = rospy.Subscriber(
'~camera_info',
CameraInfo,
self._cinfo_cb,
queue_size=1
)
# create publisher
self._tag_pub = rospy.Publisher(
'~detections',
AprilTagDetectionArray,
queue_size=1,
dt_topic_type=TopicType.PERCEPTION,
dt_help='Tag detections',
)
self._img_pub = rospy.Publisher(
'~detections/image/compressed',
CompressedImage,
queue_size=1,
dt_topic_type=TopicType.VISUALIZATION,
dt_help='Camera image with tag publishs superimposed',
)
# create thread pool
self._workers = ThreadPoolExecutor(self.ndetectors)
self._tasks = [None] * self.ndetectors
# create TF broadcaster
self._tf_bcaster = tf.TransformBroadcaster()