Skip to content

Commit

Permalink
Merge pull request #5 from JASS-2021/issue_4_fake_publisher
Browse files Browse the repository at this point in the history
fake publisher
  • Loading branch information
pro100kot authored Apr 1, 2021
2 parents 1571efc + b49d739 commit cf11291
Showing 1 changed file with 13 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,7 @@ def __init__(self):

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):
Expand All @@ -101,12 +102,24 @@ def callbackTimer(self, timer):
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=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()
Expand Down

0 comments on commit cf11291

Please sign in to comment.