Add detect another bot

packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch

@@ -0,0 +1,16 @@
+<launch>
+  <arg name="pkg_name" value="apriltag"/>
+  <arg name="node_name" default="apriltag_postprocessing_node"/>
+  <arg name="veh" doc="Name of vehicle. ex: megaman"/>
+  <arg name="config" default="baseline" doc="Specify a config."/>
+  <arg name="param_file_name" default="default" doc="Specify a param file. ex:megaman."/>
+
+  <group ns="$(arg veh)">
+    <node name="$(arg node_name)" pkg="$(arg pkg_name)" type="$(arg node_name).py" output="screen">
+      <remap from="apriltag_postprocessing_node/fsm_state" to="fsm_node/mode"/>
+        <rosparam command="load"
+          file="$(find apriltag)/config/$(arg node_name)/$(arg param_file_name).yaml"/>
+        <param name="tags_file" value="$(find apriltag)/config/apriltagsDB.yaml" />
+    </node>
+  </group>
+</launch>

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

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