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