diff --git a/packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch b/packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch
new file mode 100644
index 00000000..022494f6
--- /dev/null
+++ b/packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py
new file mode 100644
index 00000000..33f601cb
--- /dev/null
+++ b/packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py
@@ -0,0 +1,222 @@
+#!/usr/bin/env python3
+import rospkg
+import rospy
+import yaml
+from duckietown_msgs.msg import \
+ AprilTagsWithInfos, \
+ TagInfo, \
+ BoolStamped, \
+ AprilTagDetection, \
+ AprilTagDetectionArray, \
+ FSMState
+import numpy as np
+import tf.transformations as tr
+from geometry_msgs.msg import PoseStamped, Pose
+from std_msgs.msg import Header
+
+
+class AprilPostPros(object):
+ """ """
+
+ def __init__(self):
+ """ """
+ self.node_name = "apriltag_postprocessing_node"
+ self.timestamp = rospy.Time(0)
+ self.state = None
+ self.max_time_waiting = 1
+ self.last_time_apr_tag = rospy.Time(0)
+ # Load parameters
+ self.camera_x = self.setupParam("~camera_x", 0.065)
+ self.camera_y = self.setupParam("~camera_y", 0.0)
+ self.camera_z = self.setupParam("~camera_z", 0.11)
+ self.camera_theta = self.setupParam("~camera_theta", 19.0)
+ self.scale_x = self.setupParam("~scale_x", 1)
+ self.scale_y = self.setupParam("~scale_y", 1)
+ self.scale_z = self.setupParam("~scale_z", 1)
+
+ # -------- Start adding back the tag info stuff
+
+ tags_filepath = self.setupParam("~tags_file")
+
+ self.loc = self.setupParam("~loc", -1) # -1 if no location is given
+ tags_file = open(tags_filepath, 'r')
+ self.tags_dict = yaml.safe_load(tags_file)
+ tags_file.close()
+ self.info = TagInfo()
+
+ self.sign_types = {"StreetName": self.info.S_NAME,
+ "TrafficSign": self.info.SIGN,
+ "Light": self.info.LIGHT,
+ "Localization": self.info.LOCALIZE,
+ "Vehicle": self.info.VEHICLE}
+ self.traffic_sign_types = {"stop": self.info.STOP,
+ "yield": self.info.YIELD,
+ "no-right-turn": self.info.NO_RIGHT_TURN,
+ "no-left-turn": self.info.NO_LEFT_TURN,
+ "oneway-right": self.info.ONEWAY_RIGHT,
+ "oneway-left": self.info.ONEWAY_LEFT,
+ "4-way-intersect": self.info.FOUR_WAY,
+ "right-T-intersect": self.info.RIGHT_T_INTERSECT,
+ "left-T-intersect": self.info.LEFT_T_INTERSECT,
+ "T-intersection": self.info.T_INTERSECTION,
+ "do-not-enter": self.info.DO_NOT_ENTER,
+ "pedestrian": self.info.PEDESTRIAN,
+ "t-light-ahead": self.info.T_LIGHT_AHEAD,
+ "duck-crossing": self.info.DUCK_CROSSING,
+ "parking": self.info.PARKING}
+
+ # ---- end tag info stuff
+
+ self.sub_prePros = rospy.Subscriber("~detections", AprilTagDetectionArray, self.callbackDetections,
+ queue_size=1)
+ self.pub_postPros = rospy.Publisher("~apriltags_out", AprilTagsWithInfos, queue_size=1)
+ self.pub_visualize = rospy.Publisher("~tag_pose", PoseStamped, queue_size=1)
+
+ # topics for state machine
+ self.pub_postPros_parking = rospy.Publisher("~apriltags_parking", BoolStamped, queue_size=1)
+ self.pub_postPros_intersection = rospy.Publisher("~apriltags_intersection", BoolStamped, queue_size=1)
+
+ rospy.loginfo("[%s] has started", self.node_name)
+ self.sub_fsm = rospy.Subscriber("~fsm_state", FSMState, self.callbackFSMState)
+ self.fake_tag_publisher = rospy.Publisher("~detections", AprilTagDetectionArray, queue_size=1)
+ self.timer = rospy.Timer(rospy.Duration(1), self.callbackTimer)
+
+ def shutdown_hook(self):
+ rospy.loginfo("Publishing message shutdown")
+
+ def on_shutdown(self):
+ self.shutdown_hook()
+ super().on_shutdown()
+
+ def callbackFSMState(self, msg):
+ if self.state != msg.state and msg.state == "INTERSECTION_COORDINATION":
+ self.turn_type = -1
+ self.state = msg.state
+ self.timestamp = rospy.Time.now()
+ rospy.loginfo("State: {}\n Timestamp: {}".format(self.state, self.timestamp))
+
+ def callbackTimer(self, timer):
+ rospy.logwarn('Timer is called')
+ rospy.loginfo('AprilTag last time: {}'.format(self.last_time_apr_tag))
+ diff = rospy.Time.now() - self.last_time_apr_tag
+ rospy.logwarn('Time diff: {}, State: {}'.format(diff.to_sec(), self.state))
+ if self.state == 'INTERSECTION_COORDINATION' and diff.to_sec() > self.max_time_waiting:
+ rospy.loginfo('Bot is still in intersection')
+ self.fake_tag()
+
+ def setupParam(self, param_name, default_value=rospy.client._Unspecified):
+ value = rospy.get_param(param_name, default_value)
+ rospy.set_param(param_name, value)
+ return value
+
+ def fake_tag(self):
+ tags_msg = AprilTagDetectionArray()
+ tags_msg.header.stamp = rospy.Time.now()
+ tags_msg.header.frame_id = ""
+ tag = AprilTagDetection(
+ tag_id=56,
+ tag_family="tag36h11"
+ )
+ tags_msg.detections.append(tag)
+ self.fake_tag_publisher.publish(tags_msg)
+
+ def callbackDetections(self, msg):
+ tag_infos = []
+ new_tag_data = AprilTagsWithInfos()
+ # Load tag detections message
+ if msg.detections:
+ self.last_time_apr_tag = rospy.Time.now()
+ for detection in msg.detections:
+ # ------ start tag info processing
+ rospy.loginfo("detection.tag_id = %s", str(detection.tag_id))
+ new_info = TagInfo()
+ # Can use id 1 as long as no bundles are used
+ new_info.id = int(detection.tag_id)
+ id_info = self.tags_dict[new_info.id]
+ # rospy.loginfo("[%s] report detected id=[%s] for april tag!",self.node_name,new_info.id)
+ # Check yaml file to fill in ID-specific information
+ new_info.tag_type = self.sign_types[id_info['tag_type']]
+ if new_info.tag_type == self.info.S_NAME:
+ new_info.street_name = id_info['street_name']
+ elif new_info.tag_type == self.info.SIGN:
+ new_info.traffic_sign_type = self.traffic_sign_types[id_info['traffic_sign_type']]
+
+ # publish for FSM
+ # parking apriltag event
+ msg_parking = BoolStamped()
+ msg_parking.header.stamp = rospy.Time(0)
+ if new_info.traffic_sign_type == TagInfo.PARKING:
+ msg_parking.data = True
+ else:
+ msg_parking.data = False
+ self.pub_postPros_parking.publish(msg_parking)
+
+ # intersection apriltag event
+ msg_intersection = BoolStamped()
+ msg_intersection.header.stamp = rospy.Time(0)
+ if (new_info.traffic_sign_type == TagInfo.FOUR_WAY) or (
+ new_info.traffic_sign_type == TagInfo.RIGHT_T_INTERSECT) or (
+ new_info.traffic_sign_type == TagInfo.LEFT_T_INTERSECT) or (
+ new_info.traffic_sign_type == TagInfo.T_INTERSECTION):
+ msg_intersection.data = True
+ else:
+ msg_intersection.data = False
+ self.pub_postPros_intersection.publish(msg_intersection)
+
+ elif new_info.tag_type == self.info.VEHICLE:
+ new_info.vehicle_name = id_info['vehicle_name']
+
+ # TODO: Implement location more than just a float like it is now.
+ # location is now 0.0 if no location is set which is probably not that smart
+ if self.loc == 226:
+ l = (id_info['location_226'])
+ if l is not None:
+ new_info.location = l
+ elif self.loc == 316:
+ l = (id_info['location_316'])
+ if l is not None:
+ new_info.location = l
+
+ tag_infos.append(new_info)
+ # --- end tag info processing
+
+ # Define the transforms
+ veh_t_camxout = tr.translation_matrix((self.camera_x, self.camera_y, self.camera_z))
+ veh_R_camxout = tr.euler_matrix(0, self.camera_theta * np.pi / 180, 0, 'rxyz')
+ veh_T_camxout = tr.concatenate_matrices(veh_t_camxout, veh_R_camxout) # 4x4 Homogeneous Transform Matrix
+
+ camxout_T_camzout = tr.euler_matrix(-np.pi / 2, 0, -np.pi / 2, 'rzyx')
+ veh_T_camzout = tr.concatenate_matrices(veh_T_camxout, camxout_T_camzout)
+
+ tagzout_T_tagxout = tr.euler_matrix(-np.pi / 2, 0, np.pi / 2, 'rxyz')
+
+ # Load translation
+ trans = detection.transform.translation
+ rot = detection.transform.rotation
+ header = Header()
+ header.stamp = rospy.Time.now()
+ camzout_t_tagzout = tr.translation_matrix(
+ (trans.x * self.scale_x, trans.y * self.scale_y, trans.z * self.scale_z))
+ camzout_R_tagzout = tr.quaternion_matrix((rot.x, rot.y, rot.z, rot.w))
+ camzout_T_tagzout = tr.concatenate_matrices(camzout_t_tagzout, camzout_R_tagzout)
+
+ veh_T_tagxout = tr.concatenate_matrices(veh_T_camzout, camzout_T_tagzout, tagzout_T_tagxout)
+
+ # Overwrite transformed value
+ (trans.x, trans.y, trans.z) = tr.translation_from_matrix(veh_T_tagxout)
+ (rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_matrix(veh_T_tagxout)
+
+ new_tag_data.detections.append(
+ detection
+ )
+
+ new_tag_data.infos = tag_infos
+ # Publish Message
+ self.pub_postPros.publish(new_tag_data)
+
+
+
+if __name__ == '__main__':
+ rospy.init_node('AprilPostPros', anonymous=False)
+ node = AprilPostPros()
+ rospy.spin()
diff --git a/packages/dt-core/packages/lane_control/config/lane_controller_node/default.yaml b/packages/dt-core/packages/lane_control/config/lane_controller_node/default.yaml
new file mode 100644
index 00000000..5e0fda70
--- /dev/null
+++ b/packages/dt-core/packages/lane_control/config/lane_controller_node/default.yaml
@@ -0,0 +1,29 @@
+#lane_control/lane_controller_node
+v_bar: 0.19
+k_d: -6.0
+k_theta: -5
+k_Id: -0.3
+k_Iphi: 0.0
+theta_thres: 0.523
+d_thres: 0.2615
+d_offset: 0.0
+
+omega_ff: 0
+
+integral_bounds:
+ d:
+ top: 0.3
+ bot: -0.3
+ phi:
+ top: 1.2
+ bot: -1.2
+
+# TODO: lane filter should publish this information
+d_resolution: 0.011
+phi_resolution: 0.051
+
+stop_line_slowdown:
+ start: 0.6
+ end: 0.18
+
+verbose: 0
diff --git a/packages/dt-core/packages/lane_control/include/lane_controller/controller.py b/packages/dt-core/packages/lane_control/include/lane_controller/controller.py
new file mode 100644
index 00000000..4d6d0f60
--- /dev/null
+++ b/packages/dt-core/packages/lane_control/include/lane_controller/controller.py
@@ -0,0 +1,172 @@
+import numpy as np
+
+
+class LaneController:
+ """
+ The Lane Controller can be used to compute control commands from pose estimations.
+
+ The control commands are in terms of linear and angular velocity (v, omega). The input are errors in the relative
+ pose of the Duckiebot in the current lane.
+
+ This implementation is a simple PI(D) controller.
+
+ Args:
+ ~v_bar (:obj:`float`): Nominal velocity in m/s
+ ~k_d (:obj:`float`): Proportional term for lateral deviation
+ ~k_theta (:obj:`float`): Proportional term for heading deviation
+ ~k_Id (:obj:`float`): integral term for lateral deviation
+ ~k_Iphi (:obj:`float`): integral term for lateral deviation
+ ~d_thres (:obj:`float`): Maximum value for lateral error
+ ~theta_thres (:obj:`float`): Maximum value for heading error
+ ~d_offset (:obj:`float`): Goal offset from center of the lane
+ ~integral_bounds (:obj:`dict`): Bounds for integral term
+ ~d_resolution (:obj:`float`): Resolution of lateral position estimate
+ ~phi_resolution (:obj:`float`): Resolution of heading estimate
+ ~omega_ff (:obj:`float`): Feedforward part of controller
+ ~verbose (:obj:`bool`): Verbosity level (0,1,2)
+ ~stop_line_slowdown (:obj:`dict`): Start and end distances for slowdown at stop lines
+
+ """
+
+ def __init__(self, parameters):
+ self.parameters = parameters
+ self.d_I = 0.0
+ self.phi_I = 0.0
+ self.prev_d_err = 0.0
+ self.prev_phi_err = 0.0
+
+ def update_parameters(self, parameters):
+ """Updates parameters of LaneController object.
+
+ Args:
+ parameters (:obj:`dict`): dictionary containing the new parameters for LaneController object.
+ """
+ self.parameters = parameters
+
+ def compute_control_action(self, d_err, phi_err, dt, wheels_cmd_exec, stop_line_distance):
+ """Main function, computes the control action given the current error signals.
+
+ Given an estimate of the error, computes a control action (tuple of linear and angular velocity). This is done
+ via a basic PI(D) controller with anti-reset windup logic.
+
+ Args:
+ d_err (:obj:`float`): error in meters in the lateral direction
+ phi_err (:obj:`float`): error in radians in the heading direction
+ dt (:obj:`float`): time since last command update
+ wheels_cmd_exec (:obj:`bool`): confirmation that the wheel commands have been executed (to avoid
+ integration while the robot does not move)
+ stop_line_distance (:obj:`float`): distance of the stop line, None if not detected.
+ Returns:
+ v (:obj:`float`): requested linear velocity in meters/second
+ omega (:obj:`float`): requested angular velocity in radians/second
+ """
+
+ if dt is not None:
+ self.integrate_errors(d_err, phi_err, dt)
+
+ self.d_I = self.adjust_integral(
+ d_err, self.d_I, self.parameters["~integral_bounds"]["d"], self.parameters["~d_resolution"]
+ )
+ self.phi_I = self.adjust_integral(
+ phi_err,
+ self.phi_I,
+ self.parameters["~integral_bounds"]["phi"],
+ self.parameters["~phi_resolution"],
+ )
+
+ self.reset_if_needed(d_err, phi_err, wheels_cmd_exec)
+
+ # Scale the parameters linear such that their real value is at 0.22m/s
+ omega = (
+ self.parameters["~k_d"].value * d_err
+ + self.parameters["~k_theta"].value * phi_err
+ + self.parameters["~k_Id"].value * self.d_I
+ + self.parameters["~k_Iphi"].value * self.phi_I
+ )
+
+ self.prev_d_err = d_err
+ self.prev_phi_err = phi_err
+
+ v = self.compute_velocity(stop_line_distance)
+
+ return v, omega
+
+ def compute_velocity(self, stop_line_distance):
+ """Linearly decrease velocity if approaching a stop line.
+
+ If a stop line is detected, the velocity is linearly decreased to achieve a better stopping position,
+ otherwise the nominal velocity is returned.
+
+ Args:
+ stop_line_distance (:obj:`float`): distance of the stop line, None if not detected.
+ """
+ if stop_line_distance is None:
+ return self.parameters["~v_bar"].value
+ else:
+
+ d1, d2 = (
+ self.parameters["~stop_line_slowdown"]["start"],
+ self.parameters["~stop_line_slowdown"]["end"],
+ )
+ # d1 -> v_bar, d2 -> v_bar/2
+ c = (0.5 * (d1 - stop_line_distance) + (stop_line_distance - d2)) / (d1 - d2)
+ v_new = self.parameters["~v_bar"].value * c
+ v = np.max(
+ [self.parameters["~v_bar"].value / 2.0, np.min([self.parameters["~v_bar"].value, v_new])]
+ )
+ return v
+
+ def integrate_errors(self, d_err, phi_err, dt):
+ """Integrates error signals in lateral and heading direction.
+ Args:
+ d_err (:obj:`float`): error in meters in the lateral direction
+ phi_err (:obj:`float`): error in radians in the heading direction
+ dt (:obj:`float`): time delay in seconds
+ """
+ self.d_I += d_err * dt
+ self.phi_I += phi_err * dt
+
+ def reset_if_needed(self, d_err, phi_err, wheels_cmd_exec):
+ """Resets the integral error if needed.
+
+ Resets the integral errors in `d` and `phi` if either the error sign changes, or if the robot is completely
+ stopped (i.e. intersections).
+
+ Args:
+ d_err (:obj:`float`): error in meters in the lateral direction
+ phi_err (:obj:`float`): error in radians in the heading direction
+ wheels_cmd_exec (:obj:`bool`): confirmation that the wheel commands have been executed (to avoid
+ integration while the robot does not move)
+ """
+ if np.sign(d_err) != np.sign(self.prev_d_err):
+ self.d_I = 0
+ if np.sign(phi_err) != np.sign(self.prev_phi_err):
+ self.phi_I = 0
+ if wheels_cmd_exec[0] == 0 and wheels_cmd_exec[1] == 0:
+ self.d_I = 0
+ self.phi_I = 0
+
+ @staticmethod
+ def adjust_integral(error, integral, bounds, resolution):
+ """Bounds the integral error to avoid windup.
+
+ Adjusts the integral error to remain in defined bounds, and cancels it if the error is smaller than the
+ resolution of the error estimation.
+
+ Args:
+ error (:obj:`float`): current error value
+ integral (:obj:`float`): current integral value
+ bounds (:obj:`dict`): contains minimum and maximum value for the integral
+ resolution (:obj:`float`): resolution of the error estimate
+
+ Returns:
+ integral (:obj:`float`): adjusted integral value
+ """
+ if integral > bounds["top"]:
+ integral = bounds["top"]
+ elif integral < bounds["bot"]:
+ integral = bounds["bot"]
+ elif abs(error) < resolution:
+ integral = 0
+ return integral
+
diff --git a/packages/dt-core/packages/lane_control/src/lane_controller_node.py b/packages/dt-core/packages/lane_control/src/lane_controller_node.py
new file mode 100644
index 00000000..7bfe7c47
--- /dev/null
+++ b/packages/dt-core/packages/lane_control/src/lane_controller_node.py
@@ -0,0 +1,267 @@
+#!/usr/bin/env python3
+import numpy as np
+import rospy
+
+from duckietown.dtros import DTROS, NodeType, TopicType, DTParam, ParamType
+from duckietown_msgs.msg import Twist2DStamped, LanePose, WheelsCmdStamped, BoolStamped, FSMState, StopLineReading
+
+from lane_controller.controller import LaneController
+
+
+class LaneControllerNode(DTROS):
+ """Computes control action.
+ The node compute the commands in form of linear and angular velocities, by processing the estimate error in
+ lateral deviationa and heading.
+ The configuration parameters can be changed dynamically while the node is running via ``rosparam set`` commands.
+ Args:
+ node_name (:obj:`str`): a unique, descriptive name for the node that ROS will use
+ Configuration:
+ ~v_bar (:obj:`float`): Nominal velocity in m/s
+ ~k_d (:obj:`float`): Proportional term for lateral deviation
+ ~k_theta (:obj:`float`): Proportional term for heading deviation
+ ~k_Id (:obj:`float`): integral term for lateral deviation
+ ~k_Iphi (:obj:`float`): integral term for lateral deviation
+ ~d_thres (:obj:`float`): Maximum value for lateral error
+ ~theta_thres (:obj:`float`): Maximum value for heading error
+ ~d_offset (:obj:`float`): Goal offset from center of the lane
+ ~integral_bounds (:obj:`dict`): Bounds for integral term
+ ~d_resolution (:obj:`float`): Resolution of lateral position estimate
+ ~phi_resolution (:obj:`float`): Resolution of heading estimate
+ ~omega_ff (:obj:`float`): Feedforward part of controller
+ ~verbose (:obj:`bool`): Verbosity level (0,1,2)
+ ~stop_line_slowdown (:obj:`dict`): Start and end distances for slowdown at stop lines
+ Publisher:
+ ~car_cmd (:obj:`Twist2DStamped`): The computed control action
+ Subscribers:
+ ~lane_pose (:obj:`LanePose`): The lane pose estimate from the lane filter
+ ~intersection_navigation_pose (:obj:`LanePose`): The lane pose estimate from intersection navigation
+ ~wheels_cmd_executed (:obj:`WheelsCmdStamped`): Confirmation that the control action was executed
+ ~stop_line_reading (:obj:`StopLineReading`): Distance from stopline, to reduce speed
+ ~obstacle_distance_reading (:obj:`stop_line_reading`): Distancefrom obstacle virtual stopline, to reduce speed
+ """
+
+ 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 cbObstacleStopLineReading(self,msg):
+ """
+ Callback storing the current obstacle distance, if detected.
+ Args:
+ msg(:obj:`StopLineReading`): Message containing information about the virtual obstacle stopline.
+ """
+ self.obstacle_stop_line_distance = np.sqrt(msg.stop_line_point.x ** 2 + msg.stop_line_point.y ** 2)
+ self.obstacle_stop_line_detected = msg.stop_line_detected
+ self.at_stop_line = msg.at_stop_line
+
+ def cbStopLineReading(self, msg):
+ """Callback storing current distance to the next stopline, if one is detected.
+ Args:
+ msg (:obj:`StopLineReading`): Message containing information about the next stop line.
+ """
+ self.stop_line_distance = np.sqrt(msg.stop_line_point.x ** 2 + msg.stop_line_point.y ** 2)
+ self.stop_line_detected = msg.stop_line_detected
+ self.at_obstacle_stop_line = msg.at_stop_line
+
+ def cbMode(self, fsm_state_msg):
+
+ self.fsm_state = fsm_state_msg.state # String of current FSM state
+
+ if self.fsm_state == 'INTERSECTION_CONTROL':
+ self.current_pose_source = 'intersection_navigation'
+ else:
+ self.current_pose_source = 'lane_filter'
+
+ if self.params['~verbose'] == 2:
+ self.log("Pose source: %s" % self.current_pose_source)
+
+ def cbAllPoses(self, input_pose_msg, pose_source):
+ """Callback receiving pose messages from multiple topics.
+ If the source of the message corresponds with the current wanted pose source, it computes a control command.
+ Args:
+ input_pose_msg (:obj:`LanePose`): Message containing information about the current lane pose.
+ pose_source (:obj:`String`): Source of the message, specified in the subscriber.
+ """
+
+ if pose_source == self.current_pose_source:
+ self.pose_msg_dict[pose_source] = input_pose_msg
+
+ self.pose_msg = input_pose_msg
+
+ self.getControlAction(self.pose_msg)
+
+ def cbWheelsCmdExecuted(self, msg_wheels_cmd):
+ """Callback that reports if the requested control action was executed.
+ Args:
+ msg_wheels_cmd (:obj:`WheelsCmdStamped`): Executed wheel commands
+ """
+ self.wheels_cmd_executed = msg_wheels_cmd
+
+ def publishCmd(self, car_cmd_msg):
+ """Publishes a car command message.
+ Args:
+ car_cmd_msg (:obj:`Twist2DStamped`): Message containing the requested control action.
+ """
+ self.pub_car_cmd.publish(car_cmd_msg)
+
+ def getControlAction(self, pose_msg):
+ """Callback that receives a pose message and updates the related control command.
+ Using a controller object, computes the control action using the current pose estimate.
+ Args:
+ pose_msg (:obj:`LanePose`): Message containing information about the current lane pose.
+ """
+ current_s = rospy.Time.now().to_sec()
+ dt = None
+ if self.last_s is not None:
+ dt = (current_s - self.last_s)
+
+ if self.at_stop_line or self.at_obstacle_stop_line:
+ v = 0
+ omega = 0
+ else:
+
+ # Compute errors
+ d_err = pose_msg.d - self.params['~d_offset']
+ phi_err = pose_msg.phi
+
+ # We cap the error if it grows too large
+ if np.abs(d_err) > self.params['~d_thres']:
+ self.log("d_err too large, thresholding it!", 'error')
+ d_err = np.sign(d_err) * self.params['~d_thres']
+
+
+ wheels_cmd_exec = [self.wheels_cmd_executed.vel_left, self.wheels_cmd_executed.vel_right]
+ if self.obstacle_stop_line_detected:
+ v, omega = self.controller.compute_control_action(d_err, phi_err, dt, wheels_cmd_exec, self.obstacle_stop_line_distance)
+ #TODO: This is a temporarily fix to avoid vehicle image detection latency caused unable to stop in time.
+ v = v*0.25
+ omega = omega*0.25
+
+ else:
+ v, omega = self.controller.compute_control_action(d_err, phi_err, dt, wheels_cmd_exec, self.stop_line_distance)
+
+ # For feedforward action (i.e. during intersection navigation)
+ self.params['~omega_ff'] = rospy.get_param('~omega_ff', 0)
+ omega += self.params['~omega_ff']
+
+ # Initialize car control msg, add header from input message
+ car_control_msg = Twist2DStamped()
+ car_control_msg.header = pose_msg.header
+
+ # Add commands to car message
+ car_control_msg.v = v
+ car_control_msg.omega = omega
+
+ self.publishCmd(car_control_msg)
+ self.last_s = current_s
+
+ def cbParametersChanged(self):
+ """Updates parameters in the controller object."""
+
+ self.controller.update_parameters(self.params)
+
+
+if __name__ == "__main__":
+ # Initialize the node
+ lane_controller_node = LaneControllerNode(node_name='lane_controller_node')
+ # Keep it spinning
+ rospy.spin()
diff --git a/packages/dt-core/packages/lane_filter/config/lane_filter_node/default.yaml b/packages/dt-core/packages/lane_filter/config/lane_filter_node/default.yaml
new file mode 100644
index 00000000..cbc5b5e6
--- /dev/null
+++ b/packages/dt-core/packages/lane_filter/config/lane_filter_node/default.yaml
@@ -0,0 +1,23 @@
+use_propagation: True
+debug : False
+lane_filter_histogram_configuration:
+ mean_d_0: 0
+ mean_phi_0: 0
+ sigma_d_0: 0.1
+ sigma_phi_0: 0.1
+ delta_d: 0.02
+ delta_phi: 0.1
+ d_max: 0.3
+ d_min: -0.15
+ phi_min: -0.5
+ phi_max: 0.5
+ cov_v: 0.5
+ linewidth_white: 0.05
+ linewidth_yellow: 0.025
+ lanewidth: 0.23
+ min_max: 0.1
+ sigma_d_mask: 1.0
+ sigma_phi_mask: 2.0
+ range_min: 0.2
+ range_est: 0.33
+ range_max: 0.6
diff --git a/packages/dt-core/packages/led_pattern_switch/config/led_pattern_switch_node/default.yaml b/packages/dt-core/packages/led_pattern_switch/config/led_pattern_switch_node/default.yaml
new file mode 100644
index 00000000..4b3eb9d4
--- /dev/null
+++ b/packages/dt-core/packages/led_pattern_switch/config/led_pattern_switch_node/default.yaml
@@ -0,0 +1,18 @@
+mode_topic: "fsm_node/mode"
+source_topics: #Mapping from cmd source name to topics
+ coordination: "coordinator_node/change_color_pattern"
+ joystick: "led_joy_mapper_node/change_color_pattern"
+mappings: #Mapping from FSMStates.state to cmd source names. Allows different FSM mode to use the same source.
+ LANE_FOLLOWING: "joystick"
+ INTERSECTION_CONTROL: "coordination"
+ INTERSECTION_COORDINATION: "coordination"
+ COORDINATION: "coordination"
+ JOYSTICK_CONTROL: "joystick"
+ NORMAL_JOYSTICK_CONTROL: "joystick"
+ AVOID_OBSTACLE: "joystick"
+ AVOID_VEHICLE: "joystick"
+ LANE_RECOVERY: "joystick"
+ INTERSECTION_RECOVERY: "joystick"
+ PARALLEL_AUTONOMY: "coordination"
+ OBSTACLE_STOP: "coordination"
+ OBSTACLE_ALERT: "coordination"
diff --git a/packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml b/packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml
index 398c01db..3fa655ab 100644
--- a/packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml
+++ b/packages/dt-core/packages/line_detector/config/line_detector_node/default.yaml
@@ -1,44 +1,23 @@
img_size: [120,160]
top_cutoff: 40
-# should be an array of 2 elements. The first is the name of the class
-# and the second should be the parameters.
-# The class should be an abstract instance of LineDetectorInterface
-# CUSTOM CONFIG
+colors:
+ RED:
+ low_1: [0,30,100]
+ high_1: [17,255,255]
+ low_2: [165,40,100]
+ high_2: [180,255,255]
+ WHITE:
+ low: [41,0,100]
+ high: [164,100,255]
+ YELLOW:
+ low: [23,40,100]
+ high: [40,255,255]
-
-detector:
- - line_detector.LineDetectorHSV
- - configuration:
- dilation_kernel_size: 3
- canny_thresholds: [80,200]
- hough_threshold: 2
- hough_min_line_length: 3
- hough_max_line_gap: 1
-
- hsv_white1: [0,0,150]
- hsv_white2: [180,100,255]
- hsv_yellow1: [25,140,100]
- hsv_yellow2: [45,255,255]
- hsv_red1: [0,140,100]
- hsv_red2: [15,255,255]
- hsv_red3: [165,140,100]
- hsv_red4: [180,255,255]
-
-detector_intersection:
- - line_detector.LineDetectorHSV
- - configuration:
- dilation_kernel_size: 3
- canny_thresholds: [80,200]
- hough_threshold: 2
- hough_min_line_length: 3
- hough_max_line_gap: 1
-
- hsv_white1: [0,0,150]
- hsv_white2: [180,100,255]
- hsv_yellow1: [25,140,100]
- hsv_yellow2: [45,255,255]
- hsv_red1: [0,50,100]
- hsv_red2: [15,255,255]
- hsv_red3: [165,50,100]
- hsv_red4: [180,255,255]
+line_detector_parameters:
+ canny_thresholds: [80,200]
+ canny_aperture_size: 3
+ dilation_kernel_size: 3
+ hough_threshold: 2
+ hough_min_line_length: 3
+ hough_max_line_gap: 1
diff --git a/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py b/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py
new file mode 100644
index 00000000..1b967669
--- /dev/null
+++ b/packages/dt-core/packages/navigation/src/random_april_tag_turns_node.py
@@ -0,0 +1,110 @@
+#!/usr/bin/env python3
+
+import math
+
+import numpy
+
+import rospy
+from duckietown_msgs.msg import AprilTagsWithInfos, FSMState, TurnIDandType
+from std_msgs.msg import Int16 # Imports msg
+
+
+class RandomAprilTagTurnsNode:
+ def __init__(self):
+ # Save the name of the node
+ self.node_name = rospy.get_name()
+ self.turn_type = -1
+
+ rospy.loginfo(f"[{self.node_name}] Initializing.")
+
+ # Setup publishers
+ # self.pub_topic_a = rospy.Publisher("~topic_a",String, queue_size=1)
+ self.pub_turn_type = rospy.Publisher("~turn_type", Int16, queue_size=1, latch=True)
+ self.pub_id_and_type = rospy.Publisher("~turn_id_and_type", TurnIDandType, queue_size=1, latch=True)
+
+ # Setup subscribers
+ # self.sub_topic_b = rospy.Subscriber("~topic_b", String, self.cbTopic)
+ self.sub_topic_mode = rospy.Subscriber("~mode", FSMState, self.cbMode, queue_size=1)
+ # self.fsm_mode = None #TODO what is this?
+ self.sub_topic_tag = rospy.Subscriber("~tag", AprilTagsWithInfos, self.cbTag, queue_size=1)
+
+ # Read parameters
+ self.pub_timestep = self.setupParameter("~pub_timestep", 1.0)
+ # Create a timer that calls the cbTimer function every 1.0 second
+ # self.timer = rospy.Timer(rospy.Duration.from_sec(self.pub_timestep),self.cbTimer)
+
+ rospy.loginfo(f"[{self.node_name}] Initialzed.")
+
+ self.rate = rospy.Rate(30) # 10hz
+
+ def cbMode(self, mode_msg):
+ # print mode_msg
+ # TODO PUBLISH JUST ONCE
+ self.fsm_mode = mode_msg.state
+ if self.fsm_mode != mode_msg.INTERSECTION_CONTROL:
+ self.turn_type = -1
+ self.pub_turn_type.publish(self.turn_type)
+ # rospy.loginfo("Turn type now: %i" %(self.turn_type))
+
+ def cbTag(self, tag_msgs):
+ if (
+ self.fsm_mode == "INTERSECTION_CONTROL"
+ or self.fsm_mode == "INTERSECTION_COORDINATION"
+ or self.fsm_mode == "INTERSECTION_PLANNING"
+ ):
+ # loop through list of april tags
+
+ # filter out the nearest apriltag
+ dis_min = 999
+ idx_min = -1
+ for idx, taginfo in enumerate(tag_msgs.infos):
+ if taginfo.tag_type == taginfo.SIGN:
+ tag_det = (tag_msgs.detections)[idx]
+ pos = tag_det.transform.translation
+ distance = math.sqrt(pos.x ** 2 + pos.y ** 2 + pos.z ** 2)
+ if distance < dis_min:
+ dis_min = distance
+ idx_min = idx
+
+ if idx_min != -1:
+ taginfo = (tag_msgs.infos)[idx_min]
+ # go through possible intersection types
+ chosen_turn = self.get_chosen_turn()
+ rospy.loginfo("Turn type now: %i" % (chosen_turn))
+ # end of fix
+
+ self.turn_type = chosen_turn
+ self.pub_turn_type.publish(self.turn_type)
+
+ id_and_type_msg = TurnIDandType()
+ id_and_type_msg.tag_id = taginfo.id
+ id_and_type_msg.turn_type = self.turn_type
+ self.pub_id_and_type.publish(id_and_type_msg)
+
+ # rospy.loginfo("possible turns %s." %(availableTurns))
+ # rospy.loginfo("Turn type now: %i" %(self.turn_type))
+
+ def setupParameter(self, param_name, default_value):
+ value = rospy.get_param(param_name, default_value)
+ rospy.set_param(param_name, value) # Write to parameter server for transparancy
+ # rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
+ return value
+
+ def on_shutdown(self):
+ rospy.loginfo(f"[{self.node_name}] Shutting down.")
+
+ def get_chosen_turn(self): # stub
+ return 0
+
+
+if __name__ == "__main__":
+ # Initialize the node with rospy
+ rospy.init_node("random_april_tag_turns_node", anonymous=False)
+
+ # Create the NodeName object
+ node = RandomAprilTagTurnsNode()
+
+ # Setup proper shutdown behavior
+ rospy.on_shutdown(node.on_shutdown)
+ # Keep it spinning to keep the node alive
+ rospy.spin()
diff --git a/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml b/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml
index 4e2ab54b..7d4c7fa0 100644
--- a/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml
+++ b/packages/dt-core/packages/stop_line_filter/config/stop_line_filter_node/default.yaml
@@ -1,5 +1,4 @@
-#default parameters for stop_line_filter/stop_line_filter_node
-stop_distance: 0.15
+stop_distance: 0.13
min_segs: 6
-off_time: 2
+off_time: 16
max_y: 0.2
diff --git a/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py b/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py
new file mode 100644
index 00000000..3a6e2271
--- /dev/null
+++ b/packages/dt-core/packages/stop_line_filter/src/stop_line_filter_node.py
@@ -0,0 +1,163 @@
+#!/usr/bin/env python3
+import numpy as np
+
+import rospy
+from duckietown.dtros import DTParam, DTROS, NodeType, ParamType
+from duckietown_msgs.msg import BoolStamped, FSMState, LanePose, SegmentList, StopLineReading
+from geometry_msgs.msg import Point
+
+
+# import math
+
+
+class StopLineFilterNode(DTROS):
+ 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 setupParam(self,param_name,default_value):
+ # value = rospy.get_param(param_name,default_value)
+ # rospy.set_param(param_name,value) #Write to parameter server for transparancy
+ # rospy.loginfo("[%s] %s = %s " %(self.node_name,param_name,value))
+ # return value
+ #
+ # def updateParams(self,event):
+ # self.stop_distance = rospy.get_param("~stop_distance")
+ # self.min_segs = rospy.get_param("~min_segs")
+ # self.off_time = rospy.get_param("~off_time")
+ # self.max_y = rospy.get_param("~max_y")
+
+ def cb_state_change(self, msg):
+ # 3501 shit fix
+ #if (self.state == "INTERSECTION_CONTROL") and (msg.state == "LANE_FOLLOWING"):
+ # self.after_intersection_work()
+ self.loginfo("CB_STATE: {}".format(msg.state))
+ if msg.state == "INTERSECTION_CONTROL":
+ self.after_intersection_work()
+ self.state = msg.state
+
+ def after_intersection_work(self):
+ self.loginfo("Blocking stop line detection after the intersection")
+ stop_line_reading_msg = StopLineReading()
+ stop_line_reading_msg.stop_line_detected = False
+ stop_line_reading_msg.at_stop_line = False
+ self.pub_stop_line_reading.publish(stop_line_reading_msg)
+ self.sleep = True
+ rospy.sleep(self.off_time.value)
+ self.sleep = False
+ self.loginfo("Resuming stop line detection after the intersection")
+
+ # def cbSwitch(self, switch_msg):
+ # self.active = switch_msg.data
+ # if self.active and self.state == "INTERSECTION_CONTROL":
+ # self.after_intersection_work()
+ #
+ #
+
+ def cb_lane_pose(self, lane_pose_msg):
+ self.lane_pose = lane_pose_msg
+
+ def cb_segments(self, segment_list_msg):
+
+ if not self.switch or self.sleep:
+ # one more almost fix for 3501
+ stop_line_reading_msg = StopLineReading()
+ stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
+ stop_line_reading_msg.stop_line_detected = False
+ stop_line_reading_msg.at_stop_line = False
+ self.pub_stop_line_reading.publish(stop_line_reading_msg)
+ return
+
+ good_seg_count = 0
+ stop_line_x_accumulator = 0.0
+ stop_line_y_accumulator = 0.0
+ stop_line_min = 30.0
+ for segment in segment_list_msg.segments:
+ if segment.color != segment.RED:
+ continue
+ if segment.points[0].x < 0 or segment.points[1].x < 0: # the point is behind us
+ continue
+
+ p1_lane = self.to_lane_frame(segment.points[0])
+ p2_lane = self.to_lane_frame(segment.points[1])
+ avg_x = 0.5 * (p1_lane[0] + p2_lane[0])
+ avg_y = 0.5 * (p1_lane[1] + p2_lane[1])
+ if avg_x < stop_line_min:
+ stop_line_min = avg_x
+ stop_line_x_accumulator += avg_x
+ stop_line_y_accumulator += avg_y # TODO output covariance and not just mean
+ good_seg_count += 1.0
+# self.loginfo("stop_line_min_x = " + str (stop_line_min))
+ stop_line_reading_msg = StopLineReading()
+ stop_line_reading_msg.header.stamp = segment_list_msg.header.stamp
+ if good_seg_count < self.min_segs.value:
+ stop_line_reading_msg.stop_line_detected = False
+ stop_line_reading_msg.at_stop_line = False
+ self.pub_stop_line_reading.publish(stop_line_reading_msg)
+
+ # ### CRITICAL: publish false to at stop line output_topic
+ # msg = BoolStamped()
+ # msg.header.stamp = stop_line_reading_msg.header.stamp
+ # msg.data = False
+ # self.pub_at_stop_line.publish(msg)
+ # ### CRITICAL END
+
+ else:
+ stop_line_reading_msg.stop_line_detected = True
+ stop_line_point = Point()
+ stop_line_point.x = stop_line_min #stop_line_x_accumulator / good_seg_count
+ stop_line_point.y = stop_line_y_accumulator / good_seg_count
+ stop_line_reading_msg.stop_line_point = stop_line_point
+ # Only detect redline if y is within max_y distance:
+ stop_line_reading_msg.at_stop_line = (
+ stop_line_point.x < self.stop_distance.value and np.abs(stop_line_point.y) < self.max_y.value
+ )
+
+ self.pub_stop_line_reading.publish(stop_line_reading_msg)
+ if stop_line_reading_msg.at_stop_line:
+ msg = BoolStamped()
+ msg.header.stamp = stop_line_reading_msg.header.stamp
+ msg.data = True
+ self.pub_at_stop_line.publish(msg)
+
+ def to_lane_frame(self, point):
+ p_homo = np.array([point.x, point.y, 1])
+ phi = self.lane_pose.phi
+ d = self.lane_pose.d
+ T = np.array([[np.cos(phi), -np.sin(phi), 0], [np.sin(phi), np.cos(phi), d], [0, 0, 1]])
+ p_new_homo = T.dot(p_homo)
+ p_new = p_new_homo[0:2]
+ return p_new
+
+ # def onShutdown(self):
+ # rospy.loginfo("[StopLineFilterNode] Shutdown.")
+
+
+if __name__ == "__main__":
+ lane_filter_node = StopLineFilterNode(node_name="stop_line_filter")
+ rospy.spin()
diff --git a/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml b/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml
index 0575c538..c9813643 100644
--- a/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml
+++ b/packages/dt-core/packages/unicorn_intersection/config/unicorn_intersection_node/default.yaml
@@ -1,14 +1,15 @@
-time_left_turn: 7.5
-time_straight_turn: 6.5
+
+time_left_turn: 7.0
+time_straight_turn: 13
time_right_turn: 2.5
-ff_left: 0.1
+ff_left: 0.018
ff_straight: 0
-ff_right: -0.6
+ff_right: -0.061
-LFparams_left: {phi_max: -0.5, phi_min: -1.5, range_est: 0.4, red_to_white: true, use_yellow: false}
-LFparams_straight: {"red_to_white": false}
-LFparams_right: {phi_max: 2, phi_min: 0.5, range_est: 0.4, red_to_white: false}
+LFparams_left: {phi_max: 0.0, phi_min: -0.5, range_est: 0.3, red_to_white: true, use_yellow: true}
+LFparams_straight: {"red_to_white": True}
+LFparams_right: {phi_max: 0.27, phi_min: 0.0, range_est: 0.3, red_to_white: false}
omega_min_left: 0.2
omega_max_left: 0.6
diff --git a/packages/dt-core/packages/unicorn_intersection/src/unicorn_intersection_node.py b/packages/dt-core/packages/unicorn_intersection/src/unicorn_intersection_node.py
new file mode 100644
index 00000000..c25dd4a0
--- /dev/null
+++ b/packages/dt-core/packages/unicorn_intersection/src/unicorn_intersection_node.py
@@ -0,0 +1,175 @@
+#!/usr/bin/env python3
+import json
+
+import rospy
+from duckietown_msgs.msg import BoolStamped, FSMState, LanePose, TurnIDandType
+from std_msgs.msg import String
+
+
+class UnicornIntersectionNode:
+ def __init__(self):
+ self.node_name = "Unicorn Intersection Node"
+
+ ## setup Parameters
+ self.setupParams()
+
+ ## Internal variables
+ self.state = "JOYSTICK_CONTROL"
+ self.active = False
+ self.turn_type = -1
+ self.tag_id = -1
+ self.forward_pose = False
+
+ ## Subscribers
+ # self.sub_turn_type = rospy.Subscriber("~turn_type", Int16, self.cbTurnType)
+ self.sub_turn_type = rospy.Subscriber("~turn_id_and_type", TurnIDandType, self.cbTurnType)
+ self.sub_fsm = rospy.Subscriber("~fsm_state", FSMState, self.cbFSMState)
+ self.sub_int_go = rospy.Subscriber("~intersection_go", BoolStamped, self.cbIntersectionGo)
+ self.sub_lane_pose = rospy.Subscriber("~lane_pose_in", LanePose, self.cbLanePose)
+ self.sub_switch = rospy.Subscriber("~switch", BoolStamped, self.cbSwitch, queue_size=1)
+
+ ## Publisher
+ self.pub_int_done = rospy.Publisher("~intersection_done", BoolStamped, queue_size=1)
+ self.pub_LF_params = rospy.Publisher("~lane_filter_params", String, queue_size=1)
+ self.pub_lane_pose = rospy.Publisher("~lane_pose_out", LanePose, queue_size=1)
+ self.pub_int_done_detailed = rospy.Publisher(
+ "~intersection_done_detailed", TurnIDandType, queue_size=1
+ )
+
+ ## update Parameters timer
+ self.params_update = rospy.Timer(rospy.Duration.from_sec(1.0), self.updateParams)
+
+ def cbLanePose(self, msg):
+ if self.forward_pose:
+ self.pub_lane_pose.publish(msg)
+
+ def changeLFParams(self, params, reset_time):
+ data = {"params": params, "time": reset_time}
+ msg = String()
+ msg.data = json.dumps(data)
+ self.pub_LF_params.publish(msg)
+
+ def cbIntersectionGo(self, msg):
+ rospy.loginfo("[%s] Recieved intersection go message from coordinator", self.node_name)
+ if not self.active:
+ return
+
+ if not msg.data:
+ return
+
+ while self.turn_type == -1:
+ if not self.active:
+ return
+ rospy.loginfo(
+ "[%s] Requested to start intersection, but we do not see an april tag yet.", self.node_name
+ )
+ rospy.sleep(2)
+
+ tag_id = self.tag_id
+ turn_type = self.turn_type
+
+ sleeptimes = [self.time_left_turn, self.time_straight_turn, self.time_right_turn]
+ LFparams = [self.LFparams_left, self.LFparams_straight, self.LFparams_right]
+ omega_ffs = [self.ff_left, self.ff_straight, self.ff_right]
+ omega_maxs = [self.omega_max_left, self.omega_max_straight, self.omega_max_right]
+ omega_mins = [self.omega_min_left, self.omega_min_straight, self.omega_min_right]
+
+
+ rospy.set_param("~lane_controller/omega_ff", omega_ffs[turn_type])
+ rospy.set_param("~lane_controller/omega_max", omega_maxs[turn_type])
+ rospy.set_param("~lane_controller/omega_min", omega_mins[turn_type])
+ # Waiting for LF to adapt to new params
+ rospy.sleep(1)
+
+ rospy.loginfo("Starting intersection control - driving to " + str(turn_type))
+ self.forward_pose = True
+ self.changeLFParams(LFparams[turn_type], sleeptimes[turn_type] + 1.0)
+
+ rospy.sleep(sleeptimes[turn_type])
+
+ self.forward_pose = False
+ rospy.set_param("~lane_controller/omega_ff", 0)
+ rospy.set_param("~lane_controller/omega_max", 999)
+ rospy.set_param("~lane_controller/omega_min", -999)
+
+ # Publish intersection done
+ msg_done = BoolStamped()
+ msg_done.data = True
+ self.pub_int_done.publish(msg_done)
+
+ # Publish intersection done detailed
+ msg_done_detailed = TurnIDandType()
+ msg_done_detailed.tag_id = tag_id
+ msg_done_detailed.turn_type = turn_type
+ self.pub_int_done_detailed.publish(msg_done_detailed)
+
+ def cbFSMState(self, msg):
+ if self.state != msg.state and msg.state == "INTERSECTION_COORDINATION":
+ self.turn_type = -1
+
+ self.state = msg.state
+
+ def cbSwitch(self, switch_msg):
+ self.active = switch_msg.data
+
+ def cbTurnType(self, msg):
+ self.tag_id = msg.tag_id
+ if self.turn_type == -1:
+ self.turn_type = msg.turn_type
+ if self.debug_dir != -1:
+ self.turn_type = self.debug_dir
+
+ def setupParams(self):
+ self.time_left_turn = self.setupParam("~time_left_turn", 2)
+ self.time_straight_turn = self.setupParam("~time_straight_turn", 2)
+ self.time_right_turn = self.setupParam("~time_right_turn", 2)
+ self.ff_left = self.setupParam("~ff_left", 1.5)
+ self.ff_straight = self.setupParam("~ff_straight", 0)
+ self.ff_right = self.setupParam("~ff_right", -1)
+ self.LFparams_left = self.setupParam("~LFparams_left", 0)
+ self.LFparams_straight = self.setupParam("~LFparams_straight", 0)
+ self.LFparams_right = self.setupParam("~LFparams_right", 0)
+ self.omega_max_left = self.setupParam("~omega_max_left", 999)
+ self.omega_max_straight = self.setupParam("~omega_max_straight", 999)
+ self.omega_max_right = self.setupParam("~omega_max_right", 999)
+ self.omega_min_left = self.setupParam("~omega_min_left", -999)
+ self.omega_min_straight = self.setupParam("~omega_min_straight", -999)
+ self.omega_min_right = self.setupParam("~omega_min_right", -999)
+
+ self.debug_dir = self.setupParam("~debug_dir", -1)
+
+ def updateParams(self, event):
+ self.time_left_turn = rospy.get_param("~time_left_turn")
+ self.time_straight_turn = rospy.get_param("~time_straight_turn")
+ self.time_right_turn = rospy.get_param("~time_right_turn")
+ self.ff_left = rospy.get_param("~ff_left")
+ self.ff_straight = rospy.get_param("~ff_straight")
+ self.ff_right = rospy.get_param("~ff_right")
+ self.LFparams_left = rospy.get_param("~LFparams_left")
+ self.LFparams_straight = rospy.get_param("~LFparams_straight")
+ self.LFparams_right = rospy.get_param("~LFparams_right")
+ self.omega_max_left = rospy.get_param("~omega_max_left")
+ self.omega_max_straight = rospy.get_param("~omega_max_straight")
+ self.omega_max_right = rospy.get_param("~omega_max_right")
+ self.omega_min_left = rospy.get_param("~omega_min_left")
+ self.omega_min_straight = rospy.get_param("~omega_min_straight")
+ self.omega_min_right = rospy.get_param("~omega_min_right")
+
+ self.debug_dir = rospy.get_param("~debug_dir")
+
+ def setupParam(self, param_name, default_value):
+ value = rospy.get_param(param_name, default_value)
+ rospy.set_param(param_name, value) # Write to parameter server for transparancy
+ rospy.loginfo(f"[{self.node_name}] {param_name} = {value} ")
+ return value
+
+ def onShutdown(self):
+ rospy.loginfo("[UnicornIntersectionNode] Shutdown.")
+
+
+if __name__ == "__main__":
+ rospy.init_node("unicorn_intersection_node", anonymous=False)
+ unicorn_intersection_node = UnicornIntersectionNode()
+ rospy.on_shutdown(unicorn_intersection_node.onShutdown)
+ rospy.spin()
+
diff --git a/packages/dt-core/packages/vehicle_detection/launch/vehicle_detection_node.launch b/packages/dt-core/packages/vehicle_detection/launch/vehicle_detection_node.launch
new file mode 100644
index 00000000..3b490d5d
--- /dev/null
+++ b/packages/dt-core/packages/vehicle_detection/launch/vehicle_detection_node.launch
@@ -0,0 +1,17 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/packages/dt-core/packages/vehicle_detection/launch/vehicle_filter_node.launch b/packages/dt-core/packages/vehicle_detection/launch/vehicle_filter_node.launch
new file mode 100644
index 00000000..3ecbde8d
--- /dev/null
+++ b/packages/dt-core/packages/vehicle_detection/launch/vehicle_filter_node.launch
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/packages/dt-core/packages/vehicle_detection/src/vehicle_detection_node.py b/packages/dt-core/packages/vehicle_detection/src/vehicle_detection_node.py
new file mode 100644
index 00000000..335d2da5
--- /dev/null
+++ b/packages/dt-core/packages/vehicle_detection/src/vehicle_detection_node.py
@@ -0,0 +1,167 @@
+#!/usr/bin/env python3
+
+import cv2
+
+import rospy
+from cv_bridge import CvBridge
+from duckietown.dtros import DTParam, DTROS, NodeType, ParamType
+from duckietown_msgs.msg import BoolStamped, VehicleCorners
+from geometry_msgs.msg import Point32
+from sensor_msgs.msg import CompressedImage
+from std_msgs.msg import String
+
+class VehicleDetectionNode(DTROS):
+ """
+ This node detects if there is another Duckiebot in the image. This is done by recognizing the pattern of circles on
+ the back of every robot.
+ Args:
+ node_name (:obj:`str`): a unique, descriptive name for the node that ROS will use
+ Configuration:
+ ~process_frequency (:obj:`float`): Frequency at which to process the incoming images
+ ~circlepattern_dims (:obj:`list` of `int`): Number of dots in the pattern, two elements: [number of columns, number of rows]
+ ~blobdetector_min_area (:obj:`int`): Parameter for the blob detector, passed to `SimpleBlobDetector `_
+ ~blobdetector_min_dist_between_blobs (:obj:`str`): Parameter for the blob detector, passed to `SimpleBlobDetector `_
+ Subscriber:
+ ~image (:obj:`sensor_msgs.msg.CompressedImage`): Input image
+ Publishers:
+ ~centers (:obj:`duckietown_msgs.msg.VehicleCorners`): Detected pattern (if any)
+ ~debug/detection_image/compressed (:obj:`sensor_msgs.msg.CompressedImage`): Debug image that shows the detected pattern
+ ~detection (:obj:`boolStamped`): Vehicle Detection Flag
+ """
+
+ 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_not_detection = rospy.Publisher("small_msg_about_image_detection", String, 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.diff_vert = 40
+ self.diff_hor = 100
+ self.cutting_image_flag = False
+ self.x0, self.x1, self.y0, self.y1 = 0, 0, 0, 0
+ self.log("Initialization completed.")
+
+ def cbParametersChanged(self):
+
+ # TODO: THIS DOESN'T WORK WITH THE NEW DTROS!!!
+ self.publish_duration = rospy.Duration.from_sec(1.0 / self.process_frequency.value)
+ params = cv2.SimpleBlobDetector_Params()
+ params.minArea = self.blobdetector_min_area.value
+ params.minDistBetweenBlobs = self.blobdetector_min_dist_between_blobs.value
+ self.simple_blob_detector = cv2.SimpleBlobDetector_create(params)
+
+
+ def detect_bot(self, header, image_cv):
+ # if the pattern is detected, cv2.findCirclesGrid returns a non-zero result, otherwise it returns 0
+ # vehicle_detected_msg_out.data = detection > 0
+ # self.pub_detection.publish(vehicle_detected_msg_out)
+ self.vehicle_centers_msg_out = VehicleCorners()
+ self.detection_flag_msg_out = BoolStamped()
+ (detection, centers) = cv2.findCirclesGrid(
+ image_cv,
+ patternSize=tuple(self.circlepattern_dims.value),
+ flags=cv2.CALIB_CB_SYMMETRIC_GRID,
+ blobDetector=self.simple_blob_detector,
+ )
+ self.vehicle_centers_msg_out.header = header
+ self.vehicle_centers_msg_out.detection.data = detection > 0
+ self.detection_flag_msg_out.header = header
+ self.detection_flag_msg_out.data = detection > 0
+ # if the detection is successful add the information about it,
+ # otherwise publish a message saying that it was unsuccessful
+ return detection, centers
+
+ def set_detection_params(self, centers):
+ points_list = []
+ for point in centers:
+ center = Point32()
+ center.x = point[0, 0]
+ center.y = point[0, 1]
+ center.z = 0
+ points_list.append(center)
+ self.vehicle_centers_msg_out.corners = points_list
+ self.vehicle_centers_msg_out.H = self.circlepattern_dims.value[1]
+ self.vehicle_centers_msg_out.W = self.circlepattern_dims.value[0]
+
+ def save_rect_detection(self, centers):
+ self.y0, self.x0 = int(centers[0][0]), int(centers[0][1])
+ self.y1, self.x1 = int(centers[-1][0]), int(centers[-1][1])
+
+ def detection(self, header, image_cv):
+ rospy.loginfo('starting detection, it is cutting img {}'.format(self.cutting_image_flag))
+ if self.cutting_image_flag:
+ if not (self.x1 and self.y1):
+ self.cutting_image_flag = False
+ return False, False
+ else:
+ image_cv_cutted = image_cv[self.x0-self.diff_vert:self.x1+self.diff_vert, self.y0-self.diff_hor:self.y1+self.diff_hor]
+ detection, centers = self.detect_bot(header, image_cv_cutted)
+ rospy.loginfo("cutting img now")
+ else:
+ detection, centers = self.detect_bot(header, image_cv)
+ rospy.loginfo("full img now")
+ if detection > 0:
+ rospy.loginfo("detection!")
+ self.set_detection_params(centers)
+ self.save_rect_detection(centers[0])
+ else:
+ rospy.loginfo("There is no detection, change the way of detection")
+ self.cutting_image_flag = not self.cutting_image_flag
+ return centers, detection
+
+ def cb_image(self, image_msg):
+ """
+ Callback for processing a image which potentially contains a back pattern. Processes the image only if
+ sufficient time has passed since processing the previous image (relative to the chosen processing frequency).
+ The pattern detection is performed using OpenCV's `findCirclesGrid `_ function.
+ Args:
+ image_msg (:obj:`sensor_msgs.msg.CompressedImage`): Input image
+ """
+ now = rospy.Time.now()
+ if now - self.last_stamp < self.publish_duration:
+ return
+ else:
+ self.last_stamp = now
+ image_cv = self.bridge.compressed_imgmsg_to_cv2(image_msg, "bgr8")
+ centers, detection = self.detection(image_msg.header, image_cv)
+ if not detection:
+ centers, detection = self.detection(image_msg.header, image_cv)
+ self.pub_centers.publish(self.vehicle_centers_msg_out)
+ self.pub_detection_flag.publish(self.detection_flag_msg_out)
+ if self.pub_circlepattern_image.get_num_connections() > 0:
+ cv2.drawChessboardCorners(image_cv, tuple(self.circlepattern_dims.value), centers, detection)
+ image_msg_out = self.bridge.cv2_to_compressed_imgmsg(image_cv)
+ self.pub_circlepattern_image.publish(image_msg_out)
+
+
+
+
+if __name__ == "__main__":
+ vehicle_detection_node = VehicleDetectionNode("vehicle_detection")
+ rospy.spin()
\ No newline at end of file