├── .gitignore ├── docs └── imgs │ ├── ros_logo.png │ ├── image_view_example.png │ ├── rostopic_echo_result.png │ └── rostopic_list_result.png ├── config ├── costmap_common_params.yaml ├── global_costmap_params.yaml ├── local_costmap_params.yaml ├── base_local_planner_params.yaml ├── dwa_local_planner_params.yaml ├── tag_poses_maze_3.yaml ├── robot_pose.rviz ├── ar_tag_navigation.rviz └── tag_sizes.yaml ├── launch ├── static_transforms.launch ├── robot_pose.launch ├── rviz.launch ├── apriltag_detector.launch └── move_base.launch ├── LICENSE ├── start_ground_truth_demo.sh ├── scripts ├── static_transform_broadcaster.py └── robot_pose.py ├── start_demo.sh ├── package.xml ├── start_nav_demo.sh ├── README.md └── CMakeLists.txt /.gitignore: -------------------------------------------------------------------------------- 1 | *.log 2 | -------------------------------------------------------------------------------- /docs/imgs/ros_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rfzeg/apriltag_robot_pose/HEAD/docs/imgs/ros_logo.png -------------------------------------------------------------------------------- /docs/imgs/image_view_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rfzeg/apriltag_robot_pose/HEAD/docs/imgs/image_view_example.png -------------------------------------------------------------------------------- /docs/imgs/rostopic_echo_result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rfzeg/apriltag_robot_pose/HEAD/docs/imgs/rostopic_echo_result.png -------------------------------------------------------------------------------- /docs/imgs/rostopic_list_result.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rfzeg/apriltag_robot_pose/HEAD/docs/imgs/rostopic_list_result.png -------------------------------------------------------------------------------- /config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | map_type: costmap 2 | 3 | obstacle_range: 4.0 4 | raytrace_range: 5.0 5 | 6 | transform_tolerance: 1.0 7 | 8 | #observation_sources: laser_scan_sensor 9 | 10 | #laser_scan_sensor: { 11 | # sensor_frame: hokuyo, 12 | # data_type: LaserScan, 13 | # topic: /udacity_bot/laser/scan, 14 | # marking: true, 15 | # clearing: true 16 | #} 17 | -------------------------------------------------------------------------------- /launch/static_transforms.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Roberto Zegers R. 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /launch/robot_pose.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | # Independent settings for the global planner's costmap 2 | global_costmap: 3 | global_frame: map # sets what coordinate frame the costmap should run in 4 | robot_base_frame: robot_footprint 5 | update_frequency: 2.0 # the Hz at which the costmap will run its update loop 6 | publish_frequency: 1.0 # the Hz at which the costmap will publish visualization information 7 | width: 20.0 8 | height: 20.0 9 | resolution: 0.05 10 | static_map: true # determines whether or not to load an existent map provided by the map_server 11 | rolling_window: false 12 | 13 | inflation_radius: 0.4 # max. distance from an obstacle at which costs are incurred for planning paths 14 | cost_scaling_factor: 4.0 # exponential rate at which the obstacle cost drops off, setting it higher will make the decay curve more steep 15 | 16 | # define the coordinates of the robot outline 17 | footprint: [[0.25,0.16],[0.25,-0.16],[-0.25,-0.16],[-0.25,0.16]] 18 | #robot_radius: 0.4 19 | footprint_padding: 0.1 20 | 21 | # If no plugins parameters are provided the initialization code will assume that the configuration is pre-Hydro and will load a default set of plugins with default namespaces 22 | plugins: 23 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 24 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 25 | -------------------------------------------------------------------------------- /start_ground_truth_demo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "" 4 | echo "ROS Robot Pose Ground Truth Demo" 5 | echo "" 6 | # The -e option to xterm is useful for running a single command and then exiting (or closing xterm after a Ctrl+C) 7 | 8 | # Fire up Gazebo world with maze environment 9 | xterm -e "roslaunch plywood_mazes maze_3_6x6.launch" & 10 | sleep 6 11 | 12 | # Prompt for a key press to continue after Gazebo has loaded 13 | read -n 1 -r -s -p "Press any key to continue once Gazebo has loaded or Ctrl+C to abort..." 14 | echo "" 15 | 16 | # Load robot description to parameter server and spawn a robot 17 | xterm -e "roslaunch rtab_dumpster spawn_rtab_dumpster.launch diff_drive_publishTf:=false diff_drive_publishOdomTF:=false" & 18 | sleep 4 19 | 20 | # Get ground truth odometry from Gazebo and publish as odom msg 21 | xterm -e "roslaunch noisy_odometry odom_msg_from_gazebo.launch" & 22 | sleep 4 23 | 24 | # Broadcast odom w.r.t. map Tf from odom msg 25 | xterm -e "roslaunch noisy_odometry odom_msg_to_map_tf.launch" & 26 | sleep 4 27 | 28 | # Allow for Rviz choice 29 | echo "" 30 | read -p "Do you want to start RVIZ with a preconfigured view (y/n): " input_choice 31 | 32 | if [ "$input_choice" = "y" ] 33 | then 34 | # Start RVIZ 35 | xterm -e "roslaunch apriltag_robot_pose rviz.launch" & 36 | sleep 4 37 | # Load pre-made map using map server 38 | xterm -e "roslaunch plywood_mazes map_server_maze_3.launch" 39 | elif [ "$input_choice" = "n" ] 40 | then 41 | echo "" 42 | echo "Rviz *NOT* started!" 43 | echo "" 44 | else 45 | echo "" 46 | echo "Warning: Not an acceptable option. Choose (y/n)" 47 | echo "" 48 | fi 49 | -------------------------------------------------------------------------------- /launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /launch/apriltag_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /scripts/static_transform_broadcaster.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # Broadcasts static transforms. Used here to publish multiple map frame to tag frame transforms. 4 | # Author: Roberto Zegers R. 5 | # Date: 2019 June 6 | 7 | import rospy 8 | import tf_conversions # because of transformations 9 | import tf2_ros 10 | import geometry_msgs.msg 11 | import yaml 12 | 13 | def broadcast_pose(tag_info): 14 | br = tf2_ros.TransformBroadcaster() 15 | 16 | for tag_id, tf_data in tag_info.iteritems(): 17 | t = geometry_msgs.msg.TransformStamped() 18 | t.header.stamp = rospy.Time.now() 19 | t.header.frame_id = "map" 20 | t.child_frame_id = "tag_" + str(tag_id) 21 | t.transform.translation.x = tf_data[0] 22 | t.transform.translation.y = tf_data[1] 23 | t.transform.translation.z = tf_data[2] 24 | q = tf_conversions.transformations.quaternion_from_euler(tf_data[3], tf_data[4], tf_data[5]) 25 | t.transform.rotation.x = q[0] 26 | t.transform.rotation.y = q[1] 27 | t.transform.rotation.z = q[2] 28 | t.transform.rotation.w = q[3] 29 | br.sendTransform(t) 30 | 31 | if __name__ == '__main__': 32 | rospy.init_node('tag_broadcaster') 33 | rospy.loginfo("\n Initialized static transform broadcaster \n") 34 | 35 | ## Load tag_info parameters from yaml file 36 | # Get a parameter from our private namespace 37 | param_path = rospy.get_param("~tag_poses_param_path") 38 | rospy.loginfo("Tag poses broadcasted: %s", param_path) 39 | f = open(param_path, 'r') 40 | params_raw = f.read() 41 | f.close() 42 | 43 | params = yaml.load(params_raw) 44 | # Dictionary with tag poses: [x, y, z, Roll, Pitch, Yaw] 45 | tag_info = params['tag_info'] 46 | #rospy.loginfo("Tag info dict: %s", tag_info) 47 | 48 | rate = rospy.Rate(10) # 10hz or 100 ms 49 | 50 | while not rospy.is_shutdown(): 51 | broadcast_pose(tag_info) 52 | rate.sleep() 53 | -------------------------------------------------------------------------------- /config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | # Independent settings for the local costmap 2 | local_costmap: 3 | global_frame: map # sets what coordinate frame the costmap should run in 4 | robot_base_frame: robot_footprint 5 | update_frequency: 2.0 # the Hz at which the costmap will run its update loop 6 | publish_frequency: 1.0 # the Hz at which the costmap will publish visualization information 7 | width: 5.0 # Width of the local costmap in meters (default: 10) 8 | height: 5.0 # Height of the local costmap in meters (default: 10) 9 | resolution: 0.05 # Resolution of the map in meters per cell 10 | 11 | inflation_radius: 0.4 # max. distance from an obstacle at which costs are incurred for planning paths 12 | cost_scaling_factor: 0.4 # exponential rate at which the obstacle cost drops off 13 | 14 | # define the coordinates of the robot outline 15 | footprint: [[0.25,0.16],[0.25,-0.16],[-0.25,-0.16],[-0.25,0.16]] 16 | #robot_radius: 0.4 17 | footprint_padding: 0.0 # lower than the padding used by the global planner 18 | 19 | publish_cost_grid: false # to visualize the cost function produced by the local planner in rviz (for debugging) 20 | 21 | # Use a static local costmap when generating local plans without any obstacle detection sensors 22 | static_map: true # determines whether or not the costmap should initialize itself based on a map served by the map_server 23 | rolling_window: false # true means the costmap will remain centered around the robot as the robot moves through the world 24 | #origin_x: -2.5 25 | #origin_y: -2.5 26 | # If no plugins parameters are provided the initialization code will assume that the configuration is pre-Hydro and will load a default set of plugins with default namespaces 27 | plugins: 28 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 29 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 30 | # - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 31 | -------------------------------------------------------------------------------- /launch/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /config/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | # move_base parameters 2 | controller_frequency: 3.0 # rate at which move_base will be called in Hz (default: 20.0) 3 | planner_frequency: 0.5 # rate at which re-calculate the global path, in Hz (default: 0.0) 4 | 5 | TrajectoryPlannerROS: 6 | 7 | holonomic_robot: false 8 | 9 | # Parameters for setting the velocity limits of the robot 10 | max_vel_x: 0.3 # max fwd velocity allowed for the base in mt/s (default: 0.5) 11 | min_vel_x: 0.05 # min fwd velocity allowed, should be high enough to overcome friction (default: 0.1) 12 | 13 | # Parameters for evaluating possible local planner trajectories 14 | pdist_scale: 3.0 # weighting for how much the local path should stay close to the global path (default: 0.6) 15 | gdist_scale: 1.0 # weighting for how much the controller should attempt to reach its local goal, also controls speed (default 0.8) 16 | occdist_scale: 0.2 # weighting for how much the controller should attempt to avoid obstacles (default 0.01) 17 | meter_scoring: true # whether or not assume that goal_distance and path_distance are expressed in meters (default: false) 18 | 19 | heading_scoring: false # Whether to score based on the robot's heading to the path or its distance from the path (default: false) 20 | 21 | # Foward simulation parameters 22 | # Parameters that determine how far in advance and with what granularity trajectories are simulated 23 | sim_time: 3.5 # The amount of time (in sec) to forward-simulate trajectories, a higher value can result in slightly smoother trajectories (default: 0.025) 24 | sim_granularity: 0.025 # The step size, in meters, to take between points on a given trajectory (default: 0.025) 25 | # angular_sim_granularity: 0.5235987756 # The step size, in radians, to take between angular samples on a given trajectory 26 | 27 | # 0.025 rad 1.4323944878 deg 28 | # 0.0872664626 rad 5 deg 29 | # 0.1745329252 rad 10 deg 30 | # 0.2617993878 rad 15 deg 31 | # 0.5235987756 rad 30 deg 32 | # 0.7853981634 rad 45 deg 33 | # 1.0471975512 rad 60 deg 34 | # 1.5707963268 rad 90 deg 35 | 36 | vx_samples: 20.0 # The number of samples to use when exploring the x velocity space (int, default: 3) 37 | vtheta_samples: 40 # The number of samples to use when exploring the theta velocity space (int, default: 20) 38 | 39 | # Goal Tolerance Parameters 40 | yaw_goal_tolerance: 0.2617993878 # default is 0.05 rad (~3 degrees) 41 | xy_goal_tolerance: 0.2 # in meters, (default: 0.1) 42 | latch_xy_goal_tolerance: true # (default: false) if the goal position is reached it is considered permanently 43 | 44 | controller_frequency: 10.0 # rate at which TrajectoryPlannerROS will be called in Hz 45 | 46 | # debug parameters 47 | publish_cost_grid_pc: false 48 | -------------------------------------------------------------------------------- /start_demo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "" 4 | echo "ROS AR Tag Robot Pose Estimator Demo" 5 | echo "" 6 | # The -e option to xterm is useful for running a single command and then exiting (or closing xterm after a Ctrl+C) 7 | 8 | # Fire up Gazebo world with maze environment 9 | xterm -e "roslaunch plywood_mazes maze_3_6x6.launch" & 10 | sleep 6 11 | 12 | # Prompt for a key press to continue after Gazebo has loaded 13 | #read -n 1 -r -s -p "Press any key to continue once Gazebo has loaded or Ctrl+C to abort..." 14 | #echo "" 15 | 16 | # Allow for noisy odometry choice 17 | echo "" 18 | read -p "After Gazebo has loaded, do you want to start a noisy odometry node? (y/n): " input_choice_1 19 | 20 | if [ "$input_choice_1" = "y" ] 21 | then 22 | # Load robot description to parameter server and spawn a robot 23 | xterm -e "roslaunch rtab_dumpster spawn_rtab_dumpster.launch odometryTopic:=odom_perfect" & 24 | #xterm -e "roslaunch udacity_bot spawn_udacity_bot.launch odometryTopic:=odom_perfect" & 25 | sleep 4 26 | 27 | # Start noisy odometry node 28 | xterm -e "roslaunch noisy_odometry noisy_odometry.launch" & 29 | sleep 4 30 | 31 | elif [ "$input_choice_1" = "n" ] 32 | then 33 | # Load robot description to parameter server and spawn a robot 34 | xterm -e "roslaunch rtab_dumpster spawn_rtab_dumpster.launch" & 35 | sleep 4 36 | echo "" 37 | else 38 | echo "" 39 | echo "Warning: Not an acceptable option. Do you want to start a noisy odometry node? Choose (y/n)" 40 | echo "" 41 | fi 42 | 43 | # Kick off static transform broadcaster node 44 | xterm -e "roslaunch apriltag_robot_pose static_transforms.launch" & 45 | sleep 2 46 | 47 | # Launch AprilTag detector node to detect AR markers in space 48 | xterm -e "roslaunch apriltag_robot_pose apriltag_detector.launch image_topic:=/camera/rgb/image_raw info_topic:=/camera/rgb/camera_info" & 49 | sleep 4 50 | 51 | # Execute the robot pose estimator node. 52 | xterm -hold -e "roslaunch apriltag_robot_pose robot_pose.launch camera_frame:=/camera_rgbd_frame" & 53 | sleep 2 54 | 55 | # Allow for Rviz choice 56 | echo "" 57 | read -p "Do you want to start RVIZ with a preconfigured view (y/n): " input_choice_2 58 | 59 | if [ "$input_choice_2" = "y" ] 60 | then 61 | # Start RVIZ 62 | xterm -e "roslaunch apriltag_robot_pose rviz.launch" & 63 | sleep 4 64 | # Load pre-made map using map server 65 | xterm -e "roslaunch plywood_mazes map_server_maze_3.launch" & 66 | echo "" 67 | elif [ "$input_choice_2" = "n" ] 68 | then 69 | echo "" 70 | echo "Rviz *NOT* started!" 71 | echo "" 72 | else 73 | echo "" 74 | echo "Warning: Not an acceptable option. Choose (y/n)" 75 | echo "" 76 | fi 77 | # Prompt for a key press to continue.. 78 | echo "Press any key to return to the shell prompt or" 79 | read -n 1 -r -s -p "Ctrl+C to close all X-Term processes..." 80 | echo "" 81 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | apriltag_robot_pose 4 | 0.0.0 5 | The apriltag_robot_pose package 6 | 7 | 8 | 9 | 10 | robond 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | rospy 53 | tf 54 | rospy 55 | tf 56 | rospy 57 | tf 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /config/dwa_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | # For full documentation of the parameters in this file, and a list of all the 2 | # parameters available for DWAPlannerROS, please see 3 | # http://www.ros.org/wiki/dwa_local_planner 4 | 5 | # move_base parameters 6 | controller_frequency: 3.0 # rate at which move_base will be called in Hz (default: 20.0) 7 | planner_frequency: 0.5 # rate at which re-calculate the global path, in Hz (default: 0.0 8 | 9 | DWAPlannerROS: 10 | holonomic_robot: false 11 | 12 | # Robot Configuration Parameters, set the phisical limits of the robot 13 | acc_lim_x: 1.0 14 | acc_lim_y: 0.0 # diff drive robot 15 | acc_lim_theta: 2.0 16 | 17 | max_vel_x: 0.3 # max fwd velocity allowed for the base in mt/s 18 | min_vel_x: -0.3 # min velocity allowed, set to something negative to allow the planner to generate reverse trajectories 19 | max_vel_y: 0.0 # diff drive robot 20 | min_vel_y: 0.0 # diff drive robot 21 | 22 | max_trans_vel: 0.45 # The absolute value of the maximum translational velocity for the robot in m/s 23 | min_trans_vel: 0.1 # The absolute value of the minimum translational velocity for the robot in m/s 24 | # Warning! 25 | # do not set min_trans_vel to 0.0 otherwise dwa will always think translational velocities 26 | # are non-negligible and small in place rotational velocities will be created 27 | 28 | max_rot_vel: 1.0 29 | min_rot_vel: 0.4 30 | 31 | # Forward Simulation Parameters, configure how long and with what granularity to forward simulate trajectories 32 | sim_time: 3.5 # The amount of time to forward-simulate trajectories in seconds 33 | sim_granularity: 0.025 # The step size, in meters, to take between points on a given trajectory 34 | 35 | vx_samples: 20 # The number of samples to use when exploring the x velocity space (default: 3) 36 | vy_samples: 1 # diff drive robot, there is only one sample 37 | vtheta_samples: 20 38 | 39 | # Trajectory Scoring Parameters 40 | path_distance_bias: 3.0 # 32.0 - weighting for how much it should stick to the global path plan 41 | goal_distance_bias: 1.0 # 24.0 - wighting for how much it should attempt to reach its goal 42 | occdist_scale: 0.2 # 0.01 - weighting for how much the controller should avoid obstacles 43 | forward_point_distance: 0.325 # 0.325 - how far along to place an additional scoring point 44 | stop_time_buffer: 0.2 # 0.2 - amount of time a robot must stop in before colliding for a valid traj. 45 | scaling_speed: 0.25 # 0.25 - absolute velocity at which to start scaling the robot's footprint 46 | max_scaling_factor: 0.2 # 0.2 - how much to scale the robot's footprint when at speed 47 | 48 | # Oscillation Prevention Parameters 49 | oscillation_reset_dist: 0.05 # how far to travel before resetting oscillation flags 50 | 51 | # Goal Tolerance Parameters 52 | xy_goal_tolerance: 0.3 53 | yaw_goal_tolerance: 0.2617993878 # 15 deg 54 | latch_xy_goal_tolerance: true # if the goal position is reached it is considered permanently 55 | 56 | rot_stopped_vel: 0.01 57 | trans_stopped_vel: 0.01 58 | 59 | #Global Plan Parameters 60 | prune_plan: true # Eat up the plan as the robot moves along it 61 | 62 | # Debugging 63 | publish_traj_pc : true 64 | publish_cost_grid_pc: false 65 | 66 | -------------------------------------------------------------------------------- /start_nav_demo.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "" 4 | echo "ROS AR Tag Robot Pose Estimator Demo" 5 | echo "" 6 | # The -e option to xterm is useful for running a single command and then exiting (or closing xterm after a Ctrl+C) 7 | 8 | # Fire up Gazebo world with maze environment 9 | xterm -e "roslaunch plywood_mazes maze_3_6x6.launch" & 10 | sleep 6 11 | 12 | # Prompt for a key press to continue after Gazebo has loaded 13 | #read -n 1 -r -s -p "Press any key to continue once Gazebo has loaded or Ctrl+C to abort..." 14 | #echo "" 15 | 16 | # Allow for noisy odometry choice 17 | echo "" 18 | read -p "After Gazebo has loaded, do you want to start a noisy odometry node? (y/n): " input_choice_1 19 | 20 | if [ "$input_choice_1" = "y" ] 21 | then 22 | # Load robot description to parameter server and spawn a robot 23 | #xterm -e "roslaunch rtab_dumpster spawn_rtab_dumpster.launch odometryTopic:=odom_perfect" & 24 | xterm -e "roslaunch udacity_bot spawn_udacity_bot.launch odometryTopic:=odom_perfect add_laser_sensor:=false" & 25 | sleep 4 26 | 27 | # Start noisy odometry node 28 | xterm -e "roslaunch noisy_odometry noisy_odometry.launch" & 29 | sleep 4 30 | 31 | elif [ "$input_choice_1" = "n" ] 32 | then 33 | # Load robot description to parameter server and spawn a robot 34 | #xterm -e "roslaunch rtab_dumpster spawn_rtab_dumpster.launch" & 35 | xterm -e "roslaunch udacity_bot spawn_udacity_bot.launch add_laser_sensor:=false" & 36 | sleep 4 37 | echo "" 38 | else 39 | echo "" 40 | echo "Warning: Not an acceptable option. Do you want to start a noisy odometry node? Choose (y/n)" 41 | echo "" 42 | fi 43 | 44 | # Kick off static transform broadcaster node 45 | xterm -e "roslaunch apriltag_robot_pose static_transforms.launch" & 46 | sleep 2 47 | 48 | # Launch AprilTag detector node to detect AR markers in space 49 | #xterm -e "roslaunch apriltag_robot_pose apriltag_detector.launch image_topic:=/camera/rgb/image_raw info_topic:=/camera/rgb/camera_info" & 50 | xterm -e "roslaunch apriltag_robot_pose apriltag_detector.launch image_topic:=/udacity_bot/camera1/image_raw info_topic:=/udacity_bot/camera1/image_raw" & 51 | sleep 4 52 | 53 | # Execute the robot pose estimator node. 54 | #xterm -hold -e "roslaunch apriltag_robot_pose robot_pose.launch camera_frame:=/camera_rgbd_frame" & 55 | xterm -hold -e "roslaunch apriltag_robot_pose robot_pose.launch camera_frame:=/camera_tf_frame" & 56 | sleep 2 57 | 58 | # Run move_base (nav_stack) 59 | xterm -hold -e "roslaunch apriltag_robot_pose move_base.launch" & 60 | sleep 2 61 | 62 | # Allow for Rviz choice 63 | echo "" 64 | read -p "Do you want to start RVIZ with a preconfigured view (y/n): " input_choice_2 65 | 66 | if [ "$input_choice_2" = "y" ] 67 | then 68 | # Start RVIZ 69 | xterm -e "roslaunch apriltag_robot_pose rviz.launch rviz_config_file:=ar_tag_navigation.rviz" & 70 | sleep 4 71 | # Load pre-made map using map server 72 | xterm -e "roslaunch plywood_mazes map_server_maze_3.launch" & 73 | echo "" 74 | elif [ "$input_choice_2" = "n" ] 75 | then 76 | echo "" 77 | echo "Rviz *NOT* started!" 78 | echo "" 79 | else 80 | echo "" 81 | echo "Warning: Not an acceptable option. Choose (y/n)" 82 | echo "" 83 | fi 84 | # Prompt for a key press to continue.. 85 | echo "Press any key to return to the shell prompt or" 86 | read -n 1 -r -s -p "Ctrl+C to close all X-Term processes..." 87 | echo "" 88 | -------------------------------------------------------------------------------- /config/tag_poses_maze_3.yaml: -------------------------------------------------------------------------------- 1 | # Dictionary with tag poses: [x, y, z, Roll, Pitch, Yaw] (meters, radians) 2 | 3 | tag_info: 4 | 0: [-2.5, -2.489, 0.3, 1.5708, 0, 3.14159] 5 | 1: [-1.5, -2.489, 0.3, 1.5708, 0, 3.14159] 6 | 2: [-0.5, -2.489, 0.3, 1.5708, 0, 3.14159] 7 | 3: [0.5, -2.489, 0.3, 1.5708, 0, 3.14159] 8 | 4: [1.5, -2.489, 0.3, 1.5708, 0, 3.14159] 9 | 5: [2.5, -2.489, 0.3, 1.5708, 0, 3.14159] 10 | 16: [0.5, -1.489, 0.3, 1.5708, 0, 3.14159] 11 | 17: [0.5, -1.511, 0.3, 1.5708, 0, 0] 12 | 18: [1.5, -1.489, 0.3, 1.5708, 0, 3.14159] 13 | 19: [1.5, -1.511, 0.3, 1.5708, 0, 0] 14 | 30: [-2.5, -0.489, 0.3, 1.5708, 0, 3.14159] 15 | 31: [-2.5, -0.511, 0.3, 1.5708, 0, 0] 16 | 32: [-1.5, -0.489, 0.3, 1.5708, 0, 3.14159] 17 | 33: [-1.5, -0.511, 0.3, 1.5708, 0, 0] 18 | 54: [-0.5, 0.511, 0.3, 1.5708, 0, 3.14159] 19 | 55: [-0.5, 0.489, 0.3, 1.5708, 0, 0] 20 | 58: [1.5, 0.511, 0.3, 1.5708, 0, 3.14159] 21 | 59: [1.5, 0.489, 0.3, 1.5708, 0, 0] 22 | 60: [2.5, 0.511, 0.3, 1.5708, 0, 3.14159] 23 | 61: [2.5, 0.489, 0.3, 1.5708, 0, 0] 24 | 76: [0.5, 1.511, 0.3, 1.5708, 0, 3.14159] 25 | 77: [0.5, 1.489, 0.3, 1.5708, 0, 0] 26 | 78: [1.5, 1.511, 0.3, 1.5708, 0, 3.14159] 27 | 79: [1.5, 1.489, 0.3, 1.5708, 0, 0] 28 | 92: [-1.5, 2.511, 0.3, 1.5708, 0, 3.14159] 29 | 93: [-1.5, 2.489, 0.3, 1.5708, 0, 0] 30 | 94: [-0.5, 2.511, 0.3, 1.5708, 0, 3.14159] 31 | 95: [-0.5, 2.489, 0.3, 1.5708, 0, 0] 32 | 96: [0.5, 2.511, 0.3, 1.5708, 0, 3.14159] 33 | 97: [0.5, 2.489, 0.3, 1.5708, 0, 0] 34 | 111: [-2.5, 3.489, 0.3, 1.5708, 0, 0] 35 | 113: [-1.5, 3.489, 0.3, 1.5708, 0, 0] 36 | 115: [-0.5, 3.489, 0.3, 1.5708, 0, 0] 37 | 117: [0.5, 3.489, 0.3, 1.5708, 0, 0] 38 | 119: [1.5, 3.489, 0.3, 1.5708, 0, 0] 39 | 121: [2.5, 3.489, 0.3, 1.5708, 0, 0] 40 | 300: [-2.989, -2, 0.3, 1.5708, 0, 1.5708] 41 | 301: [-2.989, -1, 0.3, 1.5708, 0, 1.5708] 42 | 302: [-2.989, 0, 0.3, 1.5708, 0, 1.5708] 43 | 303: [-2.989, 1, 0.3, 1.5708, 0, 1.5708] 44 | 304: [-2.989, 2, 0.3, 1.5708, 0, 1.5708] 45 | 305: [-2.989, 3, 0.3, 1.5708, 0, 1.5708] 46 | 310: [-1.989, -2, 0.3, 1.5708, 0, 1.5708] 47 | 311: [-2.011, -2, 0.3, 1.5708, 0, -1.5708] 48 | 314: [-1.989, 0, 0.3, 1.5708, 0, 1.5708] 49 | 315: [-2.011, 0, 0.3, 1.5708, 0, -1.5708] 50 | 316: [-1.989, 1, 0.3, 1.5708, 0, 1.5708] 51 | 317: [-2.011, 1, 0.3, 1.5708, 0, -1.5708] 52 | 332: [-0.989, -1, 0.3, 1.5708, 0, 1.5708] 53 | 333: [-1.011, -1, 0.3, 1.5708, 0, -1.5708] 54 | 336: [-0.989, 1, 0.3, 1.5708, 0, 1.5708] 55 | 337: [-1.011, 1, 0.3, 1.5708, 0, -1.5708] 56 | 338: [-0.989, 2, 0.3, 1.5708, 0, 1.5708] 57 | 339: [-1.011, 2, 0.3, 1.5708, 0, -1.5708] 58 | 350: [0.011, -2, 0.3, 1.5708, 0, 1.5708] 59 | 351: [-0.011, -2, 0.3, 1.5708, 0, -1.5708] 60 | 352: [0.011, -1, 0.3, 1.5708, 0, 1.5708] 61 | 353: [-0.011, -1, 0.3, 1.5708, 0, -1.5708] 62 | 374: [1.011, 0, 0.3, 1.5708, 0, 1.5708] 63 | 375: [0.989, 0, 0.3, 1.5708, 0, -1.5708] 64 | 394: [2.011, 0, 0.3, 1.5708, 0, 1.5708] 65 | 395: [1.989, 0, 0.3, 1.5708, 0, -1.5708] 66 | 398: [2.011, 2, 0.3, 1.5708, 0, 1.5708] 67 | 399: [1.989, 2, 0.3, 1.5708, 0, -1.5708] 68 | 400: [2.011, 3, 0.3, 1.5708, 0, 1.5708] 69 | 401: [1.989, 3, 0.3, 1.5708, 0, -1.5708] 70 | 411: [2.989, -2, 0.3, 1.5708, 0, -1.5708] 71 | 413: [2.989, -1, 0.3, 1.5708, 0, -1.5708] 72 | 415: [2.989, 0, 0.3, 1.5708, 0, -1.5708] 73 | 417: [2.989, 1, 0.3, 1.5708, 0, -1.5708] 74 | 419: [2.989, 2, 0.3, 1.5708, 0, -1.5708] 75 | 421: [2.989, 3, 0.3, 1.5708, 0, -1.5708] 76 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | ## AprilTag Robot Pose 4 | Author: Roberto Zegers R. 5 | 6 | This ROS package implements a robot localization system using AprilTag markers. The tags used correspond to the family tag36h11, which has 587 different tags. 7 | 8 | ## Dependencies 9 | Packages on which this package depends: 10 | + [apriltags_ros](https://github.com/RIVeR-Lab/apriltags_ros) 11 | + [map_server](http://wiki.ros.org/map_server) 12 | 13 | ## Install 14 | 15 | First install AprilTag and its ROS wrapper by cloning the next repository into the /src directory of your catkin_workspace: 16 | `$ git clone https://github.com/RIVeR-Lab/apriltags_ros.git` 17 | 18 | Then clone this repository: 19 | `$ git clone https://github.com/rfzeg/apriltag_robot_pose.git` 20 | 21 | Compile the package with `catkin_make` and source your workspace. 22 | 23 | **Note:** 24 | The package requires to spawn a robot that publishes camera images into ROS. The standard for cameras on ROS is to publish to topics such as: 25 | + /image_raw - an unmodified camera image 26 | + /camera_info - information about the camera calibration 27 | 28 | ## Run 29 | 30 | Start all neccesary components by executing the included bash script (xterm required): 31 | `./start_demo.sh` 32 | 33 | Or alternatively start the demo launching every node manually: 34 | 35 | 1. **Fire up Gazebo world that includes AprilTag markers, e.g.:** 36 | `roslaunch plywood_mazes maze_3_6x6.launch` 37 | 2. **In a new terminal load a robot URDF file to the parameter server, e.g.:** 38 | `$ roslaunch udacity_bot robot_description.launch` 39 | 3. **And spawn the robot model, e.g.:** 40 | `$ roslaunch udacity_bot spawn_udacity_bot.launch` 41 | 4. **Launch the noisy_odom node:** 42 | `$ rosrun apriltag_robot_pose noisy_odom.py` 43 | 5. **In a new window kick off the static transform broadcaster node:** 44 | `$ roslaunch apriltag_robot_pose static_transforms.launch` 45 | 6. **Then launch the AprilTag detector node to detect AR markers in the camera image:** 46 | `$ roslaunch apriltag_robot_pose apriltag_detector.launch` 47 | 7. **Next execute the robot pose estimator node:** 48 | `$ rosrun apriltag_robot_pose robot_pose.py` 49 | 8. **In order to see the robot pose estimator node in action open RViz:** 50 | `$ roslaunch apriltag_robot_pose rviz.launch` 51 | 9. **Launch the map_server node to see the map in RViz, e.g.:** 52 | `$ roslaunch plywood_mazes map_server_maze_3.launch` 53 | 54 | If you want to manually control the robot, open a new terminal window and run: 55 | `$ rosrun rqt_robot_steering rqt_robot_steering` 56 | 57 | ## Optional Checks 58 | 59 | On a new terminal run this command to see the existing topics: 60 | `$ rostopic list` 61 | 62 | If everything is correct, a list of topics published by the apriltag detector node should appear: 63 | 64 | Fig.1 The available topics shown by using the **rostopic list** command 65 | 66 | Then check that AprilTags are being detected by placing the robot's camera in front of a tag and running: 67 | 68 | `$ rostopic echo /tag_detections` 69 | 70 | 71 | Fig.2 When a tag is detected values similar to these are displayed when running the **rostopic echo** command 72 | 73 | To view raw images, for instance on the topic /udacity_bot/camera1/image\_raw, use: 74 | `$ rosrun image_view image_view image:=/udacity_bot/camera1/image_raw` 75 | 76 | 77 | To check that the parameters defined in the tag\_sizes.yaml file were loaded into the param server type: 78 | `$ rosparam get /apriltag_detector/tag_descriptions` 79 | 80 | To look at the numeric values of a transform between the map frame and any specific AR marker tag: 81 | `$ rosrun tf tf_echo map tag_0` 82 | or visualize the complete tf tree using RQT: 83 | `$ rosrun rqt_tf_tree rqt_tf_tree` 84 | 85 | If you would like the apriltag_robot_pose node to display output at the DEBUG verbosity level use: 86 | `$ rosservice call /apriltag_robot_pose/set_logger_level "{logger: 'rosout', level: 'debug'}"` 87 | 88 | ## Troubleshooting 89 | + Gazebo is crashing as it is starting up: Usually, it is enough to run it again (probably several times). 90 | + ImportError No module named apriltags.msg: When using custom messages, make sure the packages containing them have been compiled. 91 | + ERROR: cannot launch node of type [map_server/map_server]: map_server 92 | You have to install map_server first: `$ sudo apt install ros-kinetic-map-server` 93 | 94 | This package has only been tested on Ubuntu 16.04 LTS with ROS Kinetic and Gazebo 7.15. 95 | 96 | ## Further Improvements 97 | + Averaging quaternions is not straightforward. At the moment the robot's estimated orientation, when several AR markers are detected, is the orientation of the first detected marker. 98 | One could do a SLERP to average two quaternions from two (closest) detected markers for a better robot's orientation estimation. See: https://github.com/danielduberg/safe_flight/issues/1 99 | 100 | ## Resources 101 | + http://wiki.ros.org/tf2/Tutorials/Writing%20a%20tf2%20broadcaster%20%28Python%29 102 | + The general relationship between the map, odom and base\_link frames is already described in [Coordinate Frames for Mobile Platforms](http://www.ros.org/reps/rep-0105.html). 103 | + [Average of Quaternions](https://stackoverflow.com/questions/12374087/average-of-multiple-quaternions/) 104 | + https://github.com/lucien386/DJISummerCamp2019/blob/master/ROS_Ws/src/sc_localization/odom_update_node.cpp 105 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(apriltag_robot_pose) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | tf 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | # find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a exec_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs # Or other packages containing msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if your package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES apriltag_robot_pose 107 | # CATKIN_DEPENDS rospy tf 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | include_directories( 118 | # include 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(${PROJECT_NAME} 124 | # src/${PROJECT_NAME}/apriltag_robot_pose.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | ## With catkin_make all packages are built within a single CMake context 134 | ## The recommended prefix ensures that target names across packages don't collide 135 | # add_executable(${PROJECT_NAME}_node src/apriltag_robot_pose_node.cpp) 136 | 137 | ## Rename C++ executable without prefix 138 | ## The above recommended prefix causes long target names, the following renames the 139 | ## target back to the shorter version for ease of user use 140 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 141 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 142 | 143 | ## Add cmake target dependencies of the executable 144 | ## same as for the library above 145 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 146 | 147 | ## Specify libraries to link a library or executable target against 148 | # target_link_libraries(${PROJECT_NAME}_node 149 | # ${catkin_LIBRARIES} 150 | # ) 151 | 152 | ############# 153 | ## Install ## 154 | ############# 155 | 156 | # all install targets should use catkin DESTINATION variables 157 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 158 | 159 | ## Mark executable scripts (Python etc.) for installation 160 | ## in contrast to setup.py, you can choose the destination 161 | # install(PROGRAMS 162 | # scripts/my_python_script 163 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 164 | # ) 165 | 166 | ## Mark executables and/or libraries for installation 167 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 168 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 169 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 171 | # ) 172 | 173 | ## Mark cpp header files for installation 174 | # install(DIRECTORY include/${PROJECT_NAME}/ 175 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 176 | # FILES_MATCHING PATTERN "*.h" 177 | # PATTERN ".svn" EXCLUDE 178 | # ) 179 | 180 | ## Mark other files for installation (e.g. launch and bag files, etc.) 181 | # install(FILES 182 | # # myfile1 183 | # # myfile2 184 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 185 | # ) 186 | 187 | ############# 188 | ## Testing ## 189 | ############# 190 | 191 | ## Add gtest based cpp test target and link libraries 192 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_apriltag_robot_pose.cpp) 193 | # if(TARGET ${PROJECT_NAME}-test) 194 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 195 | # endif() 196 | 197 | ## Add folders to be run by python nosetests 198 | # catkin_add_nosetests(test) 199 | -------------------------------------------------------------------------------- /config/robot_pose.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 743 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Toolbars: 31 | toolButtonStyle: 2 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Alpha: 0.5 36 | Cell Size: 1 37 | Class: rviz/Grid 38 | Color: 160; 160; 164 39 | Enabled: true 40 | Line Style: 41 | Line Width: 0.0299999993 42 | Value: Lines 43 | Name: Grid 44 | Normal Cell Count: 0 45 | Offset: 46 | X: 0 47 | Y: 0 48 | Z: 0 49 | Plane: XY 50 | Plane Cell Count: 10 51 | Reference Frame: 52 | Value: true 53 | - Class: rviz/TF 54 | Enabled: true 55 | Frame Timeout: 15 56 | Frames: 57 | All Enabled: true 58 | camera: 59 | Value: true 60 | chassis: 61 | Value: true 62 | hokuyo: 63 | Value: true 64 | left_wheel: 65 | Value: true 66 | map: 67 | Value: true 68 | odom: 69 | Value: true 70 | right_wheel: 71 | Value: true 72 | robot_footprint: 73 | Value: true 74 | tag_0: 75 | Value: true 76 | tag_1: 77 | Value: true 78 | tag_111: 79 | Value: true 80 | tag_113: 81 | Value: true 82 | tag_115: 83 | Value: true 84 | tag_117: 85 | Value: true 86 | tag_119: 87 | Value: true 88 | tag_121: 89 | Value: true 90 | tag_16: 91 | Value: true 92 | tag_17: 93 | Value: true 94 | tag_18: 95 | Value: true 96 | tag_19: 97 | Value: true 98 | tag_2: 99 | Value: true 100 | tag_3: 101 | Value: true 102 | tag_30: 103 | Value: true 104 | tag_300: 105 | Value: true 106 | tag_301: 107 | Value: true 108 | tag_302: 109 | Value: true 110 | tag_303: 111 | Value: true 112 | tag_304: 113 | Value: true 114 | tag_305: 115 | Value: true 116 | tag_31: 117 | Value: true 118 | tag_310: 119 | Value: true 120 | tag_311: 121 | Value: true 122 | tag_314: 123 | Value: true 124 | tag_315: 125 | Value: true 126 | tag_316: 127 | Value: true 128 | tag_317: 129 | Value: true 130 | tag_32: 131 | Value: true 132 | tag_33: 133 | Value: true 134 | tag_332: 135 | Value: true 136 | tag_333: 137 | Value: true 138 | tag_336: 139 | Value: true 140 | tag_337: 141 | Value: true 142 | tag_338: 143 | Value: true 144 | tag_339: 145 | Value: true 146 | tag_350: 147 | Value: true 148 | tag_351: 149 | Value: true 150 | tag_352: 151 | Value: true 152 | tag_353: 153 | Value: true 154 | tag_374: 155 | Value: true 156 | tag_375: 157 | Value: true 158 | tag_394: 159 | Value: true 160 | tag_395: 161 | Value: true 162 | tag_398: 163 | Value: true 164 | tag_399: 165 | Value: true 166 | tag_4: 167 | Value: true 168 | tag_400: 169 | Value: true 170 | tag_401: 171 | Value: true 172 | tag_411: 173 | Value: true 174 | tag_413: 175 | Value: true 176 | tag_415: 177 | Value: true 178 | tag_417: 179 | Value: true 180 | tag_419: 181 | Value: true 182 | tag_421: 183 | Value: true 184 | tag_5: 185 | Value: true 186 | tag_54: 187 | Value: true 188 | tag_55: 189 | Value: true 190 | tag_58: 191 | Value: true 192 | tag_59: 193 | Value: true 194 | tag_60: 195 | Value: true 196 | tag_61: 197 | Value: true 198 | tag_76: 199 | Value: true 200 | tag_77: 201 | Value: true 202 | tag_78: 203 | Value: true 204 | tag_79: 205 | Value: true 206 | tag_92: 207 | Value: true 208 | tag_93: 209 | Value: true 210 | tag_94: 211 | Value: true 212 | tag_95: 213 | Value: true 214 | tag_96: 215 | Value: true 216 | tag_97: 217 | Value: true 218 | Marker Scale: 1 219 | Name: TF 220 | Show Arrows: true 221 | Show Axes: true 222 | Show Names: true 223 | Tree: 224 | map: 225 | odom: 226 | robot_footprint: 227 | chassis: 228 | camera: 229 | {} 230 | hokuyo: 231 | {} 232 | left_wheel: 233 | {} 234 | right_wheel: 235 | {} 236 | tag_0: 237 | {} 238 | tag_1: 239 | {} 240 | tag_111: 241 | {} 242 | tag_113: 243 | {} 244 | tag_115: 245 | {} 246 | tag_117: 247 | {} 248 | tag_119: 249 | {} 250 | tag_121: 251 | {} 252 | tag_16: 253 | {} 254 | tag_17: 255 | {} 256 | tag_18: 257 | {} 258 | tag_19: 259 | {} 260 | tag_2: 261 | {} 262 | tag_3: 263 | {} 264 | tag_30: 265 | {} 266 | tag_300: 267 | {} 268 | tag_301: 269 | {} 270 | tag_302: 271 | {} 272 | tag_303: 273 | {} 274 | tag_304: 275 | {} 276 | tag_305: 277 | {} 278 | tag_31: 279 | {} 280 | tag_310: 281 | {} 282 | tag_311: 283 | {} 284 | tag_314: 285 | {} 286 | tag_315: 287 | {} 288 | tag_316: 289 | {} 290 | tag_317: 291 | {} 292 | tag_32: 293 | {} 294 | tag_33: 295 | {} 296 | tag_332: 297 | {} 298 | tag_333: 299 | {} 300 | tag_336: 301 | {} 302 | tag_337: 303 | {} 304 | tag_338: 305 | {} 306 | tag_339: 307 | {} 308 | tag_350: 309 | {} 310 | tag_351: 311 | {} 312 | tag_352: 313 | {} 314 | tag_353: 315 | {} 316 | tag_374: 317 | {} 318 | tag_375: 319 | {} 320 | tag_394: 321 | {} 322 | tag_395: 323 | {} 324 | tag_398: 325 | {} 326 | tag_399: 327 | {} 328 | tag_4: 329 | {} 330 | tag_400: 331 | {} 332 | tag_401: 333 | {} 334 | tag_411: 335 | {} 336 | tag_413: 337 | {} 338 | tag_415: 339 | {} 340 | tag_417: 341 | {} 342 | tag_419: 343 | {} 344 | tag_421: 345 | {} 346 | tag_5: 347 | {} 348 | tag_54: 349 | {} 350 | tag_55: 351 | {} 352 | tag_58: 353 | {} 354 | tag_59: 355 | {} 356 | tag_60: 357 | {} 358 | tag_61: 359 | {} 360 | tag_76: 361 | {} 362 | tag_77: 363 | {} 364 | tag_78: 365 | {} 366 | tag_79: 367 | {} 368 | tag_92: 369 | {} 370 | tag_93: 371 | {} 372 | tag_94: 373 | {} 374 | tag_95: 375 | {} 376 | tag_96: 377 | {} 378 | tag_97: 379 | {} 380 | Update Interval: 0 381 | Value: true 382 | - Alpha: 1 383 | Class: rviz/RobotModel 384 | Collision Enabled: false 385 | Enabled: true 386 | Links: 387 | All Links Enabled: true 388 | Expand Joint Details: false 389 | Expand Link Details: false 390 | Expand Tree: false 391 | Link Tree Style: Links in Alphabetic Order 392 | camera: 393 | Alpha: 1 394 | Show Axes: false 395 | Show Trail: false 396 | Value: true 397 | chassis: 398 | Alpha: 1 399 | Show Axes: false 400 | Show Trail: false 401 | Value: true 402 | hokuyo: 403 | Alpha: 1 404 | Show Axes: false 405 | Show Trail: false 406 | Value: true 407 | left_wheel: 408 | Alpha: 1 409 | Show Axes: false 410 | Show Trail: false 411 | Value: true 412 | right_wheel: 413 | Alpha: 1 414 | Show Axes: false 415 | Show Trail: false 416 | Value: true 417 | robot_footprint: 418 | Alpha: 1 419 | Show Axes: false 420 | Show Trail: false 421 | Name: RobotModel 422 | Robot Description: robot_description 423 | TF Prefix: "" 424 | Update Interval: 0 425 | Value: true 426 | Visual Enabled: true 427 | - Alpha: 0.699999988 428 | Class: rviz/Map 429 | Color Scheme: map 430 | Draw Behind: false 431 | Enabled: true 432 | Name: Map 433 | Topic: /map 434 | Unreliable: false 435 | Use Timestamp: false 436 | Value: true 437 | Enabled: true 438 | Global Options: 439 | Background Color: 48; 48; 48 440 | Default Light: true 441 | Fixed Frame: map 442 | Frame Rate: 30 443 | Name: root 444 | Tools: 445 | - Class: rviz/Interact 446 | Hide Inactive Objects: true 447 | - Class: rviz/MoveCamera 448 | - Class: rviz/Select 449 | - Class: rviz/FocusCamera 450 | - Class: rviz/Measure 451 | - Class: rviz/SetInitialPose 452 | Topic: /initialpose 453 | - Class: rviz/SetGoal 454 | Topic: /move_base_simple/goal 455 | - Class: rviz/PublishPoint 456 | Single click: true 457 | Topic: /clicked_point 458 | Value: true 459 | Views: 460 | Current: 461 | Class: rviz/Orbit 462 | Distance: 8.72745323 463 | Enable Stereo Rendering: 464 | Stereo Eye Separation: 0.0599999987 465 | Stereo Focal Distance: 1 466 | Swap Stereo Eyes: false 467 | Value: false 468 | Focal Point: 469 | X: 0.461928636 470 | Y: 0.215806082 471 | Z: 0.33352679 472 | Focal Shape Fixed Size: true 473 | Focal Shape Size: 0.0500000007 474 | Invert Z Axis: false 475 | Name: Current View 476 | Near Clip Distance: 0.00999999978 477 | Pitch: 0.525398076 478 | Target Frame: 479 | Value: Orbit (rviz) 480 | Yaw: 4.21358395 481 | Saved: ~ 482 | Window Geometry: 483 | Displays: 484 | collapsed: false 485 | Height: 1031 486 | Hide Left Dock: false 487 | Hide Right Dock: false 488 | QMainWindow State: 000000ff00000000fd0000000400000000000001860000036ffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a0000036f000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000036ffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a0000036f0000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004df0000036f00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 489 | Selection: 490 | collapsed: false 491 | Time: 492 | collapsed: false 493 | Tool Properties: 494 | collapsed: false 495 | Views: 496 | collapsed: false 497 | Width: 1920 498 | X: 0 499 | Y: 0 500 | -------------------------------------------------------------------------------- /scripts/robot_pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # Estimates the absolute pose a robot in a map based on the position of the AprilTag markers in the robot's camera field of view 4 | # Broadcasts the transform of odom w.r.t. map to correct odometry drift 5 | # Author: Roberto Zegers R. 6 | # Date: 2019 July 7 | 8 | import rospy 9 | from apriltags_ros.msg import AprilTagDetection, AprilTagDetectionArray 10 | from geometry_msgs.msg import Pose, PoseStamped, TransformStamped, Point, Quaternion 11 | from tf.transformations import quaternion_from_euler, euler_from_quaternion, translation_from_matrix, quaternion_from_matrix, compose_matrix, quaternion_matrix, rotation_matrix 12 | import numpy as np 13 | import tf2_ros 14 | import tf2_geometry_msgs 15 | 16 | ## Global variables 17 | nrTfRetrys = 1 18 | retryTime = 0.05 19 | rospy.init_node('apriltag_robot_pose', log_level=rospy.INFO, anonymous=False) 20 | 21 | # Initializes a tf2 listener 22 | tf_buffer = tf2_ros.Buffer(rospy.Duration(10.0)) #tf buffer length 23 | tf_listener = tf2_ros.TransformListener(tf_buffer) 24 | 25 | # Initializes a tf2 broadcaster for our map(parent)->odom(child) == odom w.r.t. map transform 26 | br_odom_wrt_map = tf2_ros.TransformBroadcaster() 27 | 28 | # Initializes an empty TransformStamped object for our map(parent)->odom(child) == odom w.r.t. map transform 29 | ts_odom_wrt_map = TransformStamped() 30 | 31 | r = rospy.Rate(10) # 10hz 32 | 33 | # Initializes an empty PoseStamped object the pose of the robot_base w.r.t map 34 | robot_pose = PoseStamped() 35 | 36 | new_translation = [] 37 | new_rotation = [] 38 | 39 | def strip_forward_slash(frame_id): 40 | ''' 41 | Removes forward slash for tf2 to work 42 | ''' 43 | if frame_id[0] == "/": 44 | new_frame_id = frame_id[1:] 45 | else: 46 | new_frame_id = frame_id 47 | return new_frame_id 48 | 49 | # get the robot's base frame 50 | if rospy.has_param("~base_frame"): 51 | # forward slash must be removed to work with tf2 52 | base_frame = strip_forward_slash(rospy.get_param("~base_frame")) 53 | else: 54 | base_frame = "base_footprint" 55 | rospy.logwarn("base_footprint frame is set to default: base_footprint") 56 | 57 | # get odom frame, the (noisy) odometry 58 | if rospy.has_param("~odom_frame"): 59 | odom_frame = strip_forward_slash(rospy.get_param("~odom_frame")) 60 | else: 61 | odom_frame = "odom" 62 | rospy.logwarn("odometry frame of reference is set to default: odom") 63 | 64 | # get world_fixed_frame 65 | if rospy.has_param("~world_fixed_frame"): 66 | world_fixed_frame = strip_forward_slash(rospy.get_param("~world_fixed_frame")) 67 | else: 68 | world_fixed_frame = "map" 69 | rospy.logwarn("world_fixed_frame frame is set to default: map") 70 | 71 | # get the camera frame 72 | if rospy.has_param("~camera_frame"): 73 | camera_frame = strip_forward_slash(rospy.get_param("~camera_frame")) 74 | else: 75 | camera_frame = "camera" 76 | rospy.logwarn("camera frame of reference is set to default: camera") 77 | 78 | def main(): 79 | rospy.Subscriber("/tag_detections", AprilTagDetectionArray, apriltag_callback, queue_size = 1) 80 | rospy.loginfo("Started node to broadcast odom wrt map transform!") 81 | # get base w.r.t. odom transform 82 | while not rospy.is_shutdown(): 83 | try: 84 | # Look for the odom->base_footprint transform 85 | rospy.logdebug("Looking for the odom->robot_footprint transform") 86 | ts_base_wrt_odom = tf_buffer.lookup_transform(odom_frame, base_frame, rospy.Time(), rospy.Duration(1.0)) # wait 1s for transform to become available 87 | rospy.logdebug("ts_base_wrt_odom: %s", ts_base_wrt_odom) 88 | # note: robot_pose (base_frame wrt map) is calculated every time the subscriber callback is executed 89 | broadcast_last_transform() 90 | except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException), ex: 91 | rospy.logerr(ex) 92 | r.sleep() 93 | 94 | def pose2poselist(pose): 95 | ''' Transforms a pose object into the form of a python list''' 96 | return [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w] 97 | 98 | def transformPose(pose, sourceFrame, target_frame): 99 | ''' 100 | Converts a pose represented as a list in the source_frame 101 | to a pose represented as a list in the target_frame 102 | ''' 103 | _pose = PoseStamped() 104 | _pose.header.frame_id = sourceFrame 105 | if len(pose) == 6: 106 | pose.append(0) 107 | pose[3:7] = quaternion_from_euler(pose[3], pose[4], pose[5]).tolist() 108 | 109 | _pose.pose.position.x = pose[0] 110 | _pose.pose.position.y = pose[1] 111 | _pose.pose.position.z = pose[2] 112 | _pose.pose.orientation.x = pose[3] 113 | _pose.pose.orientation.y = pose[4] 114 | _pose.pose.orientation.z = pose[5] 115 | _pose.pose.orientation.w = pose[6] 116 | 117 | for i in range(nrTfRetrys): 118 | try: 119 | t = rospy.Time(0) 120 | _pose.header.stamp = t 121 | # converts a Pose object from its reference frame to a Pose object in the frame target_frame 122 | transform = tf_buffer.lookup_transform(target_frame, 123 | _pose.header.frame_id, #source frame is the current object's frame of reference 124 | rospy.Time(0), #get the tf at first available time 125 | rospy.Duration(1.0)) #wait for 1 second 126 | pose_transformed = tf2_geometry_msgs.do_transform_pose(_pose, transform) 127 | p = pose_transformed.pose.position 128 | o = pose_transformed.pose.orientation 129 | return [p.x, p.y, p.z, o.x, o.y, o.z, o.w] 130 | except (tf2_ros.LookupException), e1: 131 | print("ERROR: LookupException!") 132 | rospy.logerr(e1) 133 | rospy.logwarn("No tf frame with name %s found. Check that the detected tag ID is part of the transforms that are being broadcasted by the static transform broadcaster.", target_frame) 134 | continue 135 | except (tf2_ros.ConnectivityException), e2: 136 | rospy.logwarn(e2) 137 | rospy.logerr("ERROR: ConnectivityException!") 138 | continue 139 | except (tf2_ros.ExtrapolationException), e3: 140 | rospy.logwarn(e3) 141 | rospy.logerr("ERROR: ExtrapolationException!") 142 | continue 143 | except Exception as e4: 144 | rospy.logwarn(e4) 145 | rospy.logerr("Unexpected error when transforming Pose") 146 | finally: 147 | rospy.sleep(retryTime) 148 | return None 149 | 150 | def xyzquat_from_matrix(matrix): 151 | return translation_from_matrix(matrix).tolist() + quaternion_from_matrix(matrix).tolist() 152 | 153 | def matrix_from_xyzquat(arg1, arg2=None): 154 | return matrix_from_xyzquat_np_array(arg1, arg2).tolist() 155 | 156 | def matrix_from_xyzquat_np_array(arg1, arg2=None): 157 | if arg2 is not None: 158 | translate = arg1 159 | quaternion = arg2 160 | else: 161 | translate = arg1[0:3] 162 | quaternion = arg1[3:7] 163 | 164 | return np.dot(compose_matrix(translate=translate),quaternion_matrix(quaternion)) 165 | 166 | def invPoselist(poselist): 167 | return xyzquat_from_matrix(np.linalg.inv(matrix_from_xyzquat(poselist))) 168 | 169 | def base_wrt_map_pose(pose=[0,0,0,0,0,0,1], child_frame_id='obj', parent_frame_id = world_fixed_frame, npub=1): 170 | ''' 171 | Converts from a representation of a pose as a list to a TransformStamped object (translation and rotation (Quaternion) representation) 172 | Then keeps that as a TransformStamped object 173 | Note: 174 | In Rviz it will be shown as an arrow from the robot base (child) to the map (parent) 175 | In RQT it will be shown as an arrow from the map (parent) to the robot base (child) 176 | ''' 177 | global robot_pose 178 | if len(pose) == 7: 179 | quaternion = tuple(pose[3:7]) 180 | elif len(pose) == 6: 181 | quaternion = quaternion_from_euler(*pose[3:6]) 182 | else: 183 | rospy.logerr("Bad length of pose") 184 | return None 185 | 186 | position = tuple(pose[0:3]) 187 | # Fill in PoseStamped object: stamps the transform with the current time 188 | robot_pose.header.stamp = rospy.Time.now() 189 | # Sets the frame ID of the transform to the map frame 190 | robot_pose.header.frame_id = parent_frame_id 191 | # Fill in coordinates 192 | if len(pose) == 6: 193 | pose.append(0) 194 | pose[3:7] = quaternion_from_euler(pose[3], pose[4], pose[5]).tolist() 195 | 196 | robot_pose.pose.position.x = pose[0] 197 | robot_pose.pose.position.y = pose[1] 198 | robot_pose.pose.position.z = 0 # fixate the z value of the robot base to avoid that it jumps up and down, or use pose[2] for the detected value 199 | robot_pose.pose.orientation.x = pose[3] 200 | robot_pose.pose.orientation.y = pose[4] 201 | robot_pose.pose.orientation.z = pose[5] 202 | robot_pose.pose.orientation.w = pose[6] 203 | 204 | def averagePose(pose_list): 205 | ''' 206 | Calculates the averge pose from a list of poses 207 | Position is the average of all estimated positions 208 | Orientation uses the orientation of the first detected marker 209 | ''' 210 | avg_pose = [] 211 | avg_pose.append(np.mean([pose[0] for pose in pose_list])) 212 | avg_pose.append(np.mean([pose[1] for pose in pose_list])) 213 | avg_pose.append(np.mean([pose[2] for pose in pose_list])) 214 | # Use the orientation of the first detected marker 215 | avg_pose.extend(pose_list[0][3:7]) 216 | return avg_pose 217 | 218 | def apriltag_callback(data): 219 | # rospy.logdebug(rospy.get_caller_id() + "I heard %s", data) 220 | if data.detections: 221 | poselist_base_wrt_map = [] 222 | for detection in data.detections: 223 | tag_id = detection.id # tag id 224 | rospy.logdebug("Tag ID detected: %s \n", tag_id) 225 | child_frame_id = "tag_" + str(tag_id) 226 | 227 | # Convert the deteced tag Pose object to tag pose representation as a list, only for convinience 228 | poselist_tag_wrt_camera = pose2poselist(detection.pose) 229 | rospy.logdebug("poselist_tag_wrt_camera: \n %s \n", poselist_tag_wrt_camera) 230 | 231 | # Calculate transform of tag w.r.t. robot base (in Rviz arrow points from tag (child) to robot base(parent)) 232 | poselist_tag_wrt_base = transformPose(poselist_tag_wrt_camera, camera_frame, base_frame) 233 | rospy.logdebug("transformPose(poselist_tag_wrt_camera, 'camera', 'robot_footprint'): \n %s \n", poselist_tag_wrt_base) 234 | 235 | # Calculate transform of robot base w.r.t. tag (in Rviz arrow points from robot base (child) to tag(parent)) 236 | poselist_base_wrt_tag = invPoselist(poselist_tag_wrt_base) 237 | rospy.logdebug("invPoselist( poselist_tag_wrt_base): \n %s \n", poselist_base_wrt_tag) 238 | 239 | # Calculate transform of robot base w.r.t. map (in Rviz arrow points from robot base (child) to map (parent)), returns pose of robot in the map coordinates 240 | poselist_base_wrt_map.append(transformPose(poselist_base_wrt_tag, child_frame_id, target_frame = world_fixed_frame)) 241 | rospy.logdebug("transformPose(poselist_base_wrt_tag, sourceFrame = '%s', target_frame = 'map'): \n %s \n", child_frame_id, poselist_base_wrt_map[-1]) 242 | 243 | #for counter, robot_pose in enumerate(poselist_base_wrt_map): 244 | # rospy.logdebug("\n Robot pose estimation nr. %s: %s \n",str(counter), robot_pose) 245 | 246 | estimated_avg_pose = averagePose(poselist_base_wrt_map) 247 | rospy.logdebug("\n Robot's estimated avg. pose from all AR tags detected:\n %s \n", estimated_avg_pose) 248 | 249 | # Calculate transform of robot base w.r.t. map or pose of robot in the map coordinates 250 | base_wrt_map_pose(pose = estimated_avg_pose, child_frame_id = base_frame, parent_frame_id = world_fixed_frame) 251 | 252 | map_to_odom_transform() 253 | 254 | def convert_pose_inverse_transform(input_pose): 255 | """ Helper method to invert a transform (this is built into the tf C++ classes, but ommitted from Python) """ 256 | translation = np.zeros((4,1)) 257 | translation[0] = -input_pose.pose.position.x 258 | translation[1] = -input_pose.pose.position.y 259 | translation[2] = -input_pose.pose.position.z 260 | translation[3] = 1.0 261 | 262 | rotation = (input_pose.pose.orientation.x, input_pose.pose.orientation.y, input_pose.pose.orientation.z, input_pose.pose.orientation.w) 263 | euler_angle = euler_from_quaternion(rotation) 264 | rotation = np.transpose(rotation_matrix(euler_angle[2], [0,0,1])) # the angle is a yaw 265 | transformed_translation = rotation.dot(translation) 266 | 267 | translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2]) 268 | rotation = quaternion_from_matrix(rotation) 269 | return (translation, rotation) 270 | 271 | def convert_translation_rotation_to_pose(translation, rotation): 272 | """ Convert from representation of a pose as translation and rotation (Quaternion) tuples to a geometry_msgs/Pose message """ 273 | return Pose(position=Point(x=translation[0],y=translation[1],z=translation[2]), orientation=Quaternion(x=rotation[0],y=rotation[1],z=rotation[2],w=rotation[3])) 274 | 275 | def map_to_odom_transform(): 276 | """ This method constantly updates the offset of the map and 277 | odometry coordinate systems based on the latest results from 278 | the localizer 279 | """ 280 | global new_translation, new_rotation 281 | (translation, rotation) = convert_pose_inverse_transform(robot_pose) 282 | map_wrt_base_pose = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation)) 283 | map_wrt_base_pose.header.stamp=rospy.Time.now() 284 | map_wrt_base_pose.header.frame_id=base_frame 285 | # access/look-up the transform odom (target/new frame) wrt base(source/current frame): 286 | # tf_buffer.lookup_transform(target_frame, source_frame, query_time, timeout_secs) 287 | ts_odom_wrt_base = tf_buffer.lookup_transform(odom_frame, base_frame, rospy.Time(), rospy.Duration(1.0)) # wait 1s for transform to become available 288 | map_wrt_odom_pose = tf2_geometry_msgs.do_transform_pose(map_wrt_base_pose, ts_odom_wrt_base) 289 | # invert map_wrt_odom_to get odom_wrt_map and store as tuple position, quaternion 290 | (new_translation, new_rotation) = convert_pose_inverse_transform(map_wrt_odom_pose) 291 | 292 | def broadcast_last_transform(): 293 | """ Make sure that we are always broadcasting the last 294 | map to odom transformation. This is necessary so 295 | move_base can work properly. """ 296 | # Sets the frame ID of the transform to the map frame 297 | ts_odom_wrt_map.header.frame_id = world_fixed_frame 298 | # Stamps the transform with the current time 299 | ts_odom_wrt_map.header.stamp = rospy.Time.now() 300 | # Sets the child frame ID to odom 301 | ts_odom_wrt_map.child_frame_id = odom_frame 302 | if new_translation: 303 | # Fill in coordinates 304 | list(new_translation) # convert an np array to an ordinary list 305 | ts_odom_wrt_map.transform.translation.x = new_translation[0][0] 306 | ts_odom_wrt_map.transform.translation.y = new_translation[1][0] 307 | ts_odom_wrt_map.transform.translation.z = new_translation[2][0] 308 | ts_odom_wrt_map.transform.rotation.x = new_rotation[0] 309 | ts_odom_wrt_map.transform.rotation.y = new_rotation[1] 310 | ts_odom_wrt_map.transform.rotation.z = new_rotation[2] 311 | ts_odom_wrt_map.transform.rotation.w = new_rotation[3] 312 | try: 313 | br_odom_wrt_map.sendTransform(ts_odom_wrt_map) 314 | rospy.logdebug("Broadcasted odom wrt map transform!") 315 | except Exception as exc: 316 | rospy.logwarn(exc) 317 | rospy.logerr("Unexpected error in broadcast of map to odom transformation") 318 | 319 | if __name__=='__main__': 320 | main() 321 | -------------------------------------------------------------------------------- /config/ar_tag_navigation.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /TF1/Frames1 10 | - /RobotModel1 11 | Splitter Ratio: 0.5 12 | Tree Height: 621 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.0299999993 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/TF 56 | Enabled: true 57 | Frame Timeout: 15 58 | Frames: 59 | All Enabled: false 60 | camera: 61 | Value: false 62 | camera_tf_frame: 63 | Value: false 64 | chassis: 65 | Value: false 66 | hokuyo: 67 | Value: false 68 | left_wheel: 69 | Value: false 70 | map: 71 | Value: true 72 | odom: 73 | Value: true 74 | right_wheel: 75 | Value: false 76 | robot_footprint: 77 | Value: true 78 | tag_0: 79 | Value: true 80 | tag_1: 81 | Value: true 82 | tag_111: 83 | Value: true 84 | tag_113: 85 | Value: true 86 | tag_115: 87 | Value: true 88 | tag_117: 89 | Value: true 90 | tag_119: 91 | Value: true 92 | tag_121: 93 | Value: true 94 | tag_16: 95 | Value: true 96 | tag_17: 97 | Value: true 98 | tag_18: 99 | Value: true 100 | tag_19: 101 | Value: true 102 | tag_2: 103 | Value: true 104 | tag_3: 105 | Value: true 106 | tag_30: 107 | Value: true 108 | tag_300: 109 | Value: true 110 | tag_301: 111 | Value: true 112 | tag_302: 113 | Value: true 114 | tag_303: 115 | Value: true 116 | tag_304: 117 | Value: true 118 | tag_305: 119 | Value: true 120 | tag_31: 121 | Value: true 122 | tag_310: 123 | Value: true 124 | tag_311: 125 | Value: true 126 | tag_314: 127 | Value: true 128 | tag_315: 129 | Value: true 130 | tag_316: 131 | Value: true 132 | tag_317: 133 | Value: true 134 | tag_32: 135 | Value: true 136 | tag_33: 137 | Value: true 138 | tag_332: 139 | Value: true 140 | tag_333: 141 | Value: true 142 | tag_336: 143 | Value: true 144 | tag_337: 145 | Value: true 146 | tag_338: 147 | Value: true 148 | tag_339: 149 | Value: true 150 | tag_350: 151 | Value: true 152 | tag_351: 153 | Value: true 154 | tag_352: 155 | Value: true 156 | tag_353: 157 | Value: true 158 | tag_374: 159 | Value: true 160 | tag_375: 161 | Value: true 162 | tag_394: 163 | Value: true 164 | tag_395: 165 | Value: true 166 | tag_398: 167 | Value: true 168 | tag_399: 169 | Value: true 170 | tag_4: 171 | Value: true 172 | tag_400: 173 | Value: true 174 | tag_401: 175 | Value: true 176 | tag_411: 177 | Value: true 178 | tag_413: 179 | Value: true 180 | tag_415: 181 | Value: true 182 | tag_417: 183 | Value: true 184 | tag_419: 185 | Value: true 186 | tag_421: 187 | Value: true 188 | tag_5: 189 | Value: true 190 | tag_54: 191 | Value: true 192 | tag_55: 193 | Value: true 194 | tag_58: 195 | Value: true 196 | tag_59: 197 | Value: true 198 | tag_60: 199 | Value: true 200 | tag_61: 201 | Value: true 202 | tag_76: 203 | Value: true 204 | tag_77: 205 | Value: true 206 | tag_78: 207 | Value: true 208 | tag_79: 209 | Value: true 210 | tag_92: 211 | Value: true 212 | tag_93: 213 | Value: true 214 | tag_94: 215 | Value: true 216 | tag_95: 217 | Value: true 218 | tag_96: 219 | Value: true 220 | tag_97: 221 | Value: true 222 | Marker Scale: 1 223 | Name: TF 224 | Show Arrows: false 225 | Show Axes: true 226 | Show Names: true 227 | Tree: 228 | map: 229 | odom: 230 | robot_footprint: 231 | chassis: 232 | camera: 233 | {} 234 | camera_tf_frame: 235 | {} 236 | hokuyo: 237 | {} 238 | left_wheel: 239 | {} 240 | right_wheel: 241 | {} 242 | tag_0: 243 | {} 244 | tag_1: 245 | {} 246 | tag_111: 247 | {} 248 | tag_113: 249 | {} 250 | tag_115: 251 | {} 252 | tag_117: 253 | {} 254 | tag_119: 255 | {} 256 | tag_121: 257 | {} 258 | tag_16: 259 | {} 260 | tag_17: 261 | {} 262 | tag_18: 263 | {} 264 | tag_19: 265 | {} 266 | tag_2: 267 | {} 268 | tag_3: 269 | {} 270 | tag_30: 271 | {} 272 | tag_300: 273 | {} 274 | tag_301: 275 | {} 276 | tag_302: 277 | {} 278 | tag_303: 279 | {} 280 | tag_304: 281 | {} 282 | tag_305: 283 | {} 284 | tag_31: 285 | {} 286 | tag_310: 287 | {} 288 | tag_311: 289 | {} 290 | tag_314: 291 | {} 292 | tag_315: 293 | {} 294 | tag_316: 295 | {} 296 | tag_317: 297 | {} 298 | tag_32: 299 | {} 300 | tag_33: 301 | {} 302 | tag_332: 303 | {} 304 | tag_333: 305 | {} 306 | tag_336: 307 | {} 308 | tag_337: 309 | {} 310 | tag_338: 311 | {} 312 | tag_339: 313 | {} 314 | tag_350: 315 | {} 316 | tag_351: 317 | {} 318 | tag_352: 319 | {} 320 | tag_353: 321 | {} 322 | tag_374: 323 | {} 324 | tag_375: 325 | {} 326 | tag_394: 327 | {} 328 | tag_395: 329 | {} 330 | tag_398: 331 | {} 332 | tag_399: 333 | {} 334 | tag_4: 335 | {} 336 | tag_400: 337 | {} 338 | tag_401: 339 | {} 340 | tag_411: 341 | {} 342 | tag_413: 343 | {} 344 | tag_415: 345 | {} 346 | tag_417: 347 | {} 348 | tag_419: 349 | {} 350 | tag_421: 351 | {} 352 | tag_5: 353 | {} 354 | tag_54: 355 | {} 356 | tag_55: 357 | {} 358 | tag_58: 359 | {} 360 | tag_59: 361 | {} 362 | tag_60: 363 | {} 364 | tag_61: 365 | {} 366 | tag_76: 367 | {} 368 | tag_77: 369 | {} 370 | tag_78: 371 | {} 372 | tag_79: 373 | {} 374 | tag_92: 375 | {} 376 | tag_93: 377 | {} 378 | tag_94: 379 | {} 380 | tag_95: 381 | {} 382 | tag_96: 383 | {} 384 | tag_97: 385 | {} 386 | Update Interval: 0 387 | Value: true 388 | - Alpha: 0.300000012 389 | Class: rviz/RobotModel 390 | Collision Enabled: false 391 | Enabled: true 392 | Links: 393 | All Links Enabled: true 394 | Expand Joint Details: false 395 | Expand Link Details: false 396 | Expand Tree: false 397 | Link Tree Style: Links in Alphabetic Order 398 | camera: 399 | Alpha: 1 400 | Show Axes: false 401 | Show Trail: false 402 | Value: true 403 | camera_tf_frame: 404 | Alpha: 1 405 | Show Axes: false 406 | Show Trail: false 407 | chassis: 408 | Alpha: 1 409 | Show Axes: false 410 | Show Trail: false 411 | Value: true 412 | hokuyo: 413 | Alpha: 1 414 | Show Axes: false 415 | Show Trail: false 416 | Value: true 417 | left_wheel: 418 | Alpha: 1 419 | Show Axes: false 420 | Show Trail: false 421 | Value: true 422 | right_wheel: 423 | Alpha: 1 424 | Show Axes: false 425 | Show Trail: false 426 | Value: true 427 | robot_footprint: 428 | Alpha: 1 429 | Show Axes: false 430 | Show Trail: false 431 | Name: RobotModel 432 | Robot Description: robot_description 433 | TF Prefix: "" 434 | Update Interval: 0 435 | Value: true 436 | Visual Enabled: true 437 | - Alpha: 0.699999988 438 | Class: rviz/Map 439 | Color Scheme: map 440 | Draw Behind: false 441 | Enabled: true 442 | Name: Map 443 | Topic: /map 444 | Unreliable: false 445 | Use Timestamp: false 446 | Value: true 447 | - Alpha: 1 448 | Axes Length: 1 449 | Axes Radius: 0.100000001 450 | Class: rviz/Pose 451 | Color: 255; 25; 0 452 | Enabled: true 453 | Head Length: 0.300000012 454 | Head Radius: 0.100000001 455 | Name: Navigation Goal 456 | Shaft Length: 1 457 | Shaft Radius: 0.0500000007 458 | Shape: Arrow 459 | Topic: /move_base_simple/goal 460 | Unreliable: false 461 | Value: true 462 | - Alpha: 1 463 | Class: rviz/Polygon 464 | Color: 25; 255; 0 465 | Enabled: true 466 | Name: Robot Footprint 467 | Topic: /move_base/local_costmap/footprint 468 | Unreliable: false 469 | Value: true 470 | - Alpha: 0.699999988 471 | Class: rviz/Map 472 | Color Scheme: map 473 | Draw Behind: false 474 | Enabled: true 475 | Name: Global Costmap 476 | Topic: /move_base/global_costmap/costmap 477 | Unreliable: false 478 | Use Timestamp: false 479 | Value: true 480 | - Alpha: 0.699999988 481 | Class: rviz/Map 482 | Color Scheme: costmap 483 | Draw Behind: false 484 | Enabled: true 485 | Name: Local Costmap 486 | Topic: /move_base/local_costmap/costmap 487 | Unreliable: false 488 | Use Timestamp: false 489 | Value: true 490 | - Alpha: 1 491 | Buffer Length: 1 492 | Class: rviz/Path 493 | Color: 25; 255; 0 494 | Enabled: true 495 | Head Diameter: 0.300000012 496 | Head Length: 0.200000003 497 | Length: 0.300000012 498 | Line Style: Lines 499 | Line Width: 0.0299999993 500 | Name: Global Path 501 | Offset: 502 | X: 0 503 | Y: 0 504 | Z: 0 505 | Pose Color: 255; 85; 255 506 | Pose Style: None 507 | Radius: 0.0299999993 508 | Shaft Diameter: 0.100000001 509 | Shaft Length: 0.100000001 510 | Topic: /move_base/NavfnROS/plan 511 | Unreliable: false 512 | Value: true 513 | - Alpha: 1 514 | Buffer Length: 1 515 | Class: rviz/Path 516 | Color: 255; 0; 0 517 | Enabled: true 518 | Head Diameter: 0.300000012 519 | Head Length: 0.200000003 520 | Length: 0.300000012 521 | Line Style: Lines 522 | Line Width: 0.0299999993 523 | Name: Local Path 524 | Offset: 525 | X: 0 526 | Y: 0 527 | Z: 0 528 | Pose Color: 255; 85; 255 529 | Pose Style: None 530 | Radius: 0.0299999993 531 | Shaft Diameter: 0.100000001 532 | Shaft Length: 0.100000001 533 | Topic: /move_base/TrajectoryPlannerROS/local_plan 534 | Unreliable: false 535 | Value: true 536 | Enabled: true 537 | Global Options: 538 | Background Color: 48; 48; 48 539 | Default Light: true 540 | Fixed Frame: map 541 | Frame Rate: 30 542 | Name: root 543 | Tools: 544 | - Class: rviz/Interact 545 | Hide Inactive Objects: true 546 | - Class: rviz/MoveCamera 547 | - Class: rviz/Select 548 | - Class: rviz/FocusCamera 549 | - Class: rviz/Measure 550 | - Class: rviz/SetInitialPose 551 | Topic: /initialpose 552 | - Class: rviz/SetGoal 553 | Topic: /move_base_simple/goal 554 | - Class: rviz/PublishPoint 555 | Single click: true 556 | Topic: /clicked_point 557 | Value: true 558 | Views: 559 | Current: 560 | Class: rviz/Orbit 561 | Distance: 5.05669641 562 | Enable Stereo Rendering: 563 | Stereo Eye Separation: 0.0599999987 564 | Stereo Focal Distance: 1 565 | Swap Stereo Eyes: false 566 | Value: false 567 | Focal Point: 568 | X: 1.12157452 569 | Y: 0.31264618 570 | Z: 1.02451193 571 | Focal Shape Fixed Size: true 572 | Focal Shape Size: 0.0500000007 573 | Invert Z Axis: false 574 | Name: Current View 575 | Near Clip Distance: 0.00999999978 576 | Pitch: 0.74039799 577 | Target Frame: 578 | Value: Orbit (rviz) 579 | Yaw: 5.4835844 580 | Saved: ~ 581 | Window Geometry: 582 | Displays: 583 | collapsed: false 584 | Height: 909 585 | Hide Left Dock: false 586 | Hide Right Dock: false 587 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002f5fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a000002f5000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f5fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a000002f50000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000050f000002f500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 588 | Selection: 589 | collapsed: false 590 | Time: 591 | collapsed: false 592 | Tool Properties: 593 | collapsed: false 594 | Views: 595 | collapsed: false 596 | Width: 1920 597 | X: 0 598 | Y: 0 599 | -------------------------------------------------------------------------------- /config/tag_sizes.yaml: -------------------------------------------------------------------------------- 1 | --- 2 | - 3 | id: 0 4 | frame_id: tag_0 5 | size: 0.16 6 | - 7 | id: 1 8 | frame_id: tag_1 9 | size: 0.16 10 | - 11 | id: 2 12 | frame_id: tag_2 13 | size: 0.16 14 | - 15 | id: 3 16 | frame_id: tag_3 17 | size: 0.16 18 | - 19 | id: 4 20 | frame_id: tag_4 21 | size: 0.16 22 | - 23 | id: 5 24 | frame_id: tag_5 25 | size: 0.16 26 | - 27 | id: 6 28 | frame_id: tag_6 29 | size: 0.16 30 | - 31 | id: 7 32 | frame_id: tag_7 33 | size: 0.16 34 | - 35 | id: 8 36 | frame_id: tag_8 37 | size: 0.16 38 | - 39 | id: 9 40 | frame_id: tag_9 41 | size: 0.16 42 | - 43 | id: 10 44 | frame_id: tag_10 45 | size: 0.16 46 | - 47 | id: 11 48 | frame_id: tag_11 49 | size: 0.16 50 | - 51 | id: 12 52 | frame_id: tag_12 53 | size: 0.16 54 | - 55 | id: 13 56 | frame_id: tag_13 57 | size: 0.16 58 | - 59 | id: 14 60 | frame_id: tag_14 61 | size: 0.16 62 | - 63 | id: 15 64 | frame_id: tag_15 65 | size: 0.16 66 | - 67 | id: 16 68 | frame_id: tag_16 69 | size: 0.16 70 | - 71 | id: 17 72 | frame_id: tag_17 73 | size: 0.16 74 | - 75 | id: 18 76 | frame_id: tag_18 77 | size: 0.16 78 | - 79 | id: 19 80 | frame_id: tag_19 81 | size: 0.16 82 | - 83 | id: 20 84 | frame_id: tag_20 85 | size: 0.16 86 | - 87 | id: 21 88 | frame_id: tag_21 89 | size: 0.16 90 | - 91 | id: 22 92 | frame_id: tag_22 93 | size: 0.16 94 | - 95 | id: 23 96 | frame_id: tag_23 97 | size: 0.16 98 | - 99 | id: 24 100 | frame_id: tag_24 101 | size: 0.16 102 | - 103 | id: 25 104 | frame_id: tag_25 105 | size: 0.16 106 | - 107 | id: 26 108 | frame_id: tag_26 109 | size: 0.16 110 | - 111 | id: 27 112 | frame_id: tag_27 113 | size: 0.16 114 | - 115 | id: 28 116 | frame_id: tag_28 117 | size: 0.16 118 | - 119 | id: 29 120 | frame_id: tag_29 121 | size: 0.16 122 | - 123 | id: 30 124 | frame_id: tag_30 125 | size: 0.16 126 | - 127 | id: 31 128 | frame_id: tag_31 129 | size: 0.16 130 | - 131 | id: 32 132 | frame_id: tag_32 133 | size: 0.16 134 | - 135 | id: 33 136 | frame_id: tag_33 137 | size: 0.16 138 | - 139 | id: 34 140 | frame_id: tag_34 141 | size: 0.16 142 | - 143 | id: 35 144 | frame_id: tag_35 145 | size: 0.16 146 | - 147 | id: 36 148 | frame_id: tag_36 149 | size: 0.16 150 | - 151 | id: 37 152 | frame_id: tag_37 153 | size: 0.16 154 | - 155 | id: 38 156 | frame_id: tag_38 157 | size: 0.16 158 | - 159 | id: 39 160 | frame_id: tag_39 161 | size: 0.16 162 | - 163 | id: 40 164 | frame_id: tag_40 165 | size: 0.16 166 | - 167 | id: 41 168 | frame_id: tag_41 169 | size: 0.16 170 | - 171 | id: 42 172 | frame_id: tag_42 173 | size: 0.16 174 | - 175 | id: 43 176 | frame_id: tag_43 177 | size: 0.16 178 | - 179 | id: 44 180 | frame_id: tag_44 181 | size: 0.16 182 | - 183 | id: 45 184 | frame_id: tag_45 185 | size: 0.16 186 | - 187 | id: 46 188 | frame_id: tag_46 189 | size: 0.16 190 | - 191 | id: 47 192 | frame_id: tag_47 193 | size: 0.16 194 | - 195 | id: 48 196 | frame_id: tag_48 197 | size: 0.16 198 | - 199 | id: 49 200 | frame_id: tag_49 201 | size: 0.16 202 | - 203 | id: 50 204 | frame_id: tag_50 205 | size: 0.16 206 | - 207 | id: 51 208 | frame_id: tag_51 209 | size: 0.16 210 | - 211 | id: 52 212 | frame_id: tag_52 213 | size: 0.16 214 | - 215 | id: 53 216 | frame_id: tag_53 217 | size: 0.16 218 | - 219 | id: 54 220 | frame_id: tag_54 221 | size: 0.16 222 | - 223 | id: 55 224 | frame_id: tag_55 225 | size: 0.16 226 | - 227 | id: 56 228 | frame_id: tag_56 229 | size: 0.16 230 | - 231 | id: 57 232 | frame_id: tag_57 233 | size: 0.16 234 | - 235 | id: 58 236 | frame_id: tag_58 237 | size: 0.16 238 | - 239 | id: 59 240 | frame_id: tag_59 241 | size: 0.16 242 | - 243 | id: 60 244 | frame_id: tag_60 245 | size: 0.16 246 | - 247 | id: 61 248 | frame_id: tag_61 249 | size: 0.16 250 | - 251 | id: 62 252 | frame_id: tag_62 253 | size: 0.16 254 | - 255 | id: 63 256 | frame_id: tag_63 257 | size: 0.16 258 | - 259 | id: 64 260 | frame_id: tag_64 261 | size: 0.16 262 | - 263 | id: 65 264 | frame_id: tag_65 265 | size: 0.16 266 | - 267 | id: 66 268 | frame_id: tag_66 269 | size: 0.16 270 | - 271 | id: 67 272 | frame_id: tag_67 273 | size: 0.16 274 | - 275 | id: 68 276 | frame_id: tag_68 277 | size: 0.16 278 | - 279 | id: 69 280 | frame_id: tag_69 281 | size: 0.16 282 | - 283 | id: 70 284 | frame_id: tag_70 285 | size: 0.16 286 | - 287 | id: 71 288 | frame_id: tag_71 289 | size: 0.16 290 | - 291 | id: 72 292 | frame_id: tag_72 293 | size: 0.16 294 | - 295 | id: 73 296 | frame_id: tag_73 297 | size: 0.16 298 | - 299 | id: 74 300 | frame_id: tag_74 301 | size: 0.16 302 | - 303 | id: 75 304 | frame_id: tag_75 305 | size: 0.16 306 | - 307 | id: 76 308 | frame_id: tag_76 309 | size: 0.16 310 | - 311 | id: 77 312 | frame_id: tag_77 313 | size: 0.16 314 | - 315 | id: 78 316 | frame_id: tag_78 317 | size: 0.16 318 | - 319 | id: 79 320 | frame_id: tag_79 321 | size: 0.16 322 | - 323 | id: 80 324 | frame_id: tag_80 325 | size: 0.16 326 | - 327 | id: 81 328 | frame_id: tag_81 329 | size: 0.16 330 | - 331 | id: 82 332 | frame_id: tag_82 333 | size: 0.16 334 | - 335 | id: 83 336 | frame_id: tag_83 337 | size: 0.16 338 | - 339 | id: 84 340 | frame_id: tag_84 341 | size: 0.16 342 | - 343 | id: 85 344 | frame_id: tag_85 345 | size: 0.16 346 | - 347 | id: 86 348 | frame_id: tag_86 349 | size: 0.16 350 | - 351 | id: 87 352 | frame_id: tag_87 353 | size: 0.16 354 | - 355 | id: 88 356 | frame_id: tag_88 357 | size: 0.16 358 | - 359 | id: 89 360 | frame_id: tag_89 361 | size: 0.16 362 | - 363 | id: 90 364 | frame_id: tag_90 365 | size: 0.16 366 | - 367 | id: 91 368 | frame_id: tag_91 369 | size: 0.16 370 | - 371 | id: 92 372 | frame_id: tag_92 373 | size: 0.16 374 | - 375 | id: 93 376 | frame_id: tag_93 377 | size: 0.16 378 | - 379 | id: 94 380 | frame_id: tag_94 381 | size: 0.16 382 | - 383 | id: 95 384 | frame_id: tag_95 385 | size: 0.16 386 | - 387 | id: 96 388 | frame_id: tag_96 389 | size: 0.16 390 | - 391 | id: 97 392 | frame_id: tag_97 393 | size: 0.16 394 | - 395 | id: 98 396 | frame_id: tag_98 397 | size: 0.16 398 | - 399 | id: 99 400 | frame_id: tag_99 401 | size: 0.16 402 | - 403 | id: 100 404 | frame_id: tag_100 405 | size: 0.16 406 | - 407 | id: 101 408 | frame_id: tag_101 409 | size: 0.16 410 | - 411 | id: 102 412 | frame_id: tag_102 413 | size: 0.16 414 | - 415 | id: 103 416 | frame_id: tag_103 417 | size: 0.16 418 | - 419 | id: 104 420 | frame_id: tag_104 421 | size: 0.16 422 | - 423 | id: 105 424 | frame_id: tag_105 425 | size: 0.16 426 | - 427 | id: 106 428 | frame_id: tag_106 429 | size: 0.16 430 | - 431 | id: 107 432 | frame_id: tag_107 433 | size: 0.16 434 | - 435 | id: 108 436 | frame_id: tag_108 437 | size: 0.16 438 | - 439 | id: 109 440 | frame_id: tag_109 441 | size: 0.16 442 | - 443 | id: 110 444 | frame_id: tag_110 445 | size: 0.16 446 | - 447 | id: 111 448 | frame_id: tag_111 449 | size: 0.16 450 | - 451 | id: 112 452 | frame_id: tag_112 453 | size: 0.16 454 | - 455 | id: 113 456 | frame_id: tag_113 457 | size: 0.16 458 | - 459 | id: 114 460 | frame_id: tag_114 461 | size: 0.16 462 | - 463 | id: 115 464 | frame_id: tag_115 465 | size: 0.16 466 | - 467 | id: 116 468 | frame_id: tag_116 469 | size: 0.16 470 | - 471 | id: 117 472 | frame_id: tag_117 473 | size: 0.16 474 | - 475 | id: 118 476 | frame_id: tag_118 477 | size: 0.16 478 | - 479 | id: 119 480 | frame_id: tag_119 481 | size: 0.16 482 | - 483 | id: 120 484 | frame_id: tag_120 485 | size: 0.16 486 | - 487 | id: 121 488 | frame_id: tag_121 489 | size: 0.16 490 | - 491 | id: 122 492 | frame_id: tag_122 493 | size: 0.16 494 | - 495 | id: 123 496 | frame_id: tag_123 497 | size: 0.16 498 | - 499 | id: 124 500 | frame_id: tag_124 501 | size: 0.16 502 | - 503 | id: 125 504 | frame_id: tag_125 505 | size: 0.16 506 | - 507 | id: 126 508 | frame_id: tag_126 509 | size: 0.16 510 | - 511 | id: 127 512 | frame_id: tag_127 513 | size: 0.16 514 | - 515 | id: 128 516 | frame_id: tag_128 517 | size: 0.16 518 | - 519 | id: 129 520 | frame_id: tag_129 521 | size: 0.16 522 | - 523 | id: 130 524 | frame_id: tag_130 525 | size: 0.16 526 | - 527 | id: 131 528 | frame_id: tag_131 529 | size: 0.16 530 | - 531 | id: 132 532 | frame_id: tag_132 533 | size: 0.16 534 | - 535 | id: 133 536 | frame_id: tag_133 537 | size: 0.16 538 | - 539 | id: 134 540 | frame_id: tag_134 541 | size: 0.16 542 | - 543 | id: 135 544 | frame_id: tag_135 545 | size: 0.16 546 | - 547 | id: 136 548 | frame_id: tag_136 549 | size: 0.16 550 | - 551 | id: 137 552 | frame_id: tag_137 553 | size: 0.16 554 | - 555 | id: 138 556 | frame_id: tag_138 557 | size: 0.16 558 | - 559 | id: 139 560 | frame_id: tag_139 561 | size: 0.16 562 | - 563 | id: 140 564 | frame_id: tag_140 565 | size: 0.16 566 | - 567 | id: 141 568 | frame_id: tag_141 569 | size: 0.16 570 | - 571 | id: 142 572 | frame_id: tag_142 573 | size: 0.16 574 | - 575 | id: 143 576 | frame_id: tag_143 577 | size: 0.16 578 | - 579 | id: 144 580 | frame_id: tag_144 581 | size: 0.16 582 | - 583 | id: 145 584 | frame_id: tag_145 585 | size: 0.16 586 | - 587 | id: 146 588 | frame_id: tag_146 589 | size: 0.16 590 | - 591 | id: 147 592 | frame_id: tag_147 593 | size: 0.16 594 | - 595 | id: 148 596 | frame_id: tag_148 597 | size: 0.16 598 | - 599 | id: 149 600 | frame_id: tag_149 601 | size: 0.16 602 | - 603 | id: 150 604 | frame_id: tag_150 605 | size: 0.16 606 | - 607 | id: 151 608 | frame_id: tag_151 609 | size: 0.16 610 | - 611 | id: 152 612 | frame_id: tag_152 613 | size: 0.16 614 | - 615 | id: 153 616 | frame_id: tag_153 617 | size: 0.16 618 | - 619 | id: 154 620 | frame_id: tag_154 621 | size: 0.16 622 | - 623 | id: 155 624 | frame_id: tag_155 625 | size: 0.16 626 | - 627 | id: 156 628 | frame_id: tag_156 629 | size: 0.16 630 | - 631 | id: 157 632 | frame_id: tag_157 633 | size: 0.16 634 | - 635 | id: 158 636 | frame_id: tag_158 637 | size: 0.16 638 | - 639 | id: 159 640 | frame_id: tag_159 641 | size: 0.16 642 | - 643 | id: 160 644 | frame_id: tag_160 645 | size: 0.16 646 | - 647 | id: 161 648 | frame_id: tag_161 649 | size: 0.16 650 | - 651 | id: 162 652 | frame_id: tag_162 653 | size: 0.16 654 | - 655 | id: 163 656 | frame_id: tag_163 657 | size: 0.16 658 | - 659 | id: 164 660 | frame_id: tag_164 661 | size: 0.16 662 | - 663 | id: 165 664 | frame_id: tag_165 665 | size: 0.16 666 | - 667 | id: 166 668 | frame_id: tag_166 669 | size: 0.16 670 | - 671 | id: 167 672 | frame_id: tag_167 673 | size: 0.16 674 | - 675 | id: 168 676 | frame_id: tag_168 677 | size: 0.16 678 | - 679 | id: 169 680 | frame_id: tag_169 681 | size: 0.16 682 | - 683 | id: 170 684 | frame_id: tag_170 685 | size: 0.16 686 | - 687 | id: 171 688 | frame_id: tag_171 689 | size: 0.16 690 | - 691 | id: 172 692 | frame_id: tag_172 693 | size: 0.16 694 | - 695 | id: 173 696 | frame_id: tag_173 697 | size: 0.16 698 | - 699 | id: 174 700 | frame_id: tag_174 701 | size: 0.16 702 | - 703 | id: 175 704 | frame_id: tag_175 705 | size: 0.16 706 | - 707 | id: 176 708 | frame_id: tag_176 709 | size: 0.16 710 | - 711 | id: 177 712 | frame_id: tag_177 713 | size: 0.16 714 | - 715 | id: 178 716 | frame_id: tag_178 717 | size: 0.16 718 | - 719 | id: 179 720 | frame_id: tag_179 721 | size: 0.16 722 | - 723 | id: 180 724 | frame_id: tag_180 725 | size: 0.16 726 | - 727 | id: 181 728 | frame_id: tag_181 729 | size: 0.16 730 | - 731 | id: 182 732 | frame_id: tag_182 733 | size: 0.16 734 | - 735 | id: 183 736 | frame_id: tag_183 737 | size: 0.16 738 | - 739 | id: 184 740 | frame_id: tag_184 741 | size: 0.16 742 | - 743 | id: 185 744 | frame_id: tag_185 745 | size: 0.16 746 | - 747 | id: 186 748 | frame_id: tag_186 749 | size: 0.16 750 | - 751 | id: 187 752 | frame_id: tag_187 753 | size: 0.16 754 | - 755 | id: 188 756 | frame_id: tag_188 757 | size: 0.16 758 | - 759 | id: 189 760 | frame_id: tag_189 761 | size: 0.16 762 | - 763 | id: 190 764 | frame_id: tag_190 765 | size: 0.16 766 | - 767 | id: 191 768 | frame_id: tag_191 769 | size: 0.16 770 | - 771 | id: 192 772 | frame_id: tag_192 773 | size: 0.16 774 | - 775 | id: 193 776 | frame_id: tag_193 777 | size: 0.16 778 | - 779 | id: 194 780 | frame_id: tag_194 781 | size: 0.16 782 | - 783 | id: 195 784 | frame_id: tag_195 785 | size: 0.16 786 | - 787 | id: 196 788 | frame_id: tag_196 789 | size: 0.16 790 | - 791 | id: 197 792 | frame_id: tag_197 793 | size: 0.16 794 | - 795 | id: 198 796 | frame_id: tag_198 797 | size: 0.16 798 | - 799 | id: 199 800 | frame_id: tag_199 801 | size: 0.16 802 | - 803 | id: 200 804 | frame_id: tag_200 805 | size: 0.16 806 | - 807 | id: 201 808 | frame_id: tag_201 809 | size: 0.16 810 | - 811 | id: 202 812 | frame_id: tag_202 813 | size: 0.16 814 | - 815 | id: 203 816 | frame_id: tag_203 817 | size: 0.16 818 | - 819 | id: 204 820 | frame_id: tag_204 821 | size: 0.16 822 | - 823 | id: 205 824 | frame_id: tag_205 825 | size: 0.16 826 | - 827 | id: 206 828 | frame_id: tag_206 829 | size: 0.16 830 | - 831 | id: 207 832 | frame_id: tag_207 833 | size: 0.16 834 | - 835 | id: 208 836 | frame_id: tag_208 837 | size: 0.16 838 | - 839 | id: 209 840 | frame_id: tag_209 841 | size: 0.16 842 | - 843 | id: 210 844 | frame_id: tag_210 845 | size: 0.16 846 | - 847 | id: 211 848 | frame_id: tag_211 849 | size: 0.16 850 | - 851 | id: 212 852 | frame_id: tag_212 853 | size: 0.16 854 | - 855 | id: 213 856 | frame_id: tag_213 857 | size: 0.16 858 | - 859 | id: 214 860 | frame_id: tag_214 861 | size: 0.16 862 | - 863 | id: 215 864 | frame_id: tag_215 865 | size: 0.16 866 | - 867 | id: 216 868 | frame_id: tag_216 869 | size: 0.16 870 | - 871 | id: 217 872 | frame_id: tag_217 873 | size: 0.16 874 | - 875 | id: 218 876 | frame_id: tag_218 877 | size: 0.16 878 | - 879 | id: 219 880 | frame_id: tag_219 881 | size: 0.16 882 | - 883 | id: 220 884 | frame_id: tag_220 885 | size: 0.16 886 | - 887 | id: 221 888 | frame_id: tag_221 889 | size: 0.16 890 | - 891 | id: 222 892 | frame_id: tag_222 893 | size: 0.16 894 | - 895 | id: 223 896 | frame_id: tag_223 897 | size: 0.16 898 | - 899 | id: 224 900 | frame_id: tag_224 901 | size: 0.16 902 | - 903 | id: 225 904 | frame_id: tag_225 905 | size: 0.16 906 | - 907 | id: 226 908 | frame_id: tag_226 909 | size: 0.16 910 | - 911 | id: 227 912 | frame_id: tag_227 913 | size: 0.16 914 | - 915 | id: 228 916 | frame_id: tag_228 917 | size: 0.16 918 | - 919 | id: 229 920 | frame_id: tag_229 921 | size: 0.16 922 | - 923 | id: 230 924 | frame_id: tag_230 925 | size: 0.16 926 | - 927 | id: 231 928 | frame_id: tag_231 929 | size: 0.16 930 | - 931 | id: 232 932 | frame_id: tag_232 933 | size: 0.16 934 | - 935 | id: 233 936 | frame_id: tag_233 937 | size: 0.16 938 | - 939 | id: 234 940 | frame_id: tag_234 941 | size: 0.16 942 | - 943 | id: 235 944 | frame_id: tag_235 945 | size: 0.16 946 | - 947 | id: 236 948 | frame_id: tag_236 949 | size: 0.16 950 | - 951 | id: 237 952 | frame_id: tag_237 953 | size: 0.16 954 | - 955 | id: 238 956 | frame_id: tag_238 957 | size: 0.16 958 | - 959 | id: 239 960 | frame_id: tag_239 961 | size: 0.16 962 | - 963 | id: 240 964 | frame_id: tag_240 965 | size: 0.16 966 | - 967 | id: 241 968 | frame_id: tag_241 969 | size: 0.16 970 | - 971 | id: 242 972 | frame_id: tag_242 973 | size: 0.16 974 | - 975 | id: 243 976 | frame_id: tag_243 977 | size: 0.16 978 | - 979 | id: 244 980 | frame_id: tag_244 981 | size: 0.16 982 | - 983 | id: 245 984 | frame_id: tag_245 985 | size: 0.16 986 | - 987 | id: 246 988 | frame_id: tag_246 989 | size: 0.16 990 | - 991 | id: 247 992 | frame_id: tag_247 993 | size: 0.16 994 | - 995 | id: 248 996 | frame_id: tag_248 997 | size: 0.16 998 | - 999 | id: 249 1000 | frame_id: tag_249 1001 | size: 0.16 1002 | - 1003 | id: 250 1004 | frame_id: tag_250 1005 | size: 0.16 1006 | - 1007 | id: 251 1008 | frame_id: tag_251 1009 | size: 0.16 1010 | - 1011 | id: 252 1012 | frame_id: tag_252 1013 | size: 0.16 1014 | - 1015 | id: 253 1016 | frame_id: tag_253 1017 | size: 0.16 1018 | - 1019 | id: 254 1020 | frame_id: tag_254 1021 | size: 0.16 1022 | - 1023 | id: 255 1024 | frame_id: tag_255 1025 | size: 0.16 1026 | - 1027 | id: 256 1028 | frame_id: tag_256 1029 | size: 0.16 1030 | - 1031 | id: 257 1032 | frame_id: tag_257 1033 | size: 0.16 1034 | - 1035 | id: 258 1036 | frame_id: tag_258 1037 | size: 0.16 1038 | - 1039 | id: 259 1040 | frame_id: tag_259 1041 | size: 0.16 1042 | - 1043 | id: 260 1044 | frame_id: tag_260 1045 | size: 0.16 1046 | - 1047 | id: 261 1048 | frame_id: tag_261 1049 | size: 0.16 1050 | - 1051 | id: 262 1052 | frame_id: tag_262 1053 | size: 0.16 1054 | - 1055 | id: 263 1056 | frame_id: tag_263 1057 | size: 0.16 1058 | - 1059 | id: 264 1060 | frame_id: tag_264 1061 | size: 0.16 1062 | - 1063 | id: 265 1064 | frame_id: tag_265 1065 | size: 0.16 1066 | - 1067 | id: 266 1068 | frame_id: tag_266 1069 | size: 0.16 1070 | - 1071 | id: 267 1072 | frame_id: tag_267 1073 | size: 0.16 1074 | - 1075 | id: 268 1076 | frame_id: tag_268 1077 | size: 0.16 1078 | - 1079 | id: 269 1080 | frame_id: tag_269 1081 | size: 0.16 1082 | - 1083 | id: 270 1084 | frame_id: tag_270 1085 | size: 0.16 1086 | - 1087 | id: 271 1088 | frame_id: tag_271 1089 | size: 0.16 1090 | - 1091 | id: 272 1092 | frame_id: tag_272 1093 | size: 0.16 1094 | - 1095 | id: 273 1096 | frame_id: tag_273 1097 | size: 0.16 1098 | - 1099 | id: 274 1100 | frame_id: tag_274 1101 | size: 0.16 1102 | - 1103 | id: 275 1104 | frame_id: tag_275 1105 | size: 0.16 1106 | - 1107 | id: 276 1108 | frame_id: tag_276 1109 | size: 0.16 1110 | - 1111 | id: 277 1112 | frame_id: tag_277 1113 | size: 0.16 1114 | - 1115 | id: 278 1116 | frame_id: tag_278 1117 | size: 0.16 1118 | - 1119 | id: 279 1120 | frame_id: tag_279 1121 | size: 0.16 1122 | - 1123 | id: 280 1124 | frame_id: tag_280 1125 | size: 0.16 1126 | - 1127 | id: 281 1128 | frame_id: tag_281 1129 | size: 0.16 1130 | - 1131 | id: 282 1132 | frame_id: tag_282 1133 | size: 0.16 1134 | - 1135 | id: 283 1136 | frame_id: tag_283 1137 | size: 0.16 1138 | - 1139 | id: 284 1140 | frame_id: tag_284 1141 | size: 0.16 1142 | - 1143 | id: 285 1144 | frame_id: tag_285 1145 | size: 0.16 1146 | - 1147 | id: 286 1148 | frame_id: tag_286 1149 | size: 0.16 1150 | - 1151 | id: 287 1152 | frame_id: tag_287 1153 | size: 0.16 1154 | - 1155 | id: 288 1156 | frame_id: tag_288 1157 | size: 0.16 1158 | - 1159 | id: 289 1160 | frame_id: tag_289 1161 | size: 0.16 1162 | - 1163 | id: 290 1164 | frame_id: tag_290 1165 | size: 0.16 1166 | - 1167 | id: 291 1168 | frame_id: tag_291 1169 | size: 0.16 1170 | - 1171 | id: 292 1172 | frame_id: tag_292 1173 | size: 0.16 1174 | - 1175 | id: 293 1176 | frame_id: tag_293 1177 | size: 0.16 1178 | - 1179 | id: 294 1180 | frame_id: tag_294 1181 | size: 0.16 1182 | - 1183 | id: 295 1184 | frame_id: tag_295 1185 | size: 0.16 1186 | - 1187 | id: 296 1188 | frame_id: tag_296 1189 | size: 0.16 1190 | - 1191 | id: 297 1192 | frame_id: tag_297 1193 | size: 0.16 1194 | - 1195 | id: 298 1196 | frame_id: tag_298 1197 | size: 0.16 1198 | - 1199 | id: 299 1200 | frame_id: tag_299 1201 | size: 0.16 1202 | - 1203 | id: 300 1204 | frame_id: tag_300 1205 | size: 0.16 1206 | - 1207 | id: 301 1208 | frame_id: tag_301 1209 | size: 0.16 1210 | - 1211 | id: 302 1212 | frame_id: tag_302 1213 | size: 0.16 1214 | - 1215 | id: 303 1216 | frame_id: tag_303 1217 | size: 0.16 1218 | - 1219 | id: 304 1220 | frame_id: tag_304 1221 | size: 0.16 1222 | - 1223 | id: 305 1224 | frame_id: tag_305 1225 | size: 0.16 1226 | - 1227 | id: 306 1228 | frame_id: tag_306 1229 | size: 0.16 1230 | - 1231 | id: 307 1232 | frame_id: tag_307 1233 | size: 0.16 1234 | - 1235 | id: 308 1236 | frame_id: tag_308 1237 | size: 0.16 1238 | - 1239 | id: 309 1240 | frame_id: tag_309 1241 | size: 0.16 1242 | - 1243 | id: 310 1244 | frame_id: tag_310 1245 | size: 0.16 1246 | - 1247 | id: 311 1248 | frame_id: tag_311 1249 | size: 0.16 1250 | - 1251 | id: 312 1252 | frame_id: tag_312 1253 | size: 0.16 1254 | - 1255 | id: 313 1256 | frame_id: tag_313 1257 | size: 0.16 1258 | - 1259 | id: 314 1260 | frame_id: tag_314 1261 | size: 0.16 1262 | - 1263 | id: 315 1264 | frame_id: tag_315 1265 | size: 0.16 1266 | - 1267 | id: 316 1268 | frame_id: tag_316 1269 | size: 0.16 1270 | - 1271 | id: 317 1272 | frame_id: tag_317 1273 | size: 0.16 1274 | - 1275 | id: 318 1276 | frame_id: tag_318 1277 | size: 0.16 1278 | - 1279 | id: 319 1280 | frame_id: tag_319 1281 | size: 0.16 1282 | - 1283 | id: 320 1284 | frame_id: tag_320 1285 | size: 0.16 1286 | - 1287 | id: 321 1288 | frame_id: tag_321 1289 | size: 0.16 1290 | - 1291 | id: 322 1292 | frame_id: tag_322 1293 | size: 0.16 1294 | - 1295 | id: 323 1296 | frame_id: tag_323 1297 | size: 0.16 1298 | - 1299 | id: 324 1300 | frame_id: tag_324 1301 | size: 0.16 1302 | - 1303 | id: 325 1304 | frame_id: tag_325 1305 | size: 0.16 1306 | - 1307 | id: 326 1308 | frame_id: tag_326 1309 | size: 0.16 1310 | - 1311 | id: 327 1312 | frame_id: tag_327 1313 | size: 0.16 1314 | - 1315 | id: 328 1316 | frame_id: tag_328 1317 | size: 0.16 1318 | - 1319 | id: 329 1320 | frame_id: tag_329 1321 | size: 0.16 1322 | - 1323 | id: 330 1324 | frame_id: tag_330 1325 | size: 0.16 1326 | - 1327 | id: 331 1328 | frame_id: tag_331 1329 | size: 0.16 1330 | - 1331 | id: 332 1332 | frame_id: tag_332 1333 | size: 0.16 1334 | - 1335 | id: 333 1336 | frame_id: tag_333 1337 | size: 0.16 1338 | - 1339 | id: 334 1340 | frame_id: tag_334 1341 | size: 0.16 1342 | - 1343 | id: 335 1344 | frame_id: tag_335 1345 | size: 0.16 1346 | - 1347 | id: 336 1348 | frame_id: tag_336 1349 | size: 0.16 1350 | - 1351 | id: 337 1352 | frame_id: tag_337 1353 | size: 0.16 1354 | - 1355 | id: 338 1356 | frame_id: tag_338 1357 | size: 0.16 1358 | - 1359 | id: 339 1360 | frame_id: tag_339 1361 | size: 0.16 1362 | - 1363 | id: 340 1364 | frame_id: tag_340 1365 | size: 0.16 1366 | - 1367 | id: 341 1368 | frame_id: tag_341 1369 | size: 0.16 1370 | - 1371 | id: 342 1372 | frame_id: tag_342 1373 | size: 0.16 1374 | - 1375 | id: 343 1376 | frame_id: tag_343 1377 | size: 0.16 1378 | - 1379 | id: 344 1380 | frame_id: tag_344 1381 | size: 0.16 1382 | - 1383 | id: 345 1384 | frame_id: tag_345 1385 | size: 0.16 1386 | - 1387 | id: 346 1388 | frame_id: tag_346 1389 | size: 0.16 1390 | - 1391 | id: 347 1392 | frame_id: tag_347 1393 | size: 0.16 1394 | - 1395 | id: 348 1396 | frame_id: tag_348 1397 | size: 0.16 1398 | - 1399 | id: 349 1400 | frame_id: tag_349 1401 | size: 0.16 1402 | - 1403 | id: 350 1404 | frame_id: tag_350 1405 | size: 0.16 1406 | - 1407 | id: 351 1408 | frame_id: tag_351 1409 | size: 0.16 1410 | - 1411 | id: 352 1412 | frame_id: tag_352 1413 | size: 0.16 1414 | - 1415 | id: 353 1416 | frame_id: tag_353 1417 | size: 0.16 1418 | - 1419 | id: 354 1420 | frame_id: tag_354 1421 | size: 0.16 1422 | - 1423 | id: 355 1424 | frame_id: tag_355 1425 | size: 0.16 1426 | - 1427 | id: 356 1428 | frame_id: tag_356 1429 | size: 0.16 1430 | - 1431 | id: 357 1432 | frame_id: tag_357 1433 | size: 0.16 1434 | - 1435 | id: 358 1436 | frame_id: tag_358 1437 | size: 0.16 1438 | - 1439 | id: 359 1440 | frame_id: tag_359 1441 | size: 0.16 1442 | - 1443 | id: 360 1444 | frame_id: tag_360 1445 | size: 0.16 1446 | - 1447 | id: 361 1448 | frame_id: tag_361 1449 | size: 0.16 1450 | - 1451 | id: 362 1452 | frame_id: tag_362 1453 | size: 0.16 1454 | - 1455 | id: 363 1456 | frame_id: tag_363 1457 | size: 0.16 1458 | - 1459 | id: 364 1460 | frame_id: tag_364 1461 | size: 0.16 1462 | - 1463 | id: 365 1464 | frame_id: tag_365 1465 | size: 0.16 1466 | - 1467 | id: 366 1468 | frame_id: tag_366 1469 | size: 0.16 1470 | - 1471 | id: 367 1472 | frame_id: tag_367 1473 | size: 0.16 1474 | - 1475 | id: 368 1476 | frame_id: tag_368 1477 | size: 0.16 1478 | - 1479 | id: 369 1480 | frame_id: tag_369 1481 | size: 0.16 1482 | - 1483 | id: 370 1484 | frame_id: tag_370 1485 | size: 0.16 1486 | - 1487 | id: 371 1488 | frame_id: tag_371 1489 | size: 0.16 1490 | - 1491 | id: 372 1492 | frame_id: tag_372 1493 | size: 0.16 1494 | - 1495 | id: 373 1496 | frame_id: tag_373 1497 | size: 0.16 1498 | - 1499 | id: 374 1500 | frame_id: tag_374 1501 | size: 0.16 1502 | - 1503 | id: 375 1504 | frame_id: tag_375 1505 | size: 0.16 1506 | - 1507 | id: 376 1508 | frame_id: tag_376 1509 | size: 0.16 1510 | - 1511 | id: 377 1512 | frame_id: tag_377 1513 | size: 0.16 1514 | - 1515 | id: 378 1516 | frame_id: tag_378 1517 | size: 0.16 1518 | - 1519 | id: 379 1520 | frame_id: tag_379 1521 | size: 0.16 1522 | - 1523 | id: 380 1524 | frame_id: tag_380 1525 | size: 0.16 1526 | - 1527 | id: 381 1528 | frame_id: tag_381 1529 | size: 0.16 1530 | - 1531 | id: 382 1532 | frame_id: tag_382 1533 | size: 0.16 1534 | - 1535 | id: 383 1536 | frame_id: tag_383 1537 | size: 0.16 1538 | - 1539 | id: 384 1540 | frame_id: tag_384 1541 | size: 0.16 1542 | - 1543 | id: 385 1544 | frame_id: tag_385 1545 | size: 0.16 1546 | - 1547 | id: 386 1548 | frame_id: tag_386 1549 | size: 0.16 1550 | - 1551 | id: 387 1552 | frame_id: tag_387 1553 | size: 0.16 1554 | - 1555 | id: 388 1556 | frame_id: tag_388 1557 | size: 0.16 1558 | - 1559 | id: 389 1560 | frame_id: tag_389 1561 | size: 0.16 1562 | - 1563 | id: 390 1564 | frame_id: tag_390 1565 | size: 0.16 1566 | - 1567 | id: 391 1568 | frame_id: tag_391 1569 | size: 0.16 1570 | - 1571 | id: 392 1572 | frame_id: tag_392 1573 | size: 0.16 1574 | - 1575 | id: 393 1576 | frame_id: tag_393 1577 | size: 0.16 1578 | - 1579 | id: 394 1580 | frame_id: tag_394 1581 | size: 0.16 1582 | - 1583 | id: 395 1584 | frame_id: tag_395 1585 | size: 0.16 1586 | - 1587 | id: 396 1588 | frame_id: tag_396 1589 | size: 0.16 1590 | - 1591 | id: 397 1592 | frame_id: tag_397 1593 | size: 0.16 1594 | - 1595 | id: 398 1596 | frame_id: tag_398 1597 | size: 0.16 1598 | - 1599 | id: 399 1600 | frame_id: tag_399 1601 | size: 0.16 1602 | - 1603 | id: 400 1604 | frame_id: tag_400 1605 | size: 0.16 1606 | - 1607 | id: 401 1608 | frame_id: tag_401 1609 | size: 0.16 1610 | - 1611 | id: 402 1612 | frame_id: tag_402 1613 | size: 0.16 1614 | - 1615 | id: 403 1616 | frame_id: tag_403 1617 | size: 0.16 1618 | - 1619 | id: 404 1620 | frame_id: tag_404 1621 | size: 0.16 1622 | - 1623 | id: 405 1624 | frame_id: tag_405 1625 | size: 0.16 1626 | - 1627 | id: 406 1628 | frame_id: tag_406 1629 | size: 0.16 1630 | - 1631 | id: 407 1632 | frame_id: tag_407 1633 | size: 0.16 1634 | - 1635 | id: 408 1636 | frame_id: tag_408 1637 | size: 0.16 1638 | - 1639 | id: 409 1640 | frame_id: tag_409 1641 | size: 0.16 1642 | - 1643 | id: 410 1644 | frame_id: tag_410 1645 | size: 0.16 1646 | - 1647 | id: 411 1648 | frame_id: tag_411 1649 | size: 0.16 1650 | - 1651 | id: 412 1652 | frame_id: tag_412 1653 | size: 0.16 1654 | - 1655 | id: 413 1656 | frame_id: tag_413 1657 | size: 0.16 1658 | - 1659 | id: 414 1660 | frame_id: tag_414 1661 | size: 0.16 1662 | - 1663 | id: 415 1664 | frame_id: tag_415 1665 | size: 0.16 1666 | - 1667 | id: 416 1668 | frame_id: tag_416 1669 | size: 0.16 1670 | - 1671 | id: 417 1672 | frame_id: tag_417 1673 | size: 0.16 1674 | - 1675 | id: 418 1676 | frame_id: tag_418 1677 | size: 0.16 1678 | - 1679 | id: 419 1680 | frame_id: tag_419 1681 | size: 0.16 1682 | - 1683 | id: 420 1684 | frame_id: tag_420 1685 | size: 0.16 1686 | - 1687 | id: 421 1688 | frame_id: tag_421 1689 | size: 0.16 1690 | - 1691 | id: 422 1692 | frame_id: tag_422 1693 | size: 0.16 1694 | - 1695 | id: 423 1696 | frame_id: tag_423 1697 | size: 0.16 1698 | - 1699 | id: 424 1700 | frame_id: tag_424 1701 | size: 0.16 1702 | - 1703 | id: 425 1704 | frame_id: tag_425 1705 | size: 0.16 1706 | - 1707 | id: 426 1708 | frame_id: tag_426 1709 | size: 0.16 1710 | - 1711 | id: 427 1712 | frame_id: tag_427 1713 | size: 0.16 1714 | - 1715 | id: 428 1716 | frame_id: tag_428 1717 | size: 0.16 1718 | - 1719 | id: 429 1720 | frame_id: tag_429 1721 | size: 0.16 1722 | - 1723 | id: 430 1724 | frame_id: tag_430 1725 | size: 0.16 1726 | - 1727 | id: 431 1728 | frame_id: tag_431 1729 | size: 0.16 1730 | - 1731 | id: 432 1732 | frame_id: tag_432 1733 | size: 0.16 1734 | - 1735 | id: 433 1736 | frame_id: tag_433 1737 | size: 0.16 1738 | - 1739 | id: 434 1740 | frame_id: tag_434 1741 | size: 0.16 1742 | - 1743 | id: 435 1744 | frame_id: tag_435 1745 | size: 0.16 1746 | - 1747 | id: 436 1748 | frame_id: tag_436 1749 | size: 0.16 1750 | - 1751 | id: 437 1752 | frame_id: tag_437 1753 | size: 0.16 1754 | - 1755 | id: 438 1756 | frame_id: tag_438 1757 | size: 0.16 1758 | - 1759 | id: 439 1760 | frame_id: tag_439 1761 | size: 0.16 1762 | - 1763 | id: 440 1764 | frame_id: tag_440 1765 | size: 0.16 1766 | - 1767 | id: 441 1768 | frame_id: tag_441 1769 | size: 0.16 1770 | - 1771 | id: 442 1772 | frame_id: tag_442 1773 | size: 0.16 1774 | - 1775 | id: 443 1776 | frame_id: tag_443 1777 | size: 0.16 1778 | - 1779 | id: 444 1780 | frame_id: tag_444 1781 | size: 0.16 1782 | - 1783 | id: 445 1784 | frame_id: tag_445 1785 | size: 0.16 1786 | - 1787 | id: 446 1788 | frame_id: tag_446 1789 | size: 0.16 1790 | - 1791 | id: 447 1792 | frame_id: tag_447 1793 | size: 0.16 1794 | - 1795 | id: 448 1796 | frame_id: tag_448 1797 | size: 0.16 1798 | - 1799 | id: 449 1800 | frame_id: tag_449 1801 | size: 0.16 1802 | - 1803 | id: 450 1804 | frame_id: tag_450 1805 | size: 0.16 1806 | - 1807 | id: 451 1808 | frame_id: tag_451 1809 | size: 0.16 1810 | - 1811 | id: 452 1812 | frame_id: tag_452 1813 | size: 0.16 1814 | - 1815 | id: 453 1816 | frame_id: tag_453 1817 | size: 0.16 1818 | - 1819 | id: 454 1820 | frame_id: tag_454 1821 | size: 0.16 1822 | - 1823 | id: 455 1824 | frame_id: tag_455 1825 | size: 0.16 1826 | - 1827 | id: 456 1828 | frame_id: tag_456 1829 | size: 0.16 1830 | - 1831 | id: 457 1832 | frame_id: tag_457 1833 | size: 0.16 1834 | - 1835 | id: 458 1836 | frame_id: tag_458 1837 | size: 0.16 1838 | - 1839 | id: 459 1840 | frame_id: tag_459 1841 | size: 0.16 1842 | - 1843 | id: 460 1844 | frame_id: tag_460 1845 | size: 0.16 1846 | - 1847 | id: 461 1848 | frame_id: tag_461 1849 | size: 0.16 1850 | - 1851 | id: 462 1852 | frame_id: tag_462 1853 | size: 0.16 1854 | - 1855 | id: 463 1856 | frame_id: tag_463 1857 | size: 0.16 1858 | - 1859 | id: 464 1860 | frame_id: tag_464 1861 | size: 0.16 1862 | - 1863 | id: 465 1864 | frame_id: tag_465 1865 | size: 0.16 1866 | - 1867 | id: 466 1868 | frame_id: tag_466 1869 | size: 0.16 1870 | - 1871 | id: 467 1872 | frame_id: tag_467 1873 | size: 0.16 1874 | - 1875 | id: 468 1876 | frame_id: tag_468 1877 | size: 0.16 1878 | - 1879 | id: 469 1880 | frame_id: tag_469 1881 | size: 0.16 1882 | - 1883 | id: 470 1884 | frame_id: tag_470 1885 | size: 0.16 1886 | - 1887 | id: 471 1888 | frame_id: tag_471 1889 | size: 0.16 1890 | - 1891 | id: 472 1892 | frame_id: tag_472 1893 | size: 0.16 1894 | - 1895 | id: 473 1896 | frame_id: tag_473 1897 | size: 0.16 1898 | - 1899 | id: 474 1900 | frame_id: tag_474 1901 | size: 0.16 1902 | - 1903 | id: 475 1904 | frame_id: tag_475 1905 | size: 0.16 1906 | - 1907 | id: 476 1908 | frame_id: tag_476 1909 | size: 0.16 1910 | - 1911 | id: 477 1912 | frame_id: tag_477 1913 | size: 0.16 1914 | - 1915 | id: 478 1916 | frame_id: tag_478 1917 | size: 0.16 1918 | - 1919 | id: 479 1920 | frame_id: tag_479 1921 | size: 0.16 1922 | - 1923 | id: 480 1924 | frame_id: tag_480 1925 | size: 0.16 1926 | - 1927 | id: 481 1928 | frame_id: tag_481 1929 | size: 0.16 1930 | - 1931 | id: 482 1932 | frame_id: tag_482 1933 | size: 0.16 1934 | - 1935 | id: 483 1936 | frame_id: tag_483 1937 | size: 0.16 1938 | - 1939 | id: 484 1940 | frame_id: tag_484 1941 | size: 0.16 1942 | - 1943 | id: 485 1944 | frame_id: tag_485 1945 | size: 0.16 1946 | - 1947 | id: 486 1948 | frame_id: tag_486 1949 | size: 0.16 1950 | - 1951 | id: 487 1952 | frame_id: tag_487 1953 | size: 0.16 1954 | - 1955 | id: 488 1956 | frame_id: tag_488 1957 | size: 0.16 1958 | - 1959 | id: 489 1960 | frame_id: tag_489 1961 | size: 0.16 1962 | - 1963 | id: 490 1964 | frame_id: tag_490 1965 | size: 0.16 1966 | - 1967 | id: 491 1968 | frame_id: tag_491 1969 | size: 0.16 1970 | - 1971 | id: 492 1972 | frame_id: tag_492 1973 | size: 0.16 1974 | - 1975 | id: 493 1976 | frame_id: tag_493 1977 | size: 0.16 1978 | - 1979 | id: 494 1980 | frame_id: tag_494 1981 | size: 0.16 1982 | - 1983 | id: 495 1984 | frame_id: tag_495 1985 | size: 0.16 1986 | - 1987 | id: 496 1988 | frame_id: tag_496 1989 | size: 0.16 1990 | - 1991 | id: 497 1992 | frame_id: tag_497 1993 | size: 0.16 1994 | - 1995 | id: 498 1996 | frame_id: tag_498 1997 | size: 0.16 1998 | - 1999 | id: 499 2000 | frame_id: tag_499 2001 | size: 0.16 2002 | - 2003 | id: 500 2004 | frame_id: tag_500 2005 | size: 0.16 2006 | - 2007 | id: 501 2008 | frame_id: tag_501 2009 | size: 0.16 2010 | - 2011 | id: 502 2012 | frame_id: tag_502 2013 | size: 0.16 2014 | - 2015 | id: 503 2016 | frame_id: tag_503 2017 | size: 0.16 2018 | - 2019 | id: 504 2020 | frame_id: tag_504 2021 | size: 0.16 2022 | - 2023 | id: 505 2024 | frame_id: tag_505 2025 | size: 0.16 2026 | - 2027 | id: 506 2028 | frame_id: tag_506 2029 | size: 0.16 2030 | - 2031 | id: 507 2032 | frame_id: tag_507 2033 | size: 0.16 2034 | - 2035 | id: 508 2036 | frame_id: tag_508 2037 | size: 0.16 2038 | - 2039 | id: 509 2040 | frame_id: tag_509 2041 | size: 0.16 2042 | - 2043 | id: 510 2044 | frame_id: tag_510 2045 | size: 0.16 2046 | - 2047 | id: 511 2048 | frame_id: tag_511 2049 | size: 0.16 2050 | - 2051 | id: 512 2052 | frame_id: tag_512 2053 | size: 0.16 2054 | - 2055 | id: 513 2056 | frame_id: tag_513 2057 | size: 0.16 2058 | - 2059 | id: 514 2060 | frame_id: tag_514 2061 | size: 0.16 2062 | - 2063 | id: 515 2064 | frame_id: tag_515 2065 | size: 0.16 2066 | - 2067 | id: 516 2068 | frame_id: tag_516 2069 | size: 0.16 2070 | - 2071 | id: 517 2072 | frame_id: tag_517 2073 | size: 0.16 2074 | - 2075 | id: 518 2076 | frame_id: tag_518 2077 | size: 0.16 2078 | - 2079 | id: 519 2080 | frame_id: tag_519 2081 | size: 0.16 2082 | - 2083 | id: 520 2084 | frame_id: tag_520 2085 | size: 0.16 2086 | - 2087 | id: 521 2088 | frame_id: tag_521 2089 | size: 0.16 2090 | - 2091 | id: 522 2092 | frame_id: tag_522 2093 | size: 0.16 2094 | - 2095 | id: 523 2096 | frame_id: tag_523 2097 | size: 0.16 2098 | - 2099 | id: 524 2100 | frame_id: tag_524 2101 | size: 0.16 2102 | - 2103 | id: 525 2104 | frame_id: tag_525 2105 | size: 0.16 2106 | - 2107 | id: 526 2108 | frame_id: tag_526 2109 | size: 0.16 2110 | - 2111 | id: 527 2112 | frame_id: tag_527 2113 | size: 0.16 2114 | - 2115 | id: 528 2116 | frame_id: tag_528 2117 | size: 0.16 2118 | - 2119 | id: 529 2120 | frame_id: tag_529 2121 | size: 0.16 2122 | - 2123 | id: 530 2124 | frame_id: tag_530 2125 | size: 0.16 2126 | - 2127 | id: 531 2128 | frame_id: tag_531 2129 | size: 0.16 2130 | - 2131 | id: 532 2132 | frame_id: tag_532 2133 | size: 0.16 2134 | - 2135 | id: 533 2136 | frame_id: tag_533 2137 | size: 0.16 2138 | - 2139 | id: 534 2140 | frame_id: tag_534 2141 | size: 0.16 2142 | - 2143 | id: 535 2144 | frame_id: tag_535 2145 | size: 0.16 2146 | - 2147 | id: 536 2148 | frame_id: tag_536 2149 | size: 0.16 2150 | - 2151 | id: 537 2152 | frame_id: tag_537 2153 | size: 0.16 2154 | - 2155 | id: 538 2156 | frame_id: tag_538 2157 | size: 0.16 2158 | - 2159 | id: 539 2160 | frame_id: tag_539 2161 | size: 0.16 2162 | - 2163 | id: 540 2164 | frame_id: tag_540 2165 | size: 0.16 2166 | - 2167 | id: 541 2168 | frame_id: tag_541 2169 | size: 0.16 2170 | - 2171 | id: 542 2172 | frame_id: tag_542 2173 | size: 0.16 2174 | - 2175 | id: 543 2176 | frame_id: tag_543 2177 | size: 0.16 2178 | - 2179 | id: 544 2180 | frame_id: tag_544 2181 | size: 0.16 2182 | - 2183 | id: 545 2184 | frame_id: tag_545 2185 | size: 0.16 2186 | - 2187 | id: 546 2188 | frame_id: tag_546 2189 | size: 0.16 2190 | - 2191 | id: 547 2192 | frame_id: tag_547 2193 | size: 0.16 2194 | - 2195 | id: 548 2196 | frame_id: tag_548 2197 | size: 0.16 2198 | - 2199 | id: 549 2200 | frame_id: tag_549 2201 | size: 0.16 2202 | - 2203 | id: 550 2204 | frame_id: tag_550 2205 | size: 0.16 2206 | - 2207 | id: 551 2208 | frame_id: tag_551 2209 | size: 0.16 2210 | - 2211 | id: 552 2212 | frame_id: tag_552 2213 | size: 0.16 2214 | - 2215 | id: 553 2216 | frame_id: tag_553 2217 | size: 0.16 2218 | - 2219 | id: 554 2220 | frame_id: tag_554 2221 | size: 0.16 2222 | - 2223 | id: 555 2224 | frame_id: tag_555 2225 | size: 0.16 2226 | - 2227 | id: 556 2228 | frame_id: tag_556 2229 | size: 0.16 2230 | - 2231 | id: 557 2232 | frame_id: tag_557 2233 | size: 0.16 2234 | - 2235 | id: 558 2236 | frame_id: tag_558 2237 | size: 0.16 2238 | - 2239 | id: 559 2240 | frame_id: tag_559 2241 | size: 0.16 2242 | - 2243 | id: 560 2244 | frame_id: tag_560 2245 | size: 0.16 2246 | - 2247 | id: 561 2248 | frame_id: tag_561 2249 | size: 0.16 2250 | - 2251 | id: 562 2252 | frame_id: tag_562 2253 | size: 0.16 2254 | - 2255 | id: 563 2256 | frame_id: tag_563 2257 | size: 0.16 2258 | - 2259 | id: 564 2260 | frame_id: tag_564 2261 | size: 0.16 2262 | - 2263 | id: 565 2264 | frame_id: tag_565 2265 | size: 0.16 2266 | - 2267 | id: 566 2268 | frame_id: tag_566 2269 | size: 0.16 2270 | - 2271 | id: 567 2272 | frame_id: tag_567 2273 | size: 0.16 2274 | - 2275 | id: 568 2276 | frame_id: tag_568 2277 | size: 0.16 2278 | - 2279 | id: 569 2280 | frame_id: tag_569 2281 | size: 0.16 2282 | - 2283 | id: 570 2284 | frame_id: tag_570 2285 | size: 0.16 2286 | - 2287 | id: 571 2288 | frame_id: tag_571 2289 | size: 0.16 2290 | - 2291 | id: 572 2292 | frame_id: tag_572 2293 | size: 0.16 2294 | - 2295 | id: 573 2296 | frame_id: tag_573 2297 | size: 0.16 2298 | - 2299 | id: 574 2300 | frame_id: tag_574 2301 | size: 0.16 2302 | - 2303 | id: 575 2304 | frame_id: tag_575 2305 | size: 0.16 2306 | - 2307 | id: 576 2308 | frame_id: tag_576 2309 | size: 0.16 2310 | - 2311 | id: 577 2312 | frame_id: tag_577 2313 | size: 0.16 2314 | - 2315 | id: 578 2316 | frame_id: tag_578 2317 | size: 0.16 2318 | - 2319 | id: 579 2320 | frame_id: tag_579 2321 | size: 0.16 2322 | - 2323 | id: 580 2324 | frame_id: tag_580 2325 | size: 0.16 2326 | - 2327 | id: 581 2328 | frame_id: tag_581 2329 | size: 0.16 2330 | - 2331 | id: 582 2332 | frame_id: tag_582 2333 | size: 0.16 2334 | - 2335 | id: 583 2336 | frame_id: tag_583 2337 | size: 0.16 2338 | - 2339 | id: 584 2340 | frame_id: tag_584 2341 | size: 0.16 2342 | - 2343 | id: 585 2344 | frame_id: tag_585 2345 | size: 0.16 2346 | - 2347 | id: 586 2348 | frame_id: tag_586 2349 | size: 0.16 2350 | --------------------------------------------------------------------------------