Skip to content

Commit

Permalink
Merge pull request #9 from JASS-2021/subscribe_to_fsm_state_topic_and…
Browse files Browse the repository at this point in the history 
…_control_case

add fms subscriber and timer
  • Loading branch information
@pro100kot
pro100kot authored Apr 1, 2021
2 parents d5d75ff + cf11291 commit 97bf467
Show file tree
Hide file tree
Showing 2 changed files with 102 additions and 44 deletions.
16 changes: 16 additions & 0 deletions packages/dt-core/packages/apriltag/launch/apriltag_postprocessing_node.launch
View file
Original file line number Diff line number Diff line change
@@ -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>
130 changes: 86 additions & 44 deletions packages/dt-core/packages/apriltag/src/apriltag_postprocessing_node.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,8 @@
TagInfo, \
BoolStamped, \
AprilTagDetection, \
AprilTagDetectionArray
AprilTagDetectionArray, \
FSMState
import numpy as np
import tf.transformations as tr
from geometry_msgs.msg import PoseStamped, Pose
Expand All @@ -16,10 +17,14 @@

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)
Expand All @@ -33,69 +38,102 @@ def __init__(self):

tags_filepath = self.setupParam("~tags_file")

self.loc = self.setupParam("~loc", -1) # -1 if no location is given
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}
"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.callback, queue_size=1)
self.pub_postPros = rospy.Publisher("~apriltags_out", AprilTagsWithInfos, queue_size=1)
"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 callback(self, msg):

def fake_tag(self):
tags_msg = AprilTagDetectionArray()
tags_msg.header.stamp = rospy.Time.now()
tags_msg.header.frame_id = ""
tag = AprilTagDetection(
tag_id=58,
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
# 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)
# 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:
Expand All @@ -116,7 +154,10 @@ def callback(self, msg):
# 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):
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
Expand All @@ -139,23 +180,23 @@ def callback(self, msg):
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
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')
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')
tagzout_T_tagxout = tr.euler_matrix(-np.pi / 2, 0, np.pi / 2, 'rxyz')

#Load translation
# 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_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)

Expand All @@ -174,6 +215,7 @@ def callback(self, msg):
self.pub_postPros.publish(new_tag_data)



if __name__ == '__main__':
rospy.init_node('AprilPostPros', anonymous=False)
node = AprilPostPros()
Expand Down

0 comments on commit 97bf467

Please sign in to comment.