def test_adding_duckietown_tag_info(self):
self.setup() # Setup the node
# Publish a tag with id=1
tags_msg = AprilTagDetectionArray()
tag = AprilTagDetection()
tag.pose.pose.orientation.w = 1
tags_msg.detections.append(tag)
tag.id = 1
self.pub.publish(tags_msg)
# Wait for the message to be received
timeout = rospy.Time.now() + rospy.Duration(5) # Wait at most 5 seconds for the node to reply
while not self.msg_received and not rospy.is_shutdown() and rospy.Time.now() < timeout:
rospy.sleep(0.1)
self.assertLess(rospy.Time.now(), timeout, "Waiting for apriltag detection timed out.")
# Check that the result has the same id
tag_in = self.tags_msg.detections[0]
self.assertEqual(tag_in.id, 1)
# Check that the result is a stop sign
tag_info = self.tags_msg.infos[0]
self.assertEqual(tag_info.tag_type, TagInfo.SIGN)
self.assertEqual(tag_info.traffic_sign_type, TagInfo.STOP)
def talker(self):
pub = rospy.Publisher('tag_detections_merged', AprilTagDetectionArray, queue_size=10)
rate = rospy.Rate(1) # 10hz
while not rospy.is_shutdown():
detection_array = AprilTagDetectionArray()
for tag in self.tag_list:
detections_from_cameras = []
for topic in self.camera_data.keys():
if (tag in self.camera_data[topic].keys()):
if (self.camera_data[topic][tag] != None):
detections_from_cameras.append(self.camera_data[topic][tag])
self.camera_data[topic][tag] = None
if (len(detections_from_cameras)>0):
merged_detection = AprilTagDetection()
merged_detection.id = tag
merged_detection.size = detections_from_cameras[0].size
merged_detection.pose.header = detections_from_cameras[0].pose.header
pose_list = [d.pose.pose for d in detections_from_cameras]
merged_detection.pose.pose = self.averagePose (pose_list)
detection_array.detections.append(merged_detection)
pub.publish(detection_array)
rate.sleep()
def test_transform_to_duckiebot_frame(self):
self.setup() # Setup the node
# Publish a point that should lie on the floor in front of duckiebot
scale_z = rospy.get_param("apriltags_postprocessing_node/scale_z")
camera_theta = rospy.get_param("apriltags_postprocessing_node/camera_theta")
camera_z = rospy.get_param("apriltags_postprocessing_node/camera_z")
z = camera_z/(math.sin(camera_theta*math.pi/180.0)*scale_z) # Find the distance to floor in camera frame
tags_msg = AprilTagDetectionArray()
tag = AprilTagDetection()
tag.pose.pose.position.z = z
tag.pose.pose.orientation.w = 1
tags_msg.detections.append(tag)
tag.id = 1
self.pub.publish(tags_msg)
# Wait for the message to be received
timeout = rospy.Time.now() + rospy.Duration(5) # Wait at most 5 seconds for the node to reply
while not self.msg_received and not rospy.is_shutdown() and rospy.Time.now() < timeout:
rospy.sleep(0.1)
self.assertLess(rospy.Time.now(), timeout, "Waiting for apriltag detection timed out.")
# Check that the result has the same id
tag_in = self.tags_msg.detections[0]
self.assertEqual(tag_in.id, 1)
# Check that the result is on the floor (z=0)
self.assertAlmostEqual(tag_in.pose.pose.position.z, 0)
def test_adding_duckietown_tag_info(self):
self.setup() # Setup the node
# Publish a tag with id=1
tags_msg = AprilTagDetectionArray()
tag = AprilTagDetection()
tag.pose.pose.orientation.w = 1
tags_msg.detections.append(tag)
tag.id = 1
self.pub.publish(tags_msg)
# Wait for the message to be received
timeout = rospy.Time.now() + rospy.Duration(
5) # Wait at most 5 seconds for the node to reply
while not self.msg_received and not rospy.is_shutdown(
) and rospy.Time.now() < timeout:
rospy.sleep(0.1)
self.assertLess(rospy.Time.now(), timeout,
"Waiting for apriltag detection timed out.")
# Check that the result has the same id
tag_in = self.tags_msg.detections[0]
self.assertEqual(tag_in.id, 1)
# Check that the result is a stop sign
tag_info = self.tags_msg.infos[0]
self.assertEqual(tag_info.tag_type, TagInfo.SIGN)
self.assertEqual(tag_info.traffic_sign_type, TagInfo.STOP)
def test_transform_to_duckiebot_frame(self):
self.setup() # Setup the node
# Publish a point that should lie on the floor in front of duckiebot
scale_z = rospy.get_param("apriltags_postprocessing_node/scale_z")
camera_theta = rospy.get_param(
"apriltags_postprocessing_node/camera_theta")
camera_z = rospy.get_param("apriltags_postprocessing_node/camera_z")
z = camera_z / (math.sin(camera_theta * math.pi / 180.0) * scale_z
) # Find the distance to floor in camera frame
tags_msg = AprilTagDetectionArray()
tag = AprilTagDetection()
tag.pose.pose.position.z = z
tag.pose.pose.orientation.w = 1
tags_msg.detections.append(tag)
tag.id = 1
self.pub.publish(tags_msg)
# Wait for the message to be received
timeout = rospy.Time.now() + rospy.Duration(
5) # Wait at most 5 seconds for the node to reply
while not self.msg_received and not rospy.is_shutdown(
) and rospy.Time.now() < timeout:
rospy.sleep(0.1)
self.assertLess(rospy.Time.now(), timeout,
"Waiting for apriltag detection timed out.")
# Check that the result has the same id
tag_in = self.tags_msg.detections[0]
self.assertEqual(tag_in.id, 1)
# Check that the result is on the floor (z=0)
self.assertAlmostEqual(tag_in.pose.pose.position.z, 0)
def maketag(
xIn, yIn, zIn
): #to use, do something like tagCoords = getAprilTagDetection(arguments here); myTag = maketag(tagCoords[0], tagCoords[1], tagCoords[2])
h = std_msgs.msg.Header() #std_msgs.msg.
h.stamp = rospy.Time.now(
) # Note you need to call rospy.init_node() before this will work
p = Point() #geometry_msgs.
p.x = xIn
p.y = yIn
p.z = zIn
q = Quaternion()
q.x = 0
q.y = 0
q.z = 0
q.w = 0
poseU = Pose(p, q)
poseS = PoseStamped(h, poseU)
tag = AprilTagDetection()
tag.size = 0.163513 #default from the launch file
tag.id = 1 #the tad number I believe
tag.pose = poseS
atda = AprilTagDetectionArray()
atda.detections = [tag]
return atda
def process_tag_pose(self):
''' Get the position and orientation of detected april tags from the iOS device. '''
for tags in self.april_tags:
ar = tags.split(",")
current_tag = AprilTagDetection()
current_tag.id = int(ar[0])
current_tag.size = 0.165
current_tag.pose.header.stamp = rospy.Time(float(self.ios_clock_offset) + float(ar[17]))
current_tag.pose.header.frame_id = "camera"
tag = [float(x) for x in ar[1:17]]
mat = np.matrix([tag[0:4], tag[4:8], tag[8:12], tag[12:16]])
new_mat = mat.A
trans = translation_from_matrix(new_mat)
quat = quaternion_from_matrix(new_mat)
current_tag.pose.pose.position.x = trans[0]
current_tag.pose.pose.position.y = trans[1]
current_tag.pose.pose.position.z = trans[2]
current_tag.pose.pose.orientation.x = quat[0]
current_tag.pose.pose.orientation.y = quat[1]
current_tag.pose.pose.orientation.z = quat[2]
current_tag.pose.pose.orientation.w = quat[3]
self.msg.detections.append(current_tag)
def maketagSpecifyHeader(x, y, z, h):
p = Point() #geometry_msgs.
p.x = xIn
p.y = yIn
p.z = zIn
q = Quaternion()
q.x = 0
q.y = 0
q.z = 0
q.w = 0
poseU = Pose(p, q)
poseS = PoseStamped(h, poseU)
tag = AprilTagDetection()
tag.size = 0.163513 #default from the launch file
tad.id = 1 #the tad number I believe
tag.pose = poseS
atda = AprilTagDetectionArray()
atda.detections = [tag]
return atda
# set the *_max values below to control max values
# currenty left and right are scaled.
# lights and vertical are the actual values used
left_motor_max = 10.0
right_motor_max = 10.0
lights_max = 80.0
vertical_motor_max = 70.0
current_time = 14.8
command_time = 0.9
left_motor_cmd = 0
right_motor_cmd = 0
vertical_motor_cmd = 0
lights_max_cmd = 0
last_tag_msg = AprilTagDetection()
last_robot_msg = RobotStatus()
heading_setpoint = 0
vert_setpoint = 0
forward_setpoint = 0
vert_kP = vertical_motor_max / 2.0 # max vertical thrust when 2 meters above / below
forward_kP = left_motor_max / 5.0 # max forward thrust when 5 meters away
heading_kP = left_motor_max / (3.14) # max turning when 180 deg off
def store_tag_info(data):
global last_tag_msg
last_tag_msg = data
set_setpoints()
