├── .gitignore ├── README.md ├── fiducial_vlam ├── CMakeLists.txt ├── cfg │ ├── Aruco Marker 6X6 250 IDs 01-12.pdf │ ├── fiducial_marker_locations_basement.yaml │ └── fiducial_marker_locations_office.yaml ├── include │ ├── convert_util.hpp │ ├── fiducial_math.hpp │ ├── fiducial_vlam │ │ └── fiducial_vlam.hpp │ ├── map.hpp │ ├── observation.hpp │ ├── transform_with_covariance.hpp │ ├── vloc_context.hpp │ └── vmap_context.hpp ├── launch │ ├── launch_one.py │ ├── launch_one.rviz │ ├── launch_one_gazebo.py │ ├── launch_two_gazebo.py │ ├── pair_box_pattern.sh │ ├── pair_box_pattern_b.sh │ ├── pair_brix_launch.py │ ├── pair_shared_launch.py │ ├── pair_xps_launch.py │ ├── sim_one_marker_launch.py │ ├── sim_yz_plane_of_markers_launch.py │ └── teleop_launch.py ├── package.xml └── src │ ├── convert_util.cpp │ ├── fiducial_math.cpp │ ├── map.cpp │ ├── transform_with_covariance.cpp │ ├── vloc_context.cpp │ ├── vloc_main.cpp │ ├── vloc_node.cpp │ ├── vmap_context.cpp │ ├── vmap_main.cpp │ └── vmap_node.cpp ├── fiducial_vlam_msgs ├── CMakeLists.txt ├── msg │ ├── Map.msg │ ├── Observation.msg │ └── Observations.msg └── package.xml └── images └── rviz_in_out_path.png /.gitignore: -------------------------------------------------------------------------------- 1 | .idea 2 | cmake-build-debug 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # fiducial_vlam 2 | ![rviz image](images/rviz_in_out_path.png) 3 | 4 | ### The map of marker poses. 5 | 6 | The map is a list of marker poses in the map frame. Each entry in the map list contains the id of 7 | the marker and the pose (x,y,z,roll,pitch,yaw) of the marker in the map frame. 8 | 9 | vloc_node takes raw_images and detects marker corners. It publishes these corner locations in a fiducial_vlam/observations 10 | message. If vloc_node has received a map, it will use marker corners 11 | from the image and marker poses from the map to determine and publish the camera pose in map frame. 12 | vmap_node will either load a map from a file and publish it or use observations messages to create and publish 13 | a new map. 14 | 15 | vmap_node parameters for the map: 16 | 17 | `make_not_use_map` 0 => load a predefined map from a file and publish it. 1 => create a new map 18 | from observations and periodically publish it and save it to a file. 19 | 20 | `marker_map_load_full_filename` the name (+path) of the file that vmap_node loads a map from when vmap_node is 21 | using an existing predefined map. (make_not_use_map==0) 22 | 23 | `marker_map_save_full_filename` the name (+path) of the file that vmap_node a discovered map to 24 | when vmap_node is discovering a new map. (make_not_use_map==1) 25 | 26 | #### Anchoring a map 27 | 28 | vmap_node creates/discovers a map by calculating the relative poses of the markers 29 | from images that contain multiple markers. These relative measurements are anchored 30 | to a particular frame by specifying the pose of one of the markers in that frame. 31 | 32 | The following parameters control how vmap_node will anchor a new map: 33 | 34 | `map_init_style` is the style of map initialization. The following styles are supported: 35 | 36 | * 0 => Load pose of one marker from a file. Look for the marker with id `map_init_id` 37 | in file `marker_map_load_full_filename` and use the pose found to anchor the map. 38 | * 1 => Load pose of one marker from parameters. Use the marker id `map_init_id` and 39 | the pose specified in paramters (`map_init_pose_x`, `map_init_pose_y`, `map_init_pose_z`, 40 | `map_init_pose_roll`, `map_init_pose_pitch`, `map_init_pose_yaw`) to anchor the map. 41 | * 2 => Initial camera pose from parameters. The parameters ( 42 | `map_init_pose_x`, `map_init_pose_y`, `map_init_pose_z`, 43 | `map_init_pose_roll`, `map_init_pose_pitch`, `map_init_pose_yaw`) specify the initial 44 | pose of the camera in the map frame. The pose of new markers in the map frame are determined 45 | based on this specified camera pose. 46 | 47 | 48 | -------------------------------------------------------------------------------- /fiducial_vlam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(fiducial_vlam) 3 | 4 | #============= 5 | # Setup 6 | #============= 7 | 8 | # Default to C99 9 | if (NOT CMAKE_C_STANDARD) 10 | set(CMAKE_C_STANDARD 99) 11 | endif () 12 | 13 | # Default to C++14 14 | if (NOT CMAKE_CXX_STANDARD) 15 | set(CMAKE_CXX_STANDARD 14) 16 | endif () 17 | 18 | # Emulate colcon 19 | if ($ENV{CLION_IDE}) 20 | message(STATUS "Running inside CLion") 21 | find_package(fastrtps_cmake_module REQUIRED) 22 | set(FastRTPS_INCLUDE_DIR "/opt/ros/foxy/include") 23 | set(FastRTPS_LIBRARY_RELEASE "/opt/ros/foxy/lib/libfastrtps.so") 24 | set(fiducial_vlam_msgs_DIR "${PROJECT_SOURCE_DIR}/../../../install/fiducial_vlam_msgs/share/fiducial_vlam_msgs/cmake") 25 | set(ros2_shared_DIR "${PROJECT_SOURCE_DIR}/../../../install/ros2_shared/share/ros2_shared/cmake") 26 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DRUN_INSIDE_CLION") 27 | endif () 28 | 29 | # Try for OpenCV 4.X, but settle for whatever is installed 30 | find_package(OpenCV 4 QUIET) 31 | if (NOT OpenCV_FOUND) 32 | find_package(OpenCV REQUIRED) 33 | endif () 34 | message(STATUS "Found OpenCV version ${OpenCV_VERSION}") 35 | 36 | # Find packages 37 | find_package(ament_cmake REQUIRED) 38 | find_package(class_loader REQUIRED) 39 | find_package(cv_bridge REQUIRED) 40 | find_package(fiducial_vlam_msgs REQUIRED) 41 | find_package(geometry_msgs REQUIRED) 42 | find_package(nav_msgs REQUIRED) 43 | find_package(OpenCV REQUIRED) 44 | find_package(rclcpp REQUIRED) 45 | find_package(rclcpp_components REQUIRED) 46 | find_package(ros2_shared REQUIRED) 47 | find_package(sensor_msgs REQUIRED) 48 | find_package(std_msgs REQUIRED) 49 | find_package(tf2_msgs REQUIRED) 50 | find_package(visualization_msgs REQUIRED) 51 | find_package(yaml_cpp_vendor REQUIRED) 52 | 53 | # Local includes 54 | include_directories( 55 | include 56 | ) 57 | 58 | # Debugging: set _dump_all_variables to true 59 | set(_dump_all_variables false) 60 | if (_dump_all_variables) 61 | get_cmake_property(_variable_names VARIABLES) 62 | list(SORT _variable_names) 63 | foreach (_variable_name ${_variable_names}) 64 | message(STATUS "${_variable_name}=${${_variable_name}}") 65 | endforeach () 66 | endif () 67 | 68 | # Create ament index resource which references the libraries in the binary dir 69 | set(node_plugins "") 70 | 71 | #============= 72 | # vloc node 73 | #============= 74 | 75 | set(VLOC_NODE_SOURCES 76 | src/convert_util.cpp 77 | src/fiducial_math.cpp 78 | src/map.cpp 79 | src/transform_with_covariance.cpp 80 | src/vloc_context.cpp 81 | src/vloc_node.cpp 82 | ) 83 | 84 | set(VLOC_NODE_DEPS 85 | class_loader 86 | cv_bridge 87 | fiducial_vlam_msgs 88 | nav_msgs 89 | OpenCV 90 | rclcpp 91 | ros2_shared 92 | sensor_msgs 93 | std_msgs 94 | tf2_msgs 95 | ) 96 | 97 | add_library(vloc_node SHARED 98 | ${VLOC_NODE_SOURCES}) 99 | 100 | target_compile_definitions(vloc_node 101 | PRIVATE "COMPOSITION_BUILDING_DLL") 102 | 103 | ament_target_dependencies(vloc_node 104 | ${VLOC_NODE_DEPS}) 105 | 106 | rclcpp_components_register_nodes(vloc_node "fiducial_vlam::VlocNode") 107 | set(node_plugins "${node_plugins}fiducial_vlam::VlocNode;$\n") 108 | 109 | add_executable(vloc_main 110 | src/vloc_main.cpp 111 | ${VLOC_NODE_SOURCES}) 112 | 113 | ament_target_dependencies(vloc_main 114 | ${VLOC_NODE_DEPS}) 115 | 116 | 117 | #============= 118 | # vmap node 119 | #============= 120 | 121 | set(VMAP_NODE_SOURCES 122 | src/convert_util.cpp 123 | src/fiducial_math.cpp 124 | src/map.cpp 125 | src/transform_with_covariance.cpp 126 | src/vmap_context.cpp 127 | src/vmap_main.cpp 128 | src/vmap_node.cpp 129 | ) 130 | 131 | set(VMAP_NODE_DEPS 132 | class_loader 133 | fiducial_vlam_msgs 134 | geometry_msgs 135 | OpenCV 136 | rclcpp 137 | ros2_shared 138 | std_msgs 139 | tf2_msgs 140 | visualization_msgs 141 | yaml_cpp_vendor 142 | ) 143 | 144 | add_library(vmap_node SHARED 145 | ${VMAP_NODE_SOURCES}) 146 | 147 | target_compile_definitions(vmap_node 148 | PRIVATE "COMPOSITION_BUILDING_DLL") 149 | 150 | ament_target_dependencies(vmap_node 151 | ${VMAP_NODE_DEPS}) 152 | 153 | rclcpp_components_register_nodes(vmap_node "fiducial_vlam::VmapNode") 154 | set(node_plugins "${node_plugins}fiducial_vlam::VmapNode;$\n") 155 | 156 | add_executable(vmap_main 157 | src/vmap_main.cpp 158 | ${VMAP_NODE_SOURCES}) 159 | 160 | ament_target_dependencies(vmap_main 161 | ${VMAP_NODE_DEPS}) 162 | 163 | 164 | #============= 165 | # Install 166 | #============= 167 | 168 | # Install composable nodes 169 | install(TARGETS 170 | vloc_node 171 | EXPORT export_vloc_node 172 | ARCHIVE DESTINATION lib 173 | LIBRARY DESTINATION lib 174 | RUNTIME DESTINATION bin 175 | ) 176 | 177 | install(TARGETS 178 | vmap_node 179 | EXPORT export_vmap_node 180 | ARCHIVE DESTINATION lib 181 | LIBRARY DESTINATION lib 182 | RUNTIME DESTINATION bin 183 | ) 184 | 185 | # Install executables 186 | install(TARGETS 187 | vloc_main 188 | vmap_main 189 | DESTINATION lib/fiducial_vlam 190 | ) 191 | 192 | # Install various directories 193 | install(DIRECTORY 194 | cfg 195 | launch 196 | DESTINATION share/fiducial_vlam 197 | ) 198 | 199 | install(DIRECTORY include/ 200 | DESTINATION include) 201 | 202 | 203 | #============= 204 | # Export 205 | #============= 206 | 207 | ament_export_dependencies(class_loader yaml_cpp_vendor) 208 | 209 | ament_export_include_directories(include) 210 | 211 | ament_export_targets(export_vloc_node export_vmap_node) 212 | 213 | ament_export_libraries(vloc_node vmap_node) 214 | 215 | 216 | #============= 217 | # Run ament macros 218 | #============= 219 | 220 | ament_package() 221 | -------------------------------------------------------------------------------- /fiducial_vlam/cfg/Aruco Marker 6X6 250 IDs 01-12.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ptrmu/fiducial_vlam/e76988d50e6e18ef45be825f198b3434d21fb155/fiducial_vlam/cfg/Aruco Marker 6X6 250 IDs 01-12.pdf -------------------------------------------------------------------------------- /fiducial_vlam/cfg/fiducial_marker_locations_basement.yaml: -------------------------------------------------------------------------------- 1 | marker_length: 0.1627 2 | markers: 3 | - id: 1 4 | u: 1 5 | f: 1 6 | xyz: [0, 0, 1] 7 | rpy: [1.570796326794896, -0, -1.570796326794896] 8 | - id: 2 9 | u: 1897 10 | f: 0 11 | xyz: [-0.02653022998108209, -6.954878997442631e-05, 2.013202028986516] 12 | rpy: [1.581178348787372, 0.01057880779362467, -1.560689871827545] 13 | - id: 3 14 | u: 3516 15 | f: 0 16 | xyz: [-0.01401636351437458, 1.014215528529495, 0.998845980523124] 17 | rpy: [1.575631334853308, -0.005618761949098817, -1.562566649431042] 18 | - id: 4 19 | u: 2791 20 | f: 0 21 | xyz: [-0.04598833462800195, 1.009243681725574, 2.017101074715447] 22 | rpy: [1.582380207547849, 0.009337080640085134, -1.561340362824014] 23 | - id: 5 24 | u: 3057 25 | f: 0 26 | xyz: [-0.01112748580850191, 2.026331128259423, 1.000020556641283] 27 | rpy: [1.584694142484779, 0.003457746954671916, -1.588485084180686] 28 | - id: 6 29 | u: 2363 30 | f: 0 31 | xyz: [-0.04566997338290761, 2.020140094223674, 2.024098573444329] 32 | rpy: [1.587170151105369, 0.007596491257322736, -1.592053539112061] 33 | - id: 7 34 | u: 1535 35 | f: 0 36 | xyz: [-0.01208226016502214, 3.033917315344317, 1.007885532828511] 37 | rpy: [1.588713288205935, -0.00192979926689251, -1.584657123431803] 38 | - id: 8 39 | u: 1323 40 | f: 0 41 | xyz: [-0.03334000351212729, 2.972784888326166, 2.207262272191881] 42 | rpy: [1.583039127886098, 0.007014665424738883, -1.595106312577197] 43 | - id: 9 44 | u: 1182 45 | f: 0 46 | xyz: [-0.09118022482599314, 4.033065445418707, 1.014158810294987] 47 | rpy: [1.585121943368021, -0.006178578269072981, -1.17026169081691] 48 | - id: 10 49 | u: 1144 50 | f: 0 51 | xyz: [-0.1317549667202581, 4.035738620395656, 2.033658152789891] 52 | rpy: [1.57633819211611, -0.001330222781123172, -1.159621747926497] 53 | - id: 11 54 | u: 933 55 | f: 0 56 | xyz: [-0.2485276070582368, 4.965895494265617, 1.010300653150616] 57 | rpy: [1.586688396690514, -0.0001568507458028423, -1.968091696592979] 58 | - id: 12 59 | u: 907 60 | f: 0 61 | xyz: [-0.2989926257286504, 4.963344675416578, 2.031117735885434] 62 | rpy: [1.582431609827186, 0.007906305349180395, -1.989098064868657] 63 | -------------------------------------------------------------------------------- /fiducial_vlam/cfg/fiducial_marker_locations_office.yaml: -------------------------------------------------------------------------------- 1 | marker_length: 0.1627 2 | markers: 3 | - id: 1 4 | u: 1 5 | f: 1 6 | xyz: [0, 0, 1] 7 | rpy: [1.570796326794896, -0, -1.570796326794896] 8 | - id: 2 9 | u: 541 10 | f: 0 11 | xyz: [0.02064524438009353, -1.062418001236005, 0.8735973257612596] 12 | rpy: [1.563614596470964, 0.003929610867118493, -1.580761591311048] 13 | - id: 3 14 | u: 834 15 | f: 0 16 | xyz: [0.01036851084070141, -0.5916792239813753, 0.3744306581403723] 17 | rpy: [0.274308910496085, 1.561850712189244, -2.868789140171121] 18 | - id: 4 19 | u: 517 20 | f: 0 21 | xyz: [0.01896209520309319, -1.059256726550293, 0.3312923109755858] 22 | rpy: [1.575948551755873, 0.006561336131302975, -1.571946584502854] 23 | - id: 5 24 | u: 679 25 | f: 0 26 | xyz: [0.003390555416090765, 0.008504475666190472, 0.3413293123646054] 27 | rpy: [1.554723958261568, 0.004549680030806985, -1.572845279868765] 28 | - id: 6 29 | u: 866 30 | f: 0 31 | xyz: [0.01110710529976326, -0.534142037266441, 0.771679476891493] 32 | rpy: [1.573478304353794, 0.003786446601181967, -1.574211433545589] 33 | - id: 7 34 | u: 619 35 | f: 0 36 | xyz: [-0.06791512930650077, 0.7498013720539867, 0.9896690217785548] 37 | rpy: [1.573742021489329, 0.03079142270030887, -1.536700597183157] 38 | - id: 8 39 | u: 391 40 | f: 0 41 | xyz: [-0.07159028453847051, 0.7993117718415581, 0.390980319226344] 42 | rpy: [1.56662103898105, 0.03059781007686646, -1.543229715946889] 43 | - id: 9 44 | u: 866 45 | f: 0 46 | xyz: [-0.06517363467035722, 0.4045152336451856, 0.6703771275359636] 47 | rpy: [1.576931319908283, 0.03391115674189793, -1.558102761702406] 48 | - id: 10 49 | u: 357 50 | f: 0 51 | xyz: [0.005621624403634105, -0.5372051711371885, 1.46458437632637] 52 | rpy: [1.564222733961314, 0.004720302277312763, -1.573223612353407] 53 | - id: 11 54 | u: 272 55 | f: 0 56 | xyz: [-0.001572237943289769, -0.01824716033424388, 1.640575579807485] 57 | rpy: [1.573712957922386, 0.008144293469313469, -1.585057950439028] 58 | - id: 12 59 | u: 245 60 | f: 0 61 | xyz: [0.01802535976286517, -1.073203416583014, 1.61166937083316] 62 | rpy: [1.569290741568493, 0.01972949401789535, -1.580381464155223] 63 | -------------------------------------------------------------------------------- /fiducial_vlam/include/convert_util.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef FIDUCIAL_VLAM_TF_UTIL_HPP 3 | #define FIDUCIAL_VLAM_TF_UTIL_HPP 4 | 5 | #include "geometry_msgs/msg/pose.hpp" 6 | #include "geometry_msgs/msg/pose_with_covariance.hpp" 7 | #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" 8 | 9 | namespace fiducial_vlam 10 | { 11 | class TransformWithCovariance; 12 | 13 | geometry_msgs::msg::Pose to_Pose_msg(const TransformWithCovariance &twc); 14 | 15 | geometry_msgs::msg::PoseWithCovariance to_PoseWithCovariance_msg(const TransformWithCovariance &twc); 16 | 17 | geometry_msgs::msg::PoseWithCovarianceStamped to_PoseWithCovarianceStamped_msg( 18 | const TransformWithCovariance &twc, 19 | std_msgs::msg::Header::_stamp_type stamp, 20 | std_msgs::msg::Header::_frame_id_type frame_id); 21 | 22 | TransformWithCovariance to_TransformWithCovariance(const geometry_msgs::msg::PoseWithCovariance &pwc); 23 | } 24 | #endif //FIDUCIAL_VLAM_TF_UTIL_HPP 25 | -------------------------------------------------------------------------------- /fiducial_vlam/include/fiducial_math.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FIDUCIAL_VLAM_FIDUCIAL_MATH_HPP 2 | #define FIDUCIAL_VLAM_FIDUCIAL_MATH_HPP 3 | 4 | 5 | #include 6 | 7 | #include "sensor_msgs/msg/camera_info.hpp" 8 | 9 | namespace cv_bridge 10 | { 11 | class CvImage; 12 | } 13 | 14 | namespace fiducial_vlam 15 | { 16 | class Observation; 17 | 18 | class Observations; 19 | 20 | class TransformWithCovariance; 21 | 22 | // ============================================================================== 23 | // CameraInfo class 24 | // ============================================================================== 25 | 26 | class CameraInfo 27 | { 28 | class CvCameraInfo; 29 | 30 | std::shared_ptr cv_; 31 | 32 | public: 33 | CameraInfo(); 34 | 35 | explicit CameraInfo(const sensor_msgs::msg::CameraInfo &camera_info); 36 | 37 | auto &cv() const 38 | { return cv_; } 39 | 40 | bool is_valid() const 41 | { return cv_ != nullptr; } 42 | }; 43 | 44 | // ============================================================================== 45 | // FiducialMath class 46 | // ============================================================================== 47 | 48 | class FiducialMath 49 | { 50 | class CvFiducialMath; 51 | 52 | std::shared_ptr cv_; 53 | 54 | public: 55 | explicit FiducialMath(const CameraInfo &camera_info); 56 | 57 | explicit FiducialMath(const sensor_msgs::msg::CameraInfo &camera_info_msg); 58 | 59 | TransformWithCovariance solve_t_camera_marker(const Observation &observation, double marker_length); 60 | 61 | TransformWithCovariance solve_t_map_camera(const Observations &observations, 62 | const std::vector &t_map_markers, 63 | double marker_length); 64 | 65 | Observations detect_markers(std::shared_ptr &gray, 66 | std::shared_ptr &color_marked, 67 | int corner_refinement_method); 68 | 69 | void annotate_image_with_marker_axis(std::shared_ptr &color, 70 | const TransformWithCovariance &t_camera_marker); 71 | }; 72 | } 73 | 74 | #endif //FIDUCIAL_VLAM_FIDUCIAL_MATH_HPP 75 | -------------------------------------------------------------------------------- /fiducial_vlam/include/fiducial_vlam/fiducial_vlam.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FIDUCIAL_VLAM_FIDUCIAL_VLAM_HPP 2 | #define FIDUCIAL_VLAM_FIDUCIAL_VLAM_HPP 3 | 4 | #include 5 | 6 | namespace rclcpp 7 | { 8 | class Node; 9 | 10 | class NodeOptions; 11 | } 12 | 13 | namespace fiducial_vlam 14 | { 15 | std::shared_ptr vloc_node_factory(const rclcpp::NodeOptions &options); 16 | 17 | std::shared_ptr vmap_node_factory(const rclcpp::NodeOptions &options); 18 | } 19 | #endif //FIDUCIAL_VLAM_FIDUCIAL_VLAM_HPP 20 | -------------------------------------------------------------------------------- /fiducial_vlam/include/map.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FIDUCIAL_VLAM_MAP_H 2 | #define FIDUCIAL_VLAM_MAP_H 3 | 4 | #include 5 | 6 | #include "convert_util.hpp" 7 | #include "transform_with_covariance.hpp" 8 | 9 | #include "fiducial_vlam_msgs/msg/map.hpp" 10 | 11 | // coordinate frame conventions 12 | // t_destination_source is a transformation from source frame to destination frame 13 | // xxx_f_destination means xxx is expressed in destination frame 14 | 15 | namespace cv_bridge 16 | { 17 | class CvImage; 18 | } 19 | 20 | namespace fiducial_vlam 21 | { 22 | class FiducialMath; 23 | 24 | class Observations; 25 | 26 | // ============================================================================== 27 | // Marker class 28 | // ============================================================================== 29 | 30 | class Marker 31 | { 32 | // The id of the marker 33 | int id_{}; 34 | 35 | // The pose of the marker in the map frame 36 | TransformWithCovariance t_map_marker_; 37 | 38 | // Prevent modification if true 39 | bool is_fixed_{false}; 40 | 41 | // Count of updates 42 | int update_count_{}; 43 | 44 | public: 45 | Marker() = default; 46 | 47 | Marker(int id, TransformWithCovariance t_map_marker) 48 | : id_(id), t_map_marker_(std::move(t_map_marker)), update_count_(1) 49 | {} 50 | 51 | auto id() const 52 | { return id_; } 53 | 54 | auto is_fixed() const 55 | { return is_fixed_; } 56 | 57 | void set_is_fixed(bool is_fixed) 58 | { is_fixed_ = is_fixed; } 59 | 60 | auto update_count() const 61 | { return update_count_; } 62 | 63 | void set_update_count(int update_count) 64 | { update_count_ = update_count; } 65 | 66 | const auto &t_map_marker() const 67 | { return t_map_marker_; } 68 | 69 | void set_t_map_marker(TransformWithCovariance t_map_marker) 70 | { t_map_marker_ = std::move(t_map_marker); } 71 | }; 72 | 73 | // ============================================================================== 74 | // Map class 75 | // ============================================================================== 76 | 77 | class Map 78 | { 79 | std::map markers_; 80 | double marker_length_; 81 | 82 | public: 83 | Map() = delete; 84 | 85 | explicit Map(double marker_length_); 86 | 87 | explicit Map(const fiducial_vlam_msgs::msg::Map &msg); 88 | 89 | const auto &markers() const 90 | { return markers_; } 91 | 92 | auto marker_length() const 93 | { return marker_length_; } 94 | 95 | Marker *find_marker(int id); 96 | 97 | void add_marker(Marker marker); 98 | 99 | std::unique_ptr 100 | to_map_msg(const std_msgs::msg::Header &header_msg, double marker_length); 101 | 102 | std::vector find_t_map_markers(const Observations &observations); 103 | }; 104 | 105 | // ============================================================================== 106 | // Localizer class 107 | // ============================================================================== 108 | 109 | class Localizer 110 | { 111 | const std::unique_ptr &map_; 112 | 113 | public: 114 | explicit Localizer(const std::unique_ptr &map); 115 | 116 | TransformWithCovariance average_t_map_camera(const Observations &observations, 117 | const std::vector &t_map_markers, 118 | std::shared_ptr &color_marked, 119 | FiducialMath &fm); 120 | 121 | TransformWithCovariance simultaneous_t_map_camera(const Observations &observations, 122 | const std::vector &t_map_markers, 123 | std::shared_ptr &color_marked, 124 | FiducialMath &fm); 125 | 126 | std::vector markers_t_map_cameras( 127 | const Observations &observations, 128 | const std::vector &t_map_markers, 129 | FiducialMath &fm); 130 | }; 131 | 132 | // ============================================================================== 133 | // Utility 134 | // ============================================================================== 135 | 136 | // void log_tf_transform(rclcpp::Node &node, std::string s, const tf2::Transform &transform); 137 | 138 | } // namespace fiducial_vlam 139 | 140 | #endif //FIDUCIAL_VLAM_MAP_H 141 | -------------------------------------------------------------------------------- /fiducial_vlam/include/observation.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FIDUCIAL_VLAM_OBSERVATION_HPP 2 | #define FIDUCIAL_VLAM_OBSERVATION_HPP 3 | 4 | #include 5 | 6 | #include "fiducial_vlam_msgs/msg/observations.hpp" 7 | 8 | namespace fiducial_vlam 9 | { 10 | // ============================================================================== 11 | // Observation class 12 | // ============================================================================== 13 | 14 | class Observation 15 | { 16 | // The id of the marker that we observed. 17 | int id_; 18 | 19 | double x0_, y0_; 20 | double x1_, y1_; 21 | double x2_, y2_; 22 | double x3_, y3_; 23 | 24 | // The 2D pixel coordinates of the corners in the image. 25 | // The corners need to be in the same order as is returned 26 | // from cv::aruco::detectMarkers(). 27 | // std::vector corners_f_image_; 28 | 29 | public: 30 | Observation(int id, 31 | double x0, double y0, 32 | double x1, double y1, 33 | double x2, double y2, 34 | double x3, double y3) 35 | : id_(id), 36 | x0_(x0), y0_(y0), 37 | x1_(x1), y1_(y1), 38 | x2_(x2), y2_(y2), 39 | x3_(x3), y3_(y3) 40 | {} 41 | 42 | explicit Observation(const fiducial_vlam_msgs::msg::Observation &msg) 43 | : id_(msg.id), 44 | x0_(msg.x0), y0_(msg.y0), 45 | x1_(msg.x1), y1_(msg.y1), 46 | x2_(msg.x2), y2_(msg.y2), 47 | x3_(msg.x3), y3_(msg.y3) 48 | {} 49 | 50 | auto id() const 51 | { return id_; } 52 | 53 | auto x0() const 54 | { return x0_; } 55 | 56 | auto x1() const 57 | { return x1_; } 58 | 59 | auto x2() const 60 | { return x2_; } 61 | 62 | auto x3() const 63 | { return x3_; } 64 | 65 | auto y0() const 66 | { return y0_; } 67 | 68 | auto y1() const 69 | { return y1_; } 70 | 71 | auto y2() const 72 | { return y2_; } 73 | 74 | auto y3() const 75 | { return y3_; } 76 | }; 77 | 78 | // ============================================================================== 79 | // Observations class 80 | // ============================================================================== 81 | 82 | class Observations 83 | { 84 | // The list of observations 85 | std::vector observations_; 86 | 87 | public: 88 | Observations() = default; 89 | 90 | explicit Observations(const fiducial_vlam_msgs::msg::Observations &msg); 91 | 92 | const auto &observations() const 93 | { return observations_; } 94 | 95 | auto size() const 96 | { return observations_.size(); } 97 | 98 | void add(const Observation observation) 99 | { 100 | observations_.emplace_back(observation); 101 | } 102 | 103 | fiducial_vlam_msgs::msg::Observations to_msg(std_msgs::msg::Header::_stamp_type stamp, 104 | const std_msgs::msg::Header::_frame_id_type &frame_id, 105 | const sensor_msgs::msg::CameraInfo &camera_info_msg); 106 | }; 107 | 108 | 109 | } 110 | #endif //FIDUCIAL_VLAM_OBSERVATION_HPP 111 | -------------------------------------------------------------------------------- /fiducial_vlam/include/transform_with_covariance.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FIDUCIAL_VLAM_TRANSFORM_WITH_COVARIANCE_HPP 2 | #define FIDUCIAL_VLAM_TRANSFORM_WITH_COVARIANCE_HPP 3 | 4 | #include 5 | 6 | #include "tf2/LinearMath/Transform.h" 7 | 8 | namespace fiducial_vlam 9 | { 10 | 11 | class TransformWithCovariance 12 | { 13 | public: 14 | using mu_type = std::array; 15 | using cov_type = std::array; 16 | 17 | private: 18 | bool is_valid_{false}; 19 | tf2::Transform transform_; 20 | cov_type cov_{}; 21 | 22 | static tf2::Transform to_transform(const mu_type &mu) 23 | { 24 | tf2::Quaternion q; 25 | q.setRPY(mu[3], mu[4], mu[5]); 26 | return tf2::Transform(q, tf2::Vector3(mu[0], mu[1], mu[2])); 27 | } 28 | 29 | public: 30 | TransformWithCovariance() = default; 31 | 32 | TransformWithCovariance(const tf2::Transform &transform, const cov_type &cov) 33 | : is_valid_(true), transform_(transform), cov_(cov) 34 | {} 35 | 36 | explicit TransformWithCovariance(const tf2::Transform &transform) 37 | : is_valid_(true), transform_(transform), cov_() 38 | {} 39 | 40 | TransformWithCovariance(const mu_type &mu, const cov_type &cov) 41 | : is_valid_(true), transform_(to_transform(mu)), cov_(cov) 42 | {} 43 | 44 | explicit TransformWithCovariance(const mu_type &mu) 45 | : is_valid_(true), transform_(to_transform(mu)), cov_() 46 | {} 47 | 48 | explicit TransformWithCovariance(const tf2::Quaternion &q) 49 | : is_valid_(true), transform_(q), cov_() 50 | {} 51 | 52 | TransformWithCovariance(const tf2::Quaternion &q, tf2::Vector3 &c) 53 | : is_valid_(true), transform_(q, c), cov_() 54 | {} 55 | 56 | auto is_valid() const 57 | { return is_valid_; } 58 | 59 | auto &transform() const 60 | { return transform_; } 61 | 62 | auto &cov() const 63 | { return cov_; } 64 | 65 | mu_type mu() const 66 | { 67 | double roll, pitch, yaw; 68 | transform_.getBasis().getRPY(roll, pitch, yaw); 69 | auto c = transform_.getOrigin(); 70 | return mu_type{c[0], c[1], c[2], roll, pitch, yaw}; 71 | } 72 | 73 | void update_simple_average(const TransformWithCovariance &newVal, int previous_update_count); 74 | }; 75 | 76 | } 77 | 78 | #endif //FIDUCIAL_VLAM_TRANSFORM_WITH_COVARIANCE_HPP 79 | -------------------------------------------------------------------------------- /fiducial_vlam/include/vloc_context.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FIDUCIAL_VLAM_VLOC_CONTEXT_HPP 2 | #define FIDUCIAL_VLAM_VLOC_CONTEXT_HPP 3 | 4 | #include 5 | 6 | #include "ros2_shared/context_macros.hpp" 7 | #include "transform_with_covariance.hpp" 8 | 9 | namespace rclcpp 10 | { 11 | class Node; 12 | } 13 | 14 | namespace fiducial_vlam 15 | { 16 | #define VLOC_ALL_PARAMS \ 17 | CXT_MACRO_MEMBER( /* topic for publishing fiducial observations */ \ 18 | fiducial_observations_pub_topic, \ 19 | std::string, "/fiducial_observations") \ 20 | CXT_MACRO_MEMBER( /* topic for publishing camera pose */ \ 21 | camera_pose_pub_topic, \ 22 | std::string, "camera_pose") \ 23 | CXT_MACRO_MEMBER( /* topic for publishing base pose */ \ 24 | base_pose_pub_topic, \ 25 | std::string, "base_pose") \ 26 | CXT_MACRO_MEMBER( /* topic for publishing camera odometry */ \ 27 | camera_odometry_pub_topic, \ 28 | std::string, "camera_odom") \ 29 | CXT_MACRO_MEMBER( /* topic for publishing base odometry */ \ 30 | base_odometry_pub_topic, \ 31 | std::string, "base_odom") \ 32 | CXT_MACRO_MEMBER( /* topic for republishing the image with axes added to fiducial markers */\ 33 | image_marked_pub_topic, \ 34 | std::string, "image_marked") \ 35 | \ 36 | CXT_MACRO_MEMBER( /* topic for subscription to fiducial_vlam_msgs::msg::Map */\ 37 | fiducial_map_sub_topic, \ 38 | std::string, "/fiducial_map") \ 39 | CXT_MACRO_MEMBER( /* topic for subscription to sensor_msgs::msg::CameraInfo associated with the image */ \ 40 | camera_info_sub_topic, \ 41 | std::string, "camera_info") \ 42 | CXT_MACRO_MEMBER( /* topic for subscription to sensor_msgs::msg::Image */ \ 43 | image_raw_sub_topic, \ 44 | std::string, "image_raw") \ 45 | \ 46 | CXT_MACRO_MEMBER( /* frame_id for camera pose and tf messages - normally "map" */ \ 47 | map_frame_id, \ 48 | std::string, "map") \ 49 | CXT_MACRO_MEMBER( /* frame_id for the child in the camera tf message */\ 50 | camera_frame_id, \ 51 | std::string, "camera") \ 52 | CXT_MACRO_MEMBER( /* frame_id for the child in the base_link tf message */\ 53 | base_frame_id, \ 54 | std::string, "base_link") \ 55 | \ 56 | CXT_MACRO_MEMBER( /* non-zero => publish the pose of the camera at every frame */ \ 57 | publish_camera_pose, \ 58 | int, 1) \ 59 | CXT_MACRO_MEMBER( /* non-zero => publish the pose of the base at every frame */ \ 60 | publish_base_pose, \ 61 | int, 1) \ 62 | CXT_MACRO_MEMBER( /* non-zero => publish the tf of the camera at every frame */ \ 63 | publish_tfs, \ 64 | int, 1) \ 65 | CXT_MACRO_MEMBER( /* non-zero => publish the camera tf/pose as determined by each visible marker */ \ 66 | publish_tfs_per_marker, \ 67 | int, 1) \ 68 | CXT_MACRO_MEMBER( /* non-zero => publish the odometry of the camera at every frame */ \ 69 | publish_camera_odom, \ 70 | int, 1) \ 71 | CXT_MACRO_MEMBER( /* non-zero => publish the odometry of the base at every frame */ \ 72 | publish_base_odom, \ 73 | int, 1) \ 74 | CXT_MACRO_MEMBER( /* non-zero => publish the image_marked at every frame */ \ 75 | publish_image_marked, \ 76 | int, 1) \ 77 | CXT_MACRO_MEMBER( /* non-zero => debug mode, helpful for dealing with rviz when playing bags. */ \ 78 | stamp_msgs_with_current_time, \ 79 | int, 0) \ 80 | \ 81 | CXT_MACRO_MEMBER( /* camera=>baselink transform component */ \ 82 | t_camera_base_x, \ 83 | double, 0.) \ 84 | CXT_MACRO_MEMBER( /* camera=>baselink transform component */ \ 85 | t_camera_base_y, \ 86 | double, 0.) \ 87 | CXT_MACRO_MEMBER( /* camera=>baselink transform component */ \ 88 | t_camera_base_z, \ 89 | double, -0.035) \ 90 | CXT_MACRO_MEMBER( /* camera=>baselink transform component */ \ 91 | t_camera_base_roll, \ 92 | double, TF2SIMD_HALF_PI) \ 93 | CXT_MACRO_MEMBER( /* camera=>baselink transform component */ \ 94 | t_camera_base_pitch, \ 95 | double, -TF2SIMD_HALF_PI) \ 96 | CXT_MACRO_MEMBER( /* camera=>baselink transform component */ \ 97 | t_camera_base_yaw, \ 98 | double, 0.) \ 99 | \ 100 | CXT_MACRO_MEMBER( /* subscribe to camera_info message with best_effort (gazebo camera, tello_driver) */ \ 101 | sub_camera_info_best_effort_not_reliable, \ 102 | int, 1) \ 103 | CXT_MACRO_MEMBER( /* subscribe to image_raw message with best_effort (gazebo camera, tello_driver) */ \ 104 | sub_image_raw_best_effort_not_reliable, \ 105 | int, 1) \ 106 | \ 107 | CXT_MACRO_MEMBER( /* opencv4 aruco corner refinement method */ \ 108 | corner_refinement_method, \ 109 | int, 2) \ 110 | /* End of list */ 111 | 112 | struct VlocContext 113 | { 114 | rclcpp::Node &node_; 115 | 116 | explicit VlocContext(rclcpp::Node &node) : 117 | node_{node} 118 | {} 119 | 120 | #undef CXT_MACRO_MEMBER 121 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 122 | CXT_MACRO_DEFINE_MEMBERS(VLOC_ALL_PARAMS) 123 | TransformWithCovariance t_camera_base_{}; 124 | 125 | void load_parameters(); 126 | 127 | void validate_parameters(); 128 | }; 129 | } 130 | 131 | #endif //FIDUCIAL_VLAM_VLOC_CONTEXT_HPP 132 | -------------------------------------------------------------------------------- /fiducial_vlam/include/vmap_context.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FIDUCIAL_VLAM_VMAP_CONTEXT_HPP 2 | #define FIDUCIAL_VLAM_VMAP_CONTEXT_HPP 3 | 4 | #include "ros2_shared/context_macros.hpp" 5 | #include "transform_with_covariance.hpp" 6 | 7 | namespace rclcpp 8 | { 9 | class Node; 10 | } 11 | 12 | namespace fiducial_vlam 13 | { 14 | #define VMAP_ALL_PARAMS \ 15 | CXT_MACRO_MEMBER( /* topic for publishing map of markers */ \ 16 | fiducial_map_pub_topic, \ 17 | std::string, "/fiducial_map") \ 18 | CXT_MACRO_MEMBER( /* topic for publishing rviz visualizations of the fiducial markers */ \ 19 | fiducial_markers_pub_topic, \ 20 | std::string, "fiducial_markers") \ 21 | \ 22 | CXT_MACRO_MEMBER( /* topic for subscription to fiducial_vlam_msgs::msg::Observations */ \ 23 | fiducial_observations_sub_topic, \ 24 | std::string, "/fiducial_observations") \ 25 | \ 26 | CXT_MACRO_MEMBER( /* frame_id for marker and tf messages - normally "map" */ \ 27 | map_frame_id, \ 28 | std::string, "map") \ 29 | CXT_MACRO_MEMBER( /* frame_id for the child in the marker tf message */\ 30 | marker_prefix_frame_id, \ 31 | std::string, "marker_") \ 32 | \ 33 | CXT_MACRO_MEMBER( /* non-zero => publish the tf of all the known markers */ \ 34 | publish_tfs, \ 35 | int, 1) \ 36 | CXT_MACRO_MEMBER( /* non-zero => publish a shape that represents a marker */ \ 37 | publish_marker_visualizations, \ 38 | int, 1) \ 39 | CXT_MACRO_MEMBER( /* Hz => rate at which the marker map is published */ \ 40 | marker_map_publish_frequency_hz, \ 41 | double, 0.) \ 42 | \ 43 | CXT_MACRO_MEMBER( /* name of the file to store the marker map in */ \ 44 | marker_map_save_full_filename, \ 45 | std::string, "fiducial_marker_locations.yaml") \ 46 | CXT_MACRO_MEMBER( /* name of the file to load the marker map from */ \ 47 | marker_map_load_full_filename, \ 48 | std::string, "fiducial_marker_locations_saved.yaml") \ 49 | CXT_MACRO_MEMBER( /* non-zero => create a new map */\ 50 | make_not_use_map, \ 51 | int, 1) \ 52 | CXT_MACRO_MEMBER( /* 0->marker id, pose from file, 1->marker id, pose as parameter, 2->camera pose as parameter */ \ 53 | map_init_style, \ 54 | int, 1) \ 55 | CXT_MACRO_MEMBER( /* marker id for map initialization */ \ 56 | map_init_id, \ 57 | int, 1) \ 58 | CXT_MACRO_MEMBER( /* pose component for map initialization */ \ 59 | map_init_pose_x, \ 60 | double, 0.) \ 61 | CXT_MACRO_MEMBER( /* pose component for map initialization */ \ 62 | map_init_pose_y, \ 63 | double, 0.) \ 64 | CXT_MACRO_MEMBER( /* pose component for map initialization */ \ 65 | map_init_pose_z, \ 66 | double, 1.) \ 67 | CXT_MACRO_MEMBER( /* pose component for map initialization */ \ 68 | map_init_pose_roll, \ 69 | double, TF2SIMD_HALF_PI) \ 70 | CXT_MACRO_MEMBER( /* pose component for map initialization */ \ 71 | map_init_pose_pitch, \ 72 | double, 0.) \ 73 | CXT_MACRO_MEMBER( /* pose component for map initialization */ \ 74 | map_init_pose_yaw, \ 75 | double, -TF2SIMD_HALF_PI) \ 76 | CXT_MACRO_MEMBER( /* length of a side of a marker in meters */ \ 77 | marker_length, \ 78 | double, 0.1627) \ 79 | /* End of list */ 80 | 81 | struct VmapContext 82 | { 83 | rclcpp::Node &node_; 84 | 85 | explicit VmapContext(rclcpp::Node &node) : 86 | node_{node} 87 | {} 88 | 89 | #undef CXT_MACRO_MEMBER 90 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_DEFINE_MEMBER(n, t, d) 91 | CXT_MACRO_DEFINE_MEMBERS(VMAP_ALL_PARAMS) 92 | TransformWithCovariance map_init_transform_{}; 93 | 94 | void load_parameters(); 95 | 96 | void validate_parameters(); 97 | }; 98 | } 99 | 100 | #endif //FIDUCIAL_VLAM_VMAP_CONTEXT_HPP 101 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/launch_one.py: -------------------------------------------------------------------------------- 1 | """One Tello drone, using ArUco markers and fiducial_vlam for localization""" 2 | 3 | import os 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch_ros.actions import Node 8 | from launch.actions import ExecuteProcess 9 | 10 | 11 | def generate_launch_description(): 12 | # 1 or more drones: 13 | drones = ['drone1'] 14 | 15 | tello_ros_args = [ 16 | [{ 17 | 'drone_ip': '192.168.0.21', 18 | }], 19 | ] 20 | 21 | # Starting locations: 22 | starting_locations = [ 23 | # Face the wall of markers in fiducial.world 24 | # ['-2.5', '1.5', '1', '0'], 25 | # ['-1.5', '0.5', '2', '0.785'], 26 | # ['-0.5', '1.5', '1', '0'], 27 | # ['-1.5', '2.5', '.5', '-0.785'] 28 | 29 | ['-1.5', '1.5', '1', '0'], 30 | 31 | # Face all 4 directions in f2.world 32 | # ['-2.5', '1.5', '1', '0'], 33 | # ['-1.5', '0.5', '1', '1.57'], 34 | # ['-0.5', '1.5', '1', '3.14'], 35 | # ['-1.5', '2.5', '1', '-1.57'] 36 | ] 37 | 38 | fiducial_vlam_path = get_package_share_directory('fiducial_vlam') 39 | tello_description_path = get_package_share_directory('tello_description') 40 | 41 | map_path = os.path.join(fiducial_vlam_path, 'launch', 'fiducial_marker_locations_office.yaml') 42 | rviz_config_path = os.path.join(fiducial_vlam_path, 'launch', 'launch_one.rviz') 43 | 44 | # Global entities 45 | entities = [ 46 | # Rviz 47 | ExecuteProcess(cmd=['rviz2', '-d', rviz_config_path], output='screen'), 48 | 49 | # # Launch Gazebo, loading tello.world 50 | # ExecuteProcess(cmd=[ 51 | # 'gazebo', 52 | # '--verbose', 53 | # '-s', 'libgazebo_ros_init.so', # Publish /clock 54 | # '-s', 'libgazebo_ros_factory.so', # Provide gazebo_ros::Node 55 | # world_path 56 | # ], output='screen'), 57 | 58 | # Load and publish a known map 59 | Node(package='fiducial_vlam', node_executable='vmap_main', output='screen', 60 | node_name='vmap_main', parameters=[{ 61 | 'use_sim_time': False, # Use /clock if available 62 | 'publish_tfs': 1, # Publish marker /tf 63 | 'marker_length': 0.1778, # Marker length 64 | 'marker_map_load_full_filename': map_path, # Load a pre-built map from disk 65 | 'make_not_use_map': 0 # Don't save a map to disk 66 | }]), 67 | 68 | # Joystick driver, generates /namespace/joy messages 69 | Node(package='joy', node_executable='joy_node', output='screen', 70 | node_name='joy_node', parameters=[{ 71 | 'use_sim_time': False, # Use /clock if available 72 | }]), 73 | 74 | # Flock controller (basically a joystick multiplexer, also starts/stops missions) 75 | Node(package='flock2', node_executable='flock_base', output='screen', 76 | node_name='flock_base', parameters=[{ 77 | 'use_sim_time': False, # Use /clock if available 78 | 'drones': drones 79 | }]), 80 | 81 | # WIP: planner 82 | Node(package='flock2', node_executable='planner_node', output='screen', 83 | node_name='planner_node', parameters=[{ 84 | 'use_sim_time': False, # Use /clock if available 85 | 'drones': drones, 86 | 'arena_x': -5.0, 87 | 'arena_y': -5.0, 88 | 'arena_z': 10.0, 89 | }]), 90 | ] 91 | 92 | # Per-drone entities 93 | for idx, namespace in enumerate(drones): 94 | suffix = '_' + str(idx + 1) 95 | urdf_path = os.path.join(tello_description_path, 'urdf', 'tello' + suffix + '.urdf') 96 | 97 | entities.extend([ 98 | # # Add a drone to the simulation 99 | # Node(package='tello_gazebo', node_executable='inject_entity.py', output='screen', 100 | # arguments=[urdf_path]+starting_locations[idx]), 101 | 102 | # # Publish base_link to camera_link tf 103 | # Node(package='robot_state_publisher', node_executable='robot_state_publisher', output='screen', 104 | # node_name=namespace+'_tf_pub', arguments=[urdf_path], parameters=[{ 105 | # 'use_sim_time': False, # Use /clock if available 106 | # }]), 107 | # 108 | # Localize this drone against the map 109 | # Future: need odometry for base_link, not camera_link 110 | Node(package='fiducial_vlam', node_executable='vloc_main', output='screen', 111 | node_name='vloc_main', node_namespace=namespace, parameters=[{ 112 | 'use_sim_time': False, # Use /clock if available 113 | 'publish_tfs': 1, # Publish drone and camera /tf 114 | 'stamp_msgs_with_current_time': 0, # Use incoming message time, not now() 115 | 'base_frame_id': 'base_link' + suffix, 116 | 'map_init_pose_z': -0.035, 117 | 'camera_frame_id': 'camera_link' + suffix, 118 | 'base_odometry_pub_topic': 'filtered_odom', 119 | 'sub_camera_info_best_effort_not_reliable': 1, 120 | 'publish_image_marked': 1, 121 | }]), 122 | 123 | # Odometry filter takes camera pose, generates base_link odom, and publishes map to base_link tf 124 | # Node(package='odom_filter', node_executable='filter_node', output='screen', 125 | # node_name='filter_node', node_namespace=namespace, parameters=[{ 126 | # 'use_sim_time': True, # Use /clock if available 127 | # 'map_frame': 'map', 128 | # 'base_frame': 'base_link' + suffix 129 | # }]), 130 | 131 | # Driver 132 | Node(package='tello_driver', node_executable='tello_driver', output='screen', 133 | node_name='tello_driver', node_namespace=namespace, 134 | parameters=tello_ros_args[idx]), 135 | 136 | # Drone controller 137 | Node(package='flock2', node_executable='drone_base', output='screen', 138 | node_name='drone_base', node_namespace=namespace, parameters=[{ 139 | 'use_sim_time': False, # Use /clock if available 140 | 'pid_x_kp': 0.5, 141 | 'pid_x_ks': 1.0, 142 | 'pid_y_kp': 0.5, 143 | 'pid_y_ks': 1.0, 144 | 'pid_z_kp': 0.2, 145 | 'pid_z_ks': 0.0, 146 | 'pid_yaw_kp': 0.0, 147 | 'pid_yaw_ks': 0.0, 148 | 'close_enough_xyz': 0.15, 149 | }]), 150 | ]) 151 | 152 | return LaunchDescription(entities) 153 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/launch_one.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: ~ 7 | Splitter Ratio: 0.5 8 | Tree Height: 710 9 | - Class: rviz_common/Selection 10 | Name: Selection 11 | - Class: rviz_common/Tool Properties 12 | Expanded: 13 | - /2D Nav Goal1 14 | - /Publish Point1 15 | Name: Tool Properties 16 | Splitter Ratio: 0.5886790156364441 17 | - Class: rviz_common/Views 18 | Expanded: 19 | - /Current View1 20 | Name: Views 21 | Splitter Ratio: 0.5 22 | Visualization Manager: 23 | Class: "" 24 | Displays: 25 | - Alpha: 0.5 26 | Cell Size: 1 27 | Class: rviz_default_plugins/Grid 28 | Color: 160; 160; 164 29 | Enabled: true 30 | Line Style: 31 | Line Width: 0.029999999329447746 32 | Value: Lines 33 | Name: Grid 34 | Normal Cell Count: 0 35 | Offset: 36 | X: 0 37 | Y: 0 38 | Z: 0 39 | Plane: XY 40 | Plane Cell Count: 10 41 | Reference Frame: 42 | Value: true 43 | - Class: rviz_default_plugins/Image 44 | Enabled: true 45 | Max Value: 1 46 | Median window: 5 47 | Min Value: 0 48 | Name: Image 49 | Normalize Range: true 50 | Queue Size: 10 51 | Topic: /drone1/image_marked 52 | Unreliable: false 53 | Value: true 54 | - Class: rviz_default_plugins/TF 55 | Enabled: true 56 | Frame Timeout: 15 57 | Frames: 58 | All Enabled: true 59 | base_link_1: 60 | Value: true 61 | camera_link_1: 62 | Value: true 63 | map: 64 | Value: true 65 | marker_001: 66 | Value: true 67 | marker_002: 68 | Value: true 69 | marker_003: 70 | Value: true 71 | marker_004: 72 | Value: true 73 | marker_005: 74 | Value: true 75 | marker_006: 76 | Value: true 77 | marker_007: 78 | Value: true 79 | marker_008: 80 | Value: true 81 | marker_009: 82 | Value: true 83 | marker_010: 84 | Value: true 85 | marker_011: 86 | Value: true 87 | marker_012: 88 | Value: true 89 | Marker Scale: 1 90 | Name: TF 91 | Show Arrows: false 92 | Show Axes: true 93 | Show Names: false 94 | Tree: 95 | map: 96 | base_link_1: 97 | {} 98 | camera_link_1: 99 | {} 100 | marker_001: 101 | {} 102 | marker_002: 103 | {} 104 | marker_003: 105 | {} 106 | marker_004: 107 | {} 108 | marker_005: 109 | {} 110 | marker_006: 111 | {} 112 | marker_007: 113 | {} 114 | marker_008: 115 | {} 116 | marker_009: 117 | {} 118 | marker_010: 119 | {} 120 | marker_011: 121 | {} 122 | marker_012: 123 | {} 124 | Update Interval: 0 125 | Value: true 126 | Enabled: true 127 | Global Options: 128 | Background Color: 48; 48; 48 129 | Fixed Frame: map 130 | Frame Rate: 30 131 | Name: root 132 | Tools: 133 | - Class: rviz_default_plugins/MoveCamera 134 | - Class: rviz_default_plugins/Select 135 | - Class: rviz_default_plugins/FocusCamera 136 | - Class: rviz_default_plugins/Measure 137 | Line color: 128; 128; 0 138 | - Class: rviz_default_plugins/SetInitialPose 139 | Topic: /initialpose 140 | - Class: rviz_default_plugins/SetGoal 141 | Topic: /move_base_simple/goal 142 | - Class: rviz_default_plugins/PublishPoint 143 | Single click: true 144 | Topic: /clicked_point 145 | Transformation: 146 | Current: 147 | Class: rviz_default_plugins/TF 148 | Value: true 149 | Views: 150 | Current: 151 | Class: rviz_default_plugins/Orbit 152 | Distance: 10 153 | Enable Stereo Rendering: 154 | Stereo Eye Separation: 0.05999999865889549 155 | Stereo Focal Distance: 1 156 | Swap Stereo Eyes: false 157 | Value: false 158 | Focal Point: 159 | X: 0 160 | Y: 0 161 | Z: 0 162 | Focal Shape Fixed Size: true 163 | Focal Shape Size: 0.05000000074505806 164 | Invert Z Axis: false 165 | Name: Current View 166 | Near Clip Distance: 0.009999999776482582 167 | Pitch: 0.6053981781005859 168 | Target Frame: 169 | Value: Orbit (rviz) 170 | Yaw: 3.730405330657959 171 | Saved: ~ 172 | Window Geometry: 173 | Displays: 174 | collapsed: false 175 | Height: 1154 176 | Hide Left Dock: false 177 | Hide Right Dock: false 178 | Image: 179 | collapsed: false 180 | QMainWindow State: 000000ff00000000fd0000000400000000000001bf00000428fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000351000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000394000000d10000002800ffffff000000010000010f00000428fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000428000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004a00000042800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 181 | Selection: 182 | collapsed: false 183 | Tool Properties: 184 | collapsed: false 185 | Views: 186 | collapsed: false 187 | Width: 1637 188 | X: 67 189 | Y: 60 190 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/launch_one_gazebo.py: -------------------------------------------------------------------------------- 1 | """Simulate one or more Tello drones in Gazebo, using ArUco markers and fiducial_vlam for localization""" 2 | 3 | import os 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch_ros.actions import Node 8 | from launch.actions import ExecuteProcess 9 | 10 | 11 | def generate_launch_description(): 12 | # 1 or more drones: 13 | drones = ['drone1'] 14 | 15 | # Starting locations: 16 | starting_locations = [ 17 | # Face the wall of markers in fiducial.world 18 | # ['-2.5', '1.5', '1', '0'], 19 | # ['-1.5', '0.5', '2', '0.785'], 20 | # ['-0.5', '1.5', '1', '0'], 21 | # ['-1.5', '2.5', '.5', '-0.785'] 22 | 23 | ['-1.5', '1.5', '1', '0'], 24 | 25 | # Face all 4 directions in f2.world 26 | # ['-2.5', '1.5', '1', '0'], 27 | # ['-1.5', '0.5', '1', '1.57'], 28 | # ['-0.5', '1.5', '1', '3.14'], 29 | # ['-1.5', '2.5', '1', '-1.57'] 30 | ] 31 | 32 | tello_gazebo_path = get_package_share_directory('tello_gazebo') 33 | tello_description_path = get_package_share_directory('tello_description') 34 | 35 | world_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial.world') 36 | map_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial_map.yaml') 37 | 38 | # Global entities 39 | entities = [ 40 | # Launch Gazebo, loading tello.world 41 | ExecuteProcess(cmd=[ 42 | 'gazebo', 43 | '--verbose', 44 | '-s', 'libgazebo_ros_init.so', # Publish /clock 45 | '-s', 'libgazebo_ros_factory.so', # Provide gazebo_ros::Node 46 | world_path 47 | ], output='screen'), 48 | 49 | # Load and publish a known map 50 | Node(package='fiducial_vlam', node_executable='vmap_main', output='screen', 51 | node_name='vmap_main', parameters=[{ 52 | 'use_sim_time': True, # Use /clock if available 53 | 'publish_tfs': 1, # Publish marker /tf 54 | 'marker_length': 0.1778, # Marker length 55 | 'marker_map_load_full_filename': map_path, # Load a pre-built map from disk 56 | 'make_not_use_map': 0 # Don't save a map to disk 57 | }]), 58 | 59 | # Joystick driver, generates /namespace/joy messages 60 | Node(package='joy', node_executable='joy_node', output='screen', 61 | node_name='joy_node', parameters=[{ 62 | 'use_sim_time': True, # Use /clock if available 63 | }]), 64 | 65 | # Flock controller (basically a joystick multiplexer, also starts/stops missions) 66 | Node(package='flock2', node_executable='flock_base', output='screen', 67 | node_name='flock_base', parameters=[{ 68 | 'use_sim_time': True, # Use /clock if available 69 | 'drones': drones 70 | }]), 71 | 72 | # WIP: planner 73 | Node(package='flock2', node_executable='planner_node', output='screen', 74 | node_name='planner_node', parameters=[{ 75 | 'use_sim_time': True, # Use /clock if available 76 | 'drones': drones, 77 | 'arena_x': -5.0, 78 | 'arena_y': -5.0, 79 | 'arena_z': 10.0, 80 | }]), 81 | ] 82 | 83 | # Per-drone entities 84 | for idx, namespace in enumerate(drones): 85 | suffix = '_' + str(idx + 1) 86 | urdf_path = os.path.join(tello_description_path, 'urdf', 'tello' + suffix + '.urdf') 87 | 88 | entities.extend([ 89 | # Add a drone to the simulation 90 | Node(package='tello_gazebo', node_executable='inject_entity.py', output='screen', 91 | arguments=[urdf_path]+starting_locations[idx]), 92 | 93 | # Publish base_link to camera_link tf 94 | # Node(package='robot_state_publisher', node_executable='robot_state_publisher', output='screen', 95 | # node_name=namespace+'_tf_pub', arguments=[urdf_path], parameters=[{ 96 | # 'use_sim_time': True, # Use /clock if available 97 | # }]), 98 | 99 | # Localize this drone against the map 100 | # Future: need odometry for base_link, not camera_link 101 | Node(package='fiducial_vlam', node_executable='vloc_main', output='screen', 102 | node_name='vloc_main', node_namespace=namespace, parameters=[{ 103 | 'use_sim_time': True, # Use /clock if available 104 | 'publish_tfs': 1, # Publish drone and camera /tf 105 | 'stamp_msgs_with_current_time': 0, # Use incoming message time, not now() 106 | 'base_frame_id': 'base_link' + suffix, 107 | 'map_init_pose_z': -0.035, 108 | 'camera_frame_id': 'camera_link' + suffix, 109 | 'base_odometry_pub_topic': 'filtered_odom', 110 | 'sub_camera_info_best_effort_not_reliable': 1, 111 | }]), 112 | 113 | # Odometry filter takes camera pose, generates base_link odom, and publishes map to base_link tf 114 | # Node(package='odom_filter', node_executable='filter_node', output='screen', 115 | # node_name='filter_node', node_namespace=namespace, parameters=[{ 116 | # 'use_sim_time': True, # Use /clock if available 117 | # 'map_frame': 'map', 118 | # 'base_frame': 'base_link' + suffix 119 | # }]), 120 | 121 | # Drone controller 122 | Node(package='flock2', node_executable='drone_base', output='screen', 123 | node_name='drone_base', node_namespace=namespace, parameters=[{ 124 | 'use_sim_time': True, # Use /clock if available 125 | 'pid_x_kp': 0.2, 126 | 'pid_x_ks': 0.12, 127 | 'pid_y_kp': 0.2, 128 | 'pid_y_ks': 0.12, 129 | 'pid_z_kp': 0.2, 130 | 'pid_z_ks': 0.12, 131 | 'close_enough_xyz': 0.05, 132 | }]), 133 | ]) 134 | 135 | return LaunchDescription(entities) 136 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/launch_two_gazebo.py: -------------------------------------------------------------------------------- 1 | """Simulate one or more Tello drones in Gazebo, using ArUco markers and fiducial_vlam for localization""" 2 | 3 | import os 4 | 5 | from ament_index_python.packages import get_package_share_directory 6 | from launch import LaunchDescription 7 | from launch_ros.actions import Node 8 | from launch.actions import ExecuteProcess 9 | 10 | 11 | def generate_launch_description(): 12 | # 1 or more drones: 13 | drones = ['drone1', 'drone2'] 14 | 15 | # Starting locations: 16 | starting_locations = [ 17 | # Face the wall of markers in fiducial.world 18 | # ['-2.5', '1.5', '1', '0'], 19 | # ['-1.5', '0.5', '2', '0.785'], 20 | # ['-0.5', '1.5', '1', '0'], 21 | # ['-1.5', '2.5', '.5', '-0.785'] 22 | 23 | ['-1.5', '0.5', '2', '0.785'], 24 | ['-0.5', '1.5', '1', '0'], 25 | 26 | # Face all 4 directions in f2.world 27 | # ['-2.5', '1.5', '1', '0'], 28 | # ['-1.5', '0.5', '1', '1.57'], 29 | # ['-0.5', '1.5', '1', '3.14'], 30 | # ['-1.5', '2.5', '1', '-1.57'] 31 | ] 32 | 33 | tello_gazebo_path = get_package_share_directory('tello_gazebo') 34 | tello_description_path = get_package_share_directory('tello_description') 35 | 36 | world_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial.world') 37 | map_path = os.path.join(tello_gazebo_path, 'worlds', 'fiducial_map.yaml') 38 | 39 | # Global entities 40 | entities = [ 41 | # Launch Gazebo, loading tello.world 42 | ExecuteProcess(cmd=[ 43 | 'gazebo', 44 | '--verbose', 45 | '-s', 'libgazebo_ros_init.so', # Publish /clock 46 | '-s', 'libgazebo_ros_factory.so', # Provide gazebo_ros::Node 47 | world_path 48 | ], output='screen'), 49 | 50 | # Load and publish a known map 51 | Node(package='fiducial_vlam', node_executable='vmap_main', output='screen', 52 | node_name='vmap_main', parameters=[{ 53 | 'use_sim_time': True, # Use /clock if available 54 | 'publish_tfs': 1, # Publish marker /tf 55 | 'marker_length': 0.1778, # Marker length 56 | 'marker_map_load_full_filename': map_path, # Load a pre-built map from disk 57 | 'make_not_use_map': 0 # Don't save a map to disk 58 | }]), 59 | 60 | # Joystick driver, generates /namespace/joy messages 61 | Node(package='joy', node_executable='joy_node', output='screen', 62 | node_name='joy_node', parameters=[{ 63 | 'use_sim_time': True, # Use /clock if available 64 | }]), 65 | 66 | # Flock controller (basically a joystick multiplexer, also starts/stops missions) 67 | Node(package='flock2', node_executable='flock_base', output='screen', 68 | node_name='flock_base', parameters=[{ 69 | 'use_sim_time': True, # Use /clock if available 70 | 'drones': drones 71 | }]), 72 | 73 | # WIP: planner 74 | Node(package='flock2', node_executable='planner_node', output='screen', 75 | node_name='planner_node', parameters=[{ 76 | 'use_sim_time': True, # Use /clock if available 77 | 'drones': drones, 78 | 'arena_x': -5.0, 79 | 'arena_y': -5.0, 80 | 'arena_z': 10.0, 81 | }]), 82 | ] 83 | 84 | # Per-drone entities 85 | for idx, namespace in enumerate(drones): 86 | suffix = '_' + str(idx + 1) 87 | urdf_path = os.path.join(tello_description_path, 'urdf', 'tello' + suffix + '.urdf') 88 | 89 | entities.extend([ 90 | # Add a drone to the simulation 91 | Node(package='tello_gazebo', node_executable='inject_entity.py', output='screen', 92 | arguments=[urdf_path]+starting_locations[idx]), 93 | 94 | # Publish base_link to camera_link tf 95 | # Node(package='robot_state_publisher', node_executable='robot_state_publisher', output='screen', 96 | # node_name=namespace+'_tf_pub', arguments=[urdf_path], parameters=[{ 97 | # 'use_sim_time': True, # Use /clock if available 98 | # }]), 99 | 100 | # Localize this drone against the map 101 | # Future: need odometry for base_link, not camera_link 102 | Node(package='fiducial_vlam', node_executable='vloc_main', output='screen', 103 | node_name='vloc_main', node_namespace=namespace, parameters=[{ 104 | 'use_sim_time': True, # Use /clock if available 105 | 'publish_tfs': 1, # Publish drone and camera /tf 106 | 'stamp_msgs_with_current_time': 0, # Use incoming message time, not now() 107 | 'base_frame_id': 'base_link' + suffix, 108 | 'map_init_pose_z': -0.035, 109 | 'camera_frame_id': 'camera_link' + suffix, 110 | 'base_odometry_pub_topic': 'filtered_odom', 111 | }]), 112 | 113 | # Odometry filter takes camera pose, generates base_link odom, and publishes map to base_link tf 114 | # Node(package='odom_filter', node_executable='filter_node', output='screen', 115 | # node_name='filter_node', node_namespace=namespace, parameters=[{ 116 | # 'use_sim_time': True, # Use /clock if available 117 | # 'map_frame': 'map', 118 | # 'base_frame': 'base_link' + suffix 119 | # }]), 120 | 121 | # Drone controller 122 | Node(package='flock2', node_executable='drone_base', output='screen', 123 | node_name='drone_base', node_namespace=namespace, parameters=[{ 124 | 'use_sim_time': True, # Use /clock if available 125 | }]), 126 | ]) 127 | 128 | return LaunchDescription(entities) 129 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/pair_box_pattern.sh: -------------------------------------------------------------------------------- 1 | 2 | 3 | echo Starting flight pattern 4 | 5 | function topic() { 6 | ros2 topic pub -1 /drone1/cmd_vel geometry_msgs/Twist "{linear: {$1}, angular: {x: 0.0, y: 0.0, z: 0.0}}" 7 | #ros2 topic pub -1 /drone2/cmd_vel geometry_msgs/Twist "{linear: {$1}, angular: {x: 0.0, y: 0.0, z: 0.0}}" 8 | } 9 | 10 | function cmd() { 11 | ros2 service call /drone1/tello_action tello_msgs/TelloAction "{cmd: '$1'}" 12 | #ros2 service call /drone2/tello_action tello_msgs/TelloAction "{cmd: '$1'}" 13 | sleep $2 14 | } 15 | 16 | function move_stop() { 17 | topic "$1" 18 | sleep $2 19 | topic "x: 0.0, y: 0.0, z: 0.0" 20 | sleep 0.1 21 | } 22 | 23 | function move_square() { 24 | move_stop "x: 0.0, y: 0.3, z: 0.0" 0.1 25 | move_stop "x: -0.3, y: 0.0, z: 0.0" 0.10 26 | move_stop "x: 0.0, y: -0.3, z: 0.0" 0.10 27 | move_stop "x: 0.3, y: 0.0, z: 0.0" 0.10 28 | } 29 | 30 | sleep 4 31 | echo start 32 | cmd "takeoff" 5 33 | 34 | move_stop "x: 0.0, y: 0.0, z: 0.15" 0.0 35 | move_stop "x: 0.15, y: 0.0, z: 0.0" 0.0 36 | 37 | echo continue 38 | 39 | move_square 40 | move_square 41 | 42 | cmd "land" 5 43 | 44 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/pair_box_pattern_b.sh: -------------------------------------------------------------------------------- 1 | 2 | 3 | echo Starting flight pattern 4 | 5 | function topic() { 6 | #ros2 topic pub -1 /drone1/cmd_vel geometry_msgs/Twist "{linear: {$1}, angular: {x: 0.0, y: 0.0, z: 0.0}}" 7 | ros2 topic pub -1 /drone2/cmd_vel geometry_msgs/Twist "{linear: {$1}, angular: {x: 0.0, y: 0.0, z: 0.0}}" 8 | } 9 | 10 | function cmd() { 11 | #ros2 service call /drone1/tello_action tello_msgs/TelloAction "{cmd: '$1'}" 12 | ros2 service call /drone2/tello_action tello_msgs/TelloAction "{cmd: '$1'}" 13 | sleep $2 14 | } 15 | 16 | function move_stop() { 17 | topic "$1" 18 | sleep $2 19 | topic "x: 0.0, y: 0.0, z: 0.0" 20 | sleep 0.1 21 | } 22 | 23 | function move_square() { 24 | move_stop "x: 0.0, y: 0.3, z: 0.0" 0.1 25 | move_stop "x: -0.3, y: 0.0, z: 0.0" 0.10 26 | move_stop "x: 0.0, y: -0.3, z: 0.0" 0.10 27 | move_stop "x: 0.3, y: 0.0, z: 0.0" 0.10 28 | } 29 | 30 | sleep 4 31 | echo start 32 | cmd "takeoff" 5 33 | 34 | move_stop "x: 0.0, y: 0.0, z: 0.15" 0.0 35 | move_stop "x: 0.15, y: 0.0, z: 0.0" 0.1 36 | 37 | echo continue 38 | 39 | move_square 40 | move_square 41 | 42 | cmd "land" 5 43 | 44 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/pair_brix_launch.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | 4 | from ament_index_python.packages import get_package_share_directory 5 | from launch import LaunchDescription 6 | 7 | sys.path.append(os.path.join(get_package_share_directory('fiducial_vlam'), 'launch')) 8 | import pair_shared_launch as psl 9 | 10 | def generate_launch_description(): 11 | action_list = psl.generate_secondary_action_list('drone2', 'brix') 12 | return LaunchDescription(action_list) 13 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/pair_shared_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | from ament_index_python.packages import get_package_share_directory 3 | from launch_ros.actions import Node 4 | from launch.actions import ExecuteProcess 5 | 6 | _launch_path = os.path.join(get_package_share_directory('fiducial_vlam'), 'launch') 7 | _map_filename = os.path.join(_launch_path, 'fiducial_marker_locations_basement.yaml') 8 | 9 | 10 | def generate_rviz_execute_process(computer_name): 11 | return ExecuteProcess(cmd=['rviz2', '-d', 12 | os.path.join(_launch_path, 'pair_' + computer_name + '.rviz')], 13 | output='screen') 14 | 15 | 16 | def generate_one_drone_action_list(drone_name): 17 | # supported names are tello, drone1, drone2 18 | # urdf = os.path.join(get_package_share_directory('flock2'), 'urdf', drone_name + '.urdf') 19 | ns = drone_name 20 | 21 | return [ 22 | Node(package='tello_driver', node_executable='tello_driver', output='screen', 23 | node_name='tello_driver', node_namespace=ns), 24 | Node(package='fiducial_vlam', node_executable='vloc_main', output='screen', 25 | node_name='vloc_main', node_namespace=ns, parameters=[{ 26 | 'camera_tf_pub_topic': '/tf', 27 | 'camera_frame_id': drone_name + '_camera' 28 | }]), 29 | ] 30 | 31 | 32 | def generate_primary_action_list(drone_name, computer_name): 33 | drone_actions = generate_one_drone_action_list(drone_name) 34 | 35 | main_actions = [ 36 | generate_rviz_execute_process(computer_name), 37 | Node(package='fiducial_vlam', node_executable='vmap_main', output='screen', 38 | node_name='vmap_main', parameters=[{ 39 | 'make_not_use_map': 0, 40 | 'marker_map_load_full_filename': _map_filename 41 | }]), 42 | ] 43 | 44 | return drone_actions + main_actions 45 | 46 | 47 | def generate_secondary_action_list(drone_name, computer_name): 48 | drone_actions = generate_one_drone_action_list(drone_name) 49 | 50 | main_actions = [ 51 | generate_rviz_execute_process(computer_name), 52 | ] 53 | 54 | return drone_actions + main_actions 55 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/pair_xps_launch.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | 6 | sys.path.append(get_package_share_directory('fiducial_vlam') + '/launch/') 7 | import pair_shared_launch as psl 8 | 9 | def generate_launch_description(): 10 | action_list = psl.generate_primary_action_list('drone1', 'xps') 11 | print(action_list) 12 | return LaunchDescription(action_list) 13 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/sim_one_marker_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import ExecuteProcess 6 | from launch_ros.actions import Node 7 | 8 | vlam_package_share_directory = get_package_share_directory('fiducial_vlam') 9 | sim_package_share_directory = get_package_share_directory('sim_fiducial') 10 | 11 | launch_directory = os.path.join(vlam_package_share_directory, 'launch') 12 | worlds_directory = os.path.join(sim_package_share_directory, 'worlds') 13 | sdf_directory = os.path.join(sim_package_share_directory, 'sdf') 14 | 15 | map_filename = os.path.join(worlds_directory, 'one_marker_map.yaml') 16 | world_filename = os.path.join(worlds_directory, 'one_marker.world') 17 | forward_camera_sdf = os.path.join(sdf_directory, 'forward_camera.sdf') 18 | 19 | make_not_use_map = 0 20 | 21 | vloc_args = [{ 22 | 'use_sim_time': False, # Don't use /clock 23 | 'publish_tfs': 1, # Publish drone and camera /tf 24 | 'stamp_msgs_with_current_time': 1, # Stamp with now() 25 | 'map_init_pose_z': 0, 26 | 'sub_camera_info_best_effort_not_reliable': 1, 27 | 'publish_tfs_per_marker': 0, 28 | 'publish_image_marked': 1, 29 | 'camera_frame_id': 'forward_camera', 30 | 'publish_base_pose': 1, 31 | 'publish_camera_odom': 1, 32 | 'publish_base_odom': 1, 33 | }] 34 | 35 | vmap_args = [{ 36 | 'use_sim_time': False, # Don't use /clock 37 | 'publish_tfs': 1, # Publish marker /tf 38 | 'marker_length': 0.1778, # Marker length 39 | 'marker_map_save_full_filename': map_filename, 40 | 'marker_map_load_full_filename': map_filename, 41 | 'make_not_use_map': make_not_use_map, 42 | }] 43 | 44 | 45 | # Before running Gazebo, prepare the environment: 46 | # export GAZEBO_MODEL_PATH=${PWD}/install/sim_fiducial/share/sim_fiducial/models 47 | # . /usr/share/gazebo/setup.sh 48 | 49 | 50 | def generate_launch_description(): 51 | entities = [ 52 | ExecuteProcess(cmd=[ 53 | 'gazebo', 54 | '--verbose', 55 | '-s', 'libgazebo_ros_init.so', # Publish /clock 56 | '-s', 'libgazebo_ros_factory.so', # Provide injection endpoints 57 | world_filename 58 | ], output='screen'), 59 | 60 | # Add forward-facing camera to the simulation 61 | Node(package='sim_fiducial', node_executable='inject_entity.py', output='screen', 62 | arguments=[forward_camera_sdf, '0', '0', '0', '0', '0', '0']), 63 | 64 | Node(package='fiducial_vlam', node_executable='vloc_main', output='screen', 65 | parameters=vloc_args, node_namespace='forward_camera'), 66 | Node(package='fiducial_vlam', node_executable='vmap_main', output='screen', 67 | parameters=vmap_args), 68 | ] 69 | 70 | return LaunchDescription(entities) 71 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/sim_yz_plane_of_markers_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import ExecuteProcess 6 | from launch_ros.actions import Node 7 | 8 | vlam_package_share_directory = get_package_share_directory('fiducial_vlam') 9 | sim_package_share_directory = get_package_share_directory('sim_fiducial') 10 | 11 | launch_directory = os.path.join(vlam_package_share_directory, 'launch') 12 | worlds_directory = os.path.join(sim_package_share_directory, 'worlds') 13 | sdf_directory = os.path.join(sim_package_share_directory, 'sdf') 14 | 15 | map_filename = os.path.join(worlds_directory, 'yz_plane_of_markers_map.yaml') 16 | world_filename = os.path.join(worlds_directory, 'yz_plane_of_markers.world') 17 | forward_camera_sdf = os.path.join(sdf_directory, 'forward_camera.sdf') 18 | 19 | sam_not_cv = 1 20 | sfm_not_slam = 1 21 | make_not_use_map = 0 22 | corner_measurement_sigma = 0.7 23 | 24 | vloc_args = [{ 25 | 'use_sim_time': False, # Don't use /clock 26 | 'publish_tfs': 1, # Publish drone and camera /tf 27 | 'stamp_msgs_with_current_time': 1, # Stamp with now() 28 | 'map_init_pose_z': 0, 29 | 'sub_camera_info_best_effort_not_reliable': 1, 30 | 'publish_tfs_per_marker': 0, 31 | 'publish_image_marked': 1, 32 | 'sam_not_cv': sam_not_cv, 33 | 'sfm_not_slam': sfm_not_slam, 34 | 'camera_frame_id': 'forward_camera', 35 | 'publish_base_pose': 1, 36 | 'publish_camera_odom': 1, 37 | 'publish_base_odom': 1, 38 | }] 39 | 40 | vmap_args = [{ 41 | 'use_sim_time': False, # Don't use /clock 42 | 'publish_tfs': 1, # Publish marker /tf 43 | 'marker_length': 0.1778, # Marker length 44 | 'marker_map_save_full_filename': map_filename, 45 | 'marker_map_load_full_filename': map_filename, 46 | 'make_not_use_map': make_not_use_map, 47 | 'sam_not_cv': sam_not_cv, 48 | 'sfm_not_slam': sfm_not_slam, 49 | 'corner_measurement_sigma': corner_measurement_sigma, 50 | }] 51 | 52 | 53 | # Before running Gazebo, prepare the environment: 54 | # export GAZEBO_MODEL_PATH=${PWD}/install/sim_fiducial/share/sim_fiducial/models 55 | # . /usr/share/gazebo/setup.sh 56 | 57 | 58 | def generate_launch_description(): 59 | entities = [ 60 | ExecuteProcess(cmd=[ 61 | 'gazebo', 62 | '--verbose', 63 | '-s', 'libgazebo_ros_init.so', # Publish /clock 64 | '-s', 'libgazebo_ros_factory.so', # Provide injection endpoints 65 | world_filename 66 | ], output='screen'), 67 | 68 | # Add forward-facing camera to the simulation 69 | Node(package='sim_fiducial', node_executable='inject_entity.py', output='screen', 70 | arguments=[forward_camera_sdf, '0', '0', '0', '0', '0', '0']), 71 | 72 | Node(package='fiducial_vlam', node_executable='vloc_main', output='screen', 73 | parameters=vloc_args, node_namespace='forward_camera'), 74 | Node(package='fiducial_vlam', node_executable='vmap_main', output='screen', 75 | parameters=vmap_args), 76 | ] 77 | 78 | return LaunchDescription(entities) 79 | -------------------------------------------------------------------------------- /fiducial_vlam/launch/teleop_launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | 4 | from ament_index_python.packages import get_package_share_directory 5 | from launch import LaunchDescription 6 | from launch.actions import ExecuteProcess 7 | from launch_ros.actions import Node 8 | 9 | package_name = 'fiducial_vlam' 10 | package_share_directory = get_package_share_directory(package_name); 11 | package_launch_directory = os.path.join(package_share_directory, 'launch') 12 | package_cfg_directory = os.path.join(package_share_directory, 'cfg') 13 | 14 | map_filename = os.path.join(package_cfg_directory, 'fiducial_marker_locations_office.yaml') 15 | rviz_config_filename = os.path.join(package_cfg_directory, package_name + '.rviz') 16 | 17 | print ("map_filename: ", map_filename) 18 | print ("rviz_config_filename: ", rviz_config_filename) 19 | 20 | 21 | tello_ros_args = [{ 22 | 'drone_ip': '192.168.0.30', 23 | 'command_port': '11002', 24 | 'drone_port': '8889', 25 | 'data_port': '13002', 26 | 'video_port': '14002' 27 | }] 28 | 29 | vloc_args = [{ 30 | 'use_sim_time': False, # Use /clock if available 31 | 'publish_tfs': 1, # Publish drone and camera /tf 32 | 'stamp_msgs_with_current_time': 0, # Use incoming message time, not now() 33 | # 'base_frame_id': 'base_link' + suffix, 34 | 'map_init_pose_z': -0.035, 35 | # 'camera_frame_id': 'camera_link' + suffix, 36 | 'base_odometry_pub_topic': 'filtered_odom', 37 | 'sub_camera_info_best_effort_not_reliable': 1, 38 | 'publish_image_marked': 1, 39 | }] 40 | 41 | vmap_args = [{ 42 | 'use_sim_time': False, # Use /clock if available 43 | 'publish_tfs': 1, # Publish marker /tf 44 | 'marker_length': 0.1778, # Marker length 45 | 'marker_map_load_full_filename': map_filename, # Load a pre-built map from disk 46 | 'make_not_use_map': 0 # Don't save a map to disk 47 | }] 48 | 49 | 50 | 51 | def generate_launch_description(): 52 | 53 | entities = [ 54 | ExecuteProcess(cmd=['rviz2', '-d', rviz_config_filename], output='screen'), 55 | 56 | Node(package='tello_driver', node_executable='tello_driver', output='screen', 57 | parameters=tello_ros_args), 58 | 59 | Node(package='fiducial_vlam', node_executable='vloc_main', output='screen', 60 | parameters=vloc_args), 61 | Node(package='fiducial_vlam', node_executable='vmap_main', output='screen', 62 | parameters=vmap_args), 63 | 64 | ] 65 | 66 | return LaunchDescription(entities) 67 | 68 | -------------------------------------------------------------------------------- /fiducial_vlam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | fiducial_vlam 6 | 0.0.0 7 | Fiducial Marker Visual Localization and Mapping 8 | 9 | Peter Mullen 10 | BSD 3 clause 11 | 12 | https://github.com/ptrmu/fiducial_vlam.git 13 | https://github.com/ptrmu/fiducial_vlam/issues 14 | 15 | Clyde McQueen 16 | Peter Mullen 17 | 18 | ament_cmake 19 | 20 | cv_bridge 21 | fiducial_vlam_msgs 22 | geometry_msgs 23 | rclcpp 24 | ros2_shared 25 | sensor_msgs 26 | std_msgs 27 | tf2_msgs 28 | visualization_msgs 29 | yaml_cpp_vendor 30 | 31 | 32 | ament_cmake 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /fiducial_vlam/src/convert_util.cpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "convert_util.hpp" 4 | 5 | #include "transform_with_covariance.hpp" 6 | 7 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 8 | #include "tf2/convert.h" 9 | 10 | namespace fiducial_vlam 11 | { 12 | geometry_msgs::msg::Pose to_Pose_msg(const TransformWithCovariance &twc) 13 | { 14 | geometry_msgs::msg::Pose pose; 15 | toMsg(twc.transform(), pose); 16 | return pose; 17 | } 18 | 19 | geometry_msgs::msg::PoseWithCovariance to_PoseWithCovariance_msg(const TransformWithCovariance &twc) 20 | { 21 | geometry_msgs::msg::PoseWithCovariance msg; 22 | msg.pose = to_Pose_msg(twc); 23 | msg.covariance = twc.cov(); 24 | return msg; 25 | } 26 | 27 | geometry_msgs::msg::PoseWithCovarianceStamped to_PoseWithCovarianceStamped_msg( 28 | const TransformWithCovariance &twc, 29 | std_msgs::msg::Header::_stamp_type stamp, 30 | std_msgs::msg::Header::_frame_id_type frame_id) 31 | { 32 | geometry_msgs::msg::PoseWithCovarianceStamped msg; 33 | msg.header.stamp = stamp; 34 | msg.header.frame_id = frame_id; 35 | msg.pose = to_PoseWithCovariance_msg(twc); 36 | return msg; 37 | } 38 | 39 | TransformWithCovariance to_TransformWithCovariance(const geometry_msgs::msg::PoseWithCovariance &pwc) 40 | { 41 | tf2::Transform tf; 42 | fromMsg(pwc.pose, tf); 43 | return TransformWithCovariance(tf, pwc.covariance); 44 | } 45 | } 46 | 47 | -------------------------------------------------------------------------------- /fiducial_vlam/src/fiducial_math.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "fiducial_math.hpp" 3 | 4 | #include "observation.hpp" 5 | #include "transform_with_covariance.hpp" 6 | 7 | #include "cv_bridge/cv_bridge.h" 8 | #include "opencv2/aruco.hpp" 9 | #include "opencv2/calib3d/calib3d.hpp" 10 | 11 | namespace fiducial_vlam 12 | { 13 | // ============================================================================== 14 | // CameraInfo::CvCameraInfo class 15 | // ============================================================================== 16 | 17 | class CameraInfo::CvCameraInfo 18 | { 19 | cv::Mat camera_matrix_; 20 | cv::Mat dist_coeffs_; 21 | 22 | public: 23 | CvCameraInfo() = delete; 24 | 25 | explicit CvCameraInfo(const sensor_msgs::msg::CameraInfo &msg) 26 | : camera_matrix_(3, 3, CV_64F, 0.), dist_coeffs_(1, 5, CV_64F) 27 | { 28 | camera_matrix_.at(0, 0) = msg.k[0]; 29 | camera_matrix_.at(0, 2) = msg.k[2]; 30 | camera_matrix_.at(1, 1) = msg.k[4]; 31 | camera_matrix_.at(1, 2) = msg.k[5]; 32 | camera_matrix_.at(2, 2) = 1.; 33 | 34 | // ROS and OpenCV (and everybody?) agree on this ordering: k1, k2, t1 (p1), t2 (p2), k3 35 | dist_coeffs_.at(0) = msg.d[0]; 36 | dist_coeffs_.at(1) = msg.d[1]; 37 | dist_coeffs_.at(2) = msg.d[2]; 38 | dist_coeffs_.at(3) = msg.d[3]; 39 | dist_coeffs_.at(4) = msg.d[4]; 40 | } 41 | 42 | auto &camera_matrix() 43 | { return camera_matrix_; } 44 | 45 | auto &dist_coeffs() 46 | { return dist_coeffs_; } 47 | }; 48 | 49 | // ============================================================================== 50 | // CameraInfo class 51 | // ============================================================================== 52 | 53 | CameraInfo::CameraInfo() = default; 54 | 55 | CameraInfo::CameraInfo(const sensor_msgs::msg::CameraInfo &camera_info_msg) 56 | : cv_(std::make_shared(camera_info_msg)) 57 | {} 58 | 59 | // ============================================================================== 60 | // drawDetectedMarkers function 61 | // ============================================================================== 62 | 63 | static void drawDetectedMarkers(cv::InputOutputArray image, 64 | cv::InputArrayOfArrays corners, 65 | cv::InputArray ids) 66 | { 67 | // calculate colors 68 | auto borderColor = cv::Scalar(0, 255, 0); 69 | cv::Scalar textColor = borderColor; 70 | cv::Scalar cornerColor = borderColor; 71 | 72 | std::swap(textColor.val[0], textColor.val[1]); // text color just sawp G and R 73 | std::swap(cornerColor.val[1], cornerColor.val[2]); // corner color just sawp G and B 74 | 75 | int nMarkers = static_cast(corners.total()); 76 | for (int i = 0; i < nMarkers; i++) { 77 | 78 | cv::Mat currentMarker = corners.getMat(i); 79 | CV_Assert((currentMarker.total() == 4) && (currentMarker.type() == CV_32FC2)); 80 | 81 | // draw marker sides 82 | for (int j = 0; j < 4; j++) { 83 | cv::Point2f p0, p1; 84 | p0 = currentMarker.ptr(0)[j]; 85 | p1 = currentMarker.ptr(0)[(j + 1) % 4]; 86 | line(image, p0, p1, borderColor, 1); 87 | } 88 | 89 | // draw first corner mark 90 | rectangle(image, 91 | currentMarker.ptr(0)[0] - cv::Point2f(3, 3), 92 | currentMarker.ptr(0)[0] + cv::Point2f(3, 3), 93 | cornerColor, 1, cv::LINE_AA); 94 | 95 | // draw ID 96 | // if (ids.total() != 0) { 97 | // cv::Point2f cent(0, 0); 98 | // for (int p = 0; p < 4; p++) 99 | // cent += currentMarker.ptr(0)[p]; 100 | // 101 | // cent = cent / 4.; 102 | // std::stringstream s; 103 | // s << "id=" << ids.getMat().ptr(0)[i]; 104 | // putText(image, s.str(), cent, cv::FONT_HERSHEY_SIMPLEX, 0.5, textColor, 2); 105 | // } 106 | 107 | } 108 | } 109 | // ============================================================================== 110 | // FiducialMath::CvFiducialMath class 111 | // ============================================================================== 112 | 113 | class FiducialMath::CvFiducialMath 114 | { 115 | const CameraInfo ci_; 116 | 117 | public: 118 | explicit CvFiducialMath(const CameraInfo &camera_info) 119 | : ci_(camera_info) 120 | {} 121 | 122 | explicit CvFiducialMath(const sensor_msgs::msg::CameraInfo &camera_info_msg) 123 | : ci_(camera_info_msg) 124 | {} 125 | 126 | TransformWithCovariance solve_t_camera_marker( 127 | const Observation &observation, 128 | double marker_length) 129 | { 130 | // Build up two lists of corner points: 2D in the image frame, 3D in the marker frame 131 | std::vector corners_f_marker; 132 | std::vector corners_f_image; 133 | 134 | append_corners_f_marker(corners_f_marker, marker_length); 135 | append_corners_f_image(corners_f_image, observation); 136 | 137 | // Figure out image location. 138 | cv::Vec3d rvec, tvec; 139 | cv::solvePnP(corners_f_marker, corners_f_image, 140 | ci_.cv()->camera_matrix(), ci_.cv()->dist_coeffs(), 141 | rvec, tvec); 142 | 143 | // rvec, tvec output from solvePnp "brings points from the model coordinate system to the 144 | // camera coordinate system". In this case the marker frame is the model coordinate system. 145 | // So rvec, tvec are the transformation t_camera_marker. 146 | return TransformWithCovariance(to_tf2_transform(rvec, tvec)); 147 | } 148 | 149 | TransformWithCovariance solve_t_map_camera(const Observations &observations, 150 | const std::vector &t_map_markers, 151 | double marker_length) 152 | { 153 | // Build up two lists of corner points: 2D in the image frame, 3D in the marker frame 154 | std::vector all_corners_f_map; 155 | std::vector all_corners_f_image; 156 | 157 | for (int i = 0; i < observations.size(); i += 1) { 158 | auto &observation = observations.observations()[i]; 159 | auto &t_map_marker = t_map_markers[i]; 160 | if (t_map_marker.is_valid()) { 161 | append_corners_f_map(all_corners_f_map, t_map_marker, marker_length); 162 | append_corners_f_image(all_corners_f_image, observation); 163 | } 164 | } 165 | 166 | // If there are no known markers in the observation set, then don't 167 | // try to find the camera position 168 | if (all_corners_f_map.empty()) { 169 | return TransformWithCovariance{}; 170 | } 171 | 172 | // Figure out camera location. 173 | cv::Vec3d rvec, tvec; 174 | cv::solvePnP(all_corners_f_map, all_corners_f_image, 175 | ci_.cv()->camera_matrix(), ci_.cv()->dist_coeffs(), 176 | rvec, tvec); 177 | 178 | // For certain cases, there is a chance that the multi marker solvePnP will 179 | // return the mirror of the correct solution. So try solvePn[Ransac as well. 180 | if (all_corners_f_image.size() > 1 * 4 && all_corners_f_image.size() < 4 * 4) { 181 | cv::Vec3d rvecRansac, tvecRansac; 182 | cv::solvePnPRansac(all_corners_f_map, all_corners_f_image, 183 | ci_.cv()->camera_matrix(), ci_.cv()->dist_coeffs(), 184 | rvecRansac, tvecRansac); 185 | 186 | // If the pose returned from the ransac version is very different from 187 | // that returned from the normal version, then use the ransac results. 188 | // solvePnp can sometimes pick up the wrong solution (a mirror solution). 189 | // solvePnpRansac does a better job in that case. But solvePnp does a 190 | // better job smoothing out image noise so it is prefered when it works. 191 | if (std::abs(rvec[0] - rvecRansac[0]) > 0.5 || 192 | std::abs(rvec[1] - rvecRansac[1]) > 0.5 || 193 | std::abs(rvec[2] - rvecRansac[2]) > 0.5) { 194 | rvec = rvecRansac; 195 | tvec = tvecRansac; 196 | } 197 | } 198 | 199 | if (tvec[0] < 0) { // specific tests for bad pose determination 200 | int xxx = 9; 201 | } 202 | 203 | // rvec, tvec output from solvePnp "brings points from the model coordinate system to the 204 | // camera coordinate system". In this case the map frame is the model coordinate system. 205 | // So rvec, tvec are the transformation t_camera_map. 206 | auto tf2_t_map_camera = to_tf2_transform(rvec, tvec).inverse(); 207 | return TransformWithCovariance(tf2_t_map_camera); 208 | } 209 | 210 | Observations detect_markers(cv_bridge::CvImagePtr &gray, 211 | std::shared_ptr &color_marked, 212 | int corner_refinement_method) 213 | { 214 | // Todo: make the dictionary a parameter 215 | auto dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_250); 216 | auto detectorParameters = cv::aruco::DetectorParameters::create(); 217 | #if (CV_VERSION_MAJOR == 4) 218 | // 0 = CORNER_REFINE_NONE, ///< Tag and corners detection based on the ArUco approach 219 | // 1 = CORNER_REFINE_SUBPIX, ///< ArUco approach and refine the corners locations using corner subpixel accuracy 220 | // 2 = CORNER_REFINE_CONTOUR, ///< ArUco approach and refine the corners locations using the contour-points line fitting 221 | // 3 = CORNER_REFINE_APRILTAG, ///< Tag and corners detection based on the AprilTag 2 approach @cite wang2016iros 222 | 223 | // Potentially use the new AprilTag 2 corner algorithm, much better but much slower 224 | // detectorParameters->cornerRefinementMethod = cv::aruco::CornerRefineMethod::CORNER_REFINE_CONTOUR; 225 | detectorParameters->cornerRefinementMethod = corner_refinement_method; 226 | #else 227 | detectorParameters->doCornerRefinement = true; 228 | #endif 229 | 230 | // Detect markers 231 | std::vector ids; 232 | std::vector> corners; 233 | cv::aruco::detectMarkers(gray->image, dictionary, corners, ids, detectorParameters); 234 | 235 | // Annotate the markers 236 | if (color_marked) { 237 | drawDetectedMarkers(color_marked->image, corners, ids); 238 | } 239 | 240 | // return the corners as a list of observations 241 | return to_observations(ids, corners); 242 | } 243 | 244 | void annotate_image_with_marker_axis(std::shared_ptr &color_marked, 245 | const TransformWithCovariance &t_camera_marker) 246 | { 247 | cv::Vec3d rvec; 248 | cv::Vec3d tvec; 249 | to_cv_rvec_tvec(t_camera_marker, rvec, tvec); 250 | 251 | cv::aruco::drawAxis(color_marked->image, 252 | ci_.cv()->camera_matrix(), ci_.cv()->dist_coeffs(), 253 | rvec, tvec, 0.1); 254 | } 255 | 256 | private: 257 | void append_corners_f_map(std::vector &corners_f_map, 258 | const TransformWithCovariance &t_map_marker, 259 | double marker_length) 260 | { 261 | // Build up a list of the corner locations in the map frame. 262 | tf2::Vector3 corner0_f_marker(-marker_length / 2.f, marker_length / 2.f, 0.f); 263 | tf2::Vector3 corner1_f_marker(marker_length / 2.f, marker_length / 2.f, 0.f); 264 | tf2::Vector3 corner2_f_marker(marker_length / 2.f, -marker_length / 2.f, 0.f); 265 | tf2::Vector3 corner3_f_marker(-marker_length / 2.f, -marker_length / 2.f, 0.f); 266 | 267 | const auto &t_map_marker_tf = t_map_marker.transform(); 268 | auto corner0_f_map = t_map_marker_tf * corner0_f_marker; 269 | auto corner1_f_map = t_map_marker_tf * corner1_f_marker; 270 | auto corner2_f_map = t_map_marker_tf * corner2_f_marker; 271 | auto corner3_f_map = t_map_marker_tf * corner3_f_marker; 272 | 273 | corners_f_map.emplace_back(cv::Point3d(corner0_f_map.x(), corner0_f_map.y(), corner0_f_map.z())); 274 | corners_f_map.emplace_back(cv::Point3d(corner1_f_map.x(), corner1_f_map.y(), corner1_f_map.z())); 275 | corners_f_map.emplace_back(cv::Point3d(corner2_f_map.x(), corner2_f_map.y(), corner2_f_map.z())); 276 | corners_f_map.emplace_back(cv::Point3d(corner3_f_map.x(), corner3_f_map.y(), corner3_f_map.z())); 277 | } 278 | 279 | void append_corners_f_marker(std::vector &corners_f_marker, double marker_length) 280 | { 281 | // Add to the list of the corner locations in the map frame. 282 | corners_f_marker.emplace_back(cv::Point3d(-marker_length / 2.f, marker_length / 2.f, 0.f)); 283 | corners_f_marker.emplace_back(cv::Point3d(marker_length / 2.f, marker_length / 2.f, 0.f)); 284 | corners_f_marker.emplace_back(cv::Point3d(marker_length / 2.f, -marker_length / 2.f, 0.f)); 285 | corners_f_marker.emplace_back(cv::Point3d(-marker_length / 2.f, -marker_length / 2.f, 0.f)); 286 | } 287 | 288 | void append_corners_f_image(std::vector &corners_f_image, const Observation &observation) 289 | { 290 | corners_f_image.emplace_back( 291 | cv::Point2f(static_cast(observation.x0()), static_cast(observation.y0()))); 292 | corners_f_image.emplace_back( 293 | cv::Point2f(static_cast(observation.x1()), static_cast(observation.y1()))); 294 | corners_f_image.emplace_back( 295 | cv::Point2f(static_cast(observation.x2()), static_cast(observation.y2()))); 296 | corners_f_image.emplace_back( 297 | cv::Point2f(static_cast(observation.x3()), static_cast(observation.y3()))); 298 | }; 299 | 300 | Observations to_observations(const std::vector &ids, const std::vector> &corners) 301 | { 302 | Observations observations; 303 | for (int i = 0; i < ids.size(); i += 1) { 304 | observations.add(Observation(ids[i], 305 | corners[i][0].x, corners[i][0].y, 306 | corners[i][1].x, corners[i][1].y, 307 | corners[i][2].x, corners[i][2].y, 308 | corners[i][3].x, corners[i][3].y)); 309 | } 310 | return observations; 311 | } 312 | 313 | tf2::Transform to_tf2_transform(const cv::Vec3d &rvec, const cv::Vec3d &tvec) 314 | { 315 | tf2::Vector3 t(tvec[0], tvec[1], tvec[2]); 316 | cv::Mat rmat; 317 | cv::Rodrigues(rvec, rmat); 318 | tf2::Matrix3x3 m; 319 | for (int row = 0; row < 3; row++) { 320 | for (int col = 0; col < 3; col++) { 321 | m[row][col] = rmat.at(row, col); // Row- vs. column-major order 322 | } 323 | } 324 | tf2::Transform result(m, t); 325 | return result; 326 | } 327 | 328 | void to_cv_rvec_tvec(const TransformWithCovariance &t, cv::Vec3d &rvec, cv::Vec3d &tvec) 329 | { 330 | auto c = t.transform().getOrigin(); 331 | tvec[0] = c.x(); 332 | tvec[1] = c.y(); 333 | tvec[2] = c.z(); 334 | auto R = t.transform().getBasis(); 335 | cv::Mat rmat(3, 3, CV_64FC1); 336 | for (int row = 0; row < 3; row++) { 337 | for (int col = 0; col < 3; col++) { 338 | rmat.at(row, col) = R[row][col]; 339 | } 340 | } 341 | cv::Rodrigues(rmat, rvec); 342 | } 343 | }; 344 | 345 | // ============================================================================== 346 | // FiducialMath class 347 | // ============================================================================== 348 | 349 | FiducialMath::FiducialMath(const CameraInfo &camera_info) 350 | : cv_(std::make_shared(camera_info)) 351 | {} 352 | 353 | FiducialMath::FiducialMath(const sensor_msgs::msg::CameraInfo &camera_info_msg) 354 | : cv_(std::make_shared(camera_info_msg)) 355 | {} 356 | 357 | TransformWithCovariance FiducialMath::solve_t_camera_marker( 358 | const Observation &observation, 359 | double marker_length) 360 | { 361 | return cv_->solve_t_camera_marker(observation, marker_length); 362 | } 363 | 364 | TransformWithCovariance FiducialMath::solve_t_map_camera(const Observations &observations, 365 | const std::vector &t_map_markers, 366 | double marker_length) 367 | { 368 | return cv_->solve_t_map_camera(observations, t_map_markers, marker_length); 369 | } 370 | 371 | Observations FiducialMath::detect_markers(std::shared_ptr &gray, 372 | std::shared_ptr &color_marked, 373 | int corner_refinement_method) 374 | { 375 | return cv_->detect_markers(gray, color_marked, corner_refinement_method); 376 | } 377 | 378 | void FiducialMath::annotate_image_with_marker_axis(std::shared_ptr &color_marked, 379 | const TransformWithCovariance &t_camera_marker) 380 | { 381 | cv_->annotate_image_with_marker_axis(color_marked, t_camera_marker); 382 | } 383 | } 384 | -------------------------------------------------------------------------------- /fiducial_vlam/src/map.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "map.hpp" 3 | 4 | #include "fiducial_math.hpp" 5 | #include "observation.hpp" 6 | 7 | namespace fiducial_vlam 8 | { 9 | 10 | // ============================================================================== 11 | // Observations class 12 | // ============================================================================== 13 | 14 | Observations::Observations(const fiducial_vlam_msgs::msg::Observations &msg) 15 | { 16 | for (auto &obs : msg.observations) { 17 | observations_.emplace_back(Observation(obs.id, 18 | obs.x0, obs.y0, 19 | obs.x1, obs.y1, 20 | obs.x2, obs.y2, 21 | obs.x3, obs.y3)); 22 | } 23 | } 24 | 25 | fiducial_vlam_msgs::msg::Observations Observations::to_msg(std_msgs::msg::Header::_stamp_type stamp, 26 | const std_msgs::msg::Header::_frame_id_type &frame_id, 27 | const sensor_msgs::msg::CameraInfo &camera_info_msg) 28 | { 29 | fiducial_vlam_msgs::msg::Observations msg; 30 | msg.header.frame_id = frame_id; 31 | msg.header.stamp = stamp; 32 | msg.camera_info = camera_info_msg; 33 | for ( 34 | auto observation: observations_) { 35 | fiducial_vlam_msgs::msg::Observation obs_msg; 36 | obs_msg.id = observation.id(); 37 | obs_msg.x0 = observation.x0(); 38 | obs_msg.x1 = observation.x1(); 39 | obs_msg.x2 = observation.x2(); 40 | obs_msg.x3 = observation.x3(); 41 | obs_msg.y0 = observation.y0(); 42 | obs_msg.y1 = observation.y1(); 43 | obs_msg.y2 = observation.y2(); 44 | obs_msg.y3 = observation.y3(); 45 | msg.observations. 46 | emplace_back(obs_msg); 47 | } 48 | return 49 | msg; 50 | } 51 | 52 | // ============================================================================== 53 | // Map class 54 | // ============================================================================== 55 | 56 | Map::Map(double marker_length) 57 | { 58 | marker_length_ = marker_length; 59 | } 60 | 61 | Map::Map(const fiducial_vlam_msgs::msg::Map &msg) 62 | { 63 | marker_length_ = msg.marker_length; 64 | for (int i = 0; i < msg.ids.size(); i += 1) { 65 | Marker marker(msg.ids[i], to_TransformWithCovariance(msg.poses[i])); 66 | marker.set_is_fixed(msg.fixed_flags[i] != 0); 67 | add_marker(std::move(marker)); 68 | } 69 | } 70 | 71 | std::unique_ptr 72 | Map::to_map_msg(const std_msgs::msg::Header &header_msg, double marker_length) 73 | { 74 | auto map_msg_unique = std::make_unique(); 75 | auto &map_msg = *map_msg_unique; 76 | for (auto &marker_pair : markers_) { 77 | auto &marker = marker_pair.second; 78 | map_msg.ids.emplace_back(marker.id()); 79 | map_msg.poses.emplace_back(to_PoseWithCovariance_msg(marker.t_map_marker())); 80 | map_msg.fixed_flags.emplace_back(marker.is_fixed() ? 1 : 0); 81 | } 82 | map_msg.header = header_msg; 83 | map_msg.marker_length = marker_length; 84 | return map_msg_unique; 85 | } 86 | 87 | Marker *Map::find_marker(int id) 88 | { 89 | auto marker_pair = markers_.find(id); 90 | return marker_pair == markers_.end() ? nullptr : &marker_pair->second; 91 | } 92 | 93 | void Map::add_marker(Marker marker) 94 | { 95 | assert(markers_.count(marker.id()) == 0); 96 | markers_.emplace(marker.id(), std::move(marker)); 97 | } 98 | 99 | std::vector Map::find_t_map_markers(const Observations &observations) 100 | { 101 | std::vector t_map_markers{}; 102 | t_map_markers.reserve(observations.size()); 103 | for (auto &obs: observations.observations()) { 104 | auto marker_ptr = find_marker(obs.id()); 105 | t_map_markers.emplace_back(marker_ptr ? marker_ptr->t_map_marker() : TransformWithCovariance()); 106 | } 107 | return t_map_markers; 108 | } 109 | 110 | 111 | // ============================================================================== 112 | // Localizer class 113 | // ============================================================================== 114 | 115 | static void annotate_image_with_marker_axes(std::shared_ptr &color_marked, 116 | const TransformWithCovariance &t_map_camera, 117 | const std::vector &t_map_markers, 118 | FiducialMath &fm) 119 | { 120 | // Annotate the image by drawing axes on each marker that was used for the location 121 | // calculation. This calculation uses the average t_map_camera and the t_map_markers 122 | // to figure out where the axes should be. This is different from the t_camera_marker 123 | // that was solved for above. 124 | if (color_marked && t_map_camera.is_valid()) { 125 | 126 | // Cache a transform. 127 | auto tf_t_camera_map = t_map_camera.transform().inverse(); 128 | 129 | // Loop through the ids of the markers visible in this image 130 | for (int i = 0; i < t_map_markers.size(); i += 1) { 131 | auto &t_map_marker = t_map_markers[i]; 132 | 133 | if (t_map_marker.is_valid()) { 134 | // Calculalte t_camera_marker and draw the axis. 135 | auto t_camera_marker = TransformWithCovariance(tf_t_camera_map * t_map_marker.transform()); 136 | fm.annotate_image_with_marker_axis(color_marked, t_camera_marker); 137 | } 138 | } 139 | } 140 | } 141 | 142 | Localizer::Localizer(const std::unique_ptr &map) 143 | : map_(map) 144 | { 145 | } 146 | 147 | TransformWithCovariance Localizer::average_t_map_camera(const Observations &observations, 148 | const std::vector &t_map_markers, 149 | std::shared_ptr &color_marked, 150 | FiducialMath &fm) 151 | { 152 | TransformWithCovariance average_t_map_camera{}; 153 | int observations_count{0}; 154 | 155 | // For each observation. 156 | for (int i = 0; i < observations.size(); i += 1) { 157 | auto &observation = observations.observations()[i]; 158 | auto &t_map_marker = t_map_markers[i]; 159 | 160 | if (t_map_marker.is_valid()) { 161 | 162 | // Find the camera marker transform 163 | auto t_camera_marker = fm.solve_t_camera_marker(observation, map_->marker_length()); 164 | 165 | // The solvePnP function returns the marker in the camera frame: t_camera_marker 166 | auto tf2_t_marker_camera = t_camera_marker.transform().inverse(); 167 | auto tf2_t_map_camera = t_map_marker.transform() * tf2_t_marker_camera; 168 | TransformWithCovariance t_map_camera(tf2_t_map_camera); 169 | 170 | // Average this new measurement with the previous 171 | if (observations_count == 0) { 172 | average_t_map_camera = t_map_camera; 173 | observations_count += 1; 174 | } else { 175 | average_t_map_camera.update_simple_average(t_map_camera, observations_count); 176 | observations_count += 1; 177 | } 178 | } 179 | } 180 | 181 | annotate_image_with_marker_axes(color_marked, average_t_map_camera, t_map_markers, fm); 182 | 183 | return average_t_map_camera; 184 | } 185 | 186 | TransformWithCovariance Localizer::simultaneous_t_map_camera(const Observations &observations, 187 | const std::vector &t_map_markers, 188 | std::shared_ptr &color_marked, 189 | FiducialMath &fm) 190 | { 191 | auto t_map_camera = fm.solve_t_map_camera(observations, t_map_markers, map_->marker_length()); 192 | 193 | annotate_image_with_marker_axes(color_marked, t_map_camera, t_map_markers, fm); 194 | 195 | return t_map_camera; 196 | } 197 | 198 | std::vector Localizer::markers_t_map_cameras( 199 | const Observations &observations, 200 | const std::vector &t_map_markers, 201 | FiducialMath &fm) 202 | { 203 | std::vector t_map_cameras; 204 | 205 | for (int i = 0; i < observations.size(); i += 1) { 206 | TransformWithCovariance t_map_camera{}; 207 | auto &t_map_marker = t_map_markers[i]; 208 | 209 | if (t_map_marker.is_valid()) { 210 | Observations single_observation{}; 211 | single_observation.add(observations.observations()[i]); 212 | std::vector single_t_map_markers{}; 213 | single_t_map_markers.emplace_back(t_map_marker); 214 | t_map_camera = fm.solve_t_map_camera(single_observation, single_t_map_markers, map_->marker_length()); 215 | } 216 | 217 | t_map_cameras.emplace_back(t_map_camera); 218 | } 219 | 220 | return t_map_cameras; 221 | } 222 | 223 | // ============================================================================== 224 | // Utility 225 | // ============================================================================== 226 | 227 | // void log_tf_transform(rclcpp::Node &node, std::string s, const tf2::Transform &transform) 228 | // { 229 | // auto t = transform.getOrigin(); 230 | // double r, p, y; 231 | // transform.getBasis().getRPY(r, p, y); 232 | // 233 | // RCLCPP_DEBUG(node.get_logger(), "%s xyz:%lf %lf %lf, rpy:%lf %lf %lf", 234 | // s.c_str(), t.x(), t.y(), t.z(), r, p, y); 235 | // } 236 | 237 | 238 | } 239 | 240 | -------------------------------------------------------------------------------- /fiducial_vlam/src/transform_with_covariance.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "transform_with_covariance.hpp" 3 | 4 | namespace fiducial_vlam 5 | { 6 | void TransformWithCovariance::update_simple_average(const TransformWithCovariance &newVal, int previous_update_count) 7 | { 8 | double previous_weight = double(previous_update_count) / (previous_update_count + 1); 9 | double current_weight = 1.0 / (previous_update_count + 1); 10 | 11 | transform_.setOrigin(transform_.getOrigin() * previous_weight + 12 | newVal.transform_.getOrigin() * current_weight); 13 | 14 | tf2::Quaternion q1 = transform_.getRotation(); 15 | tf2::Quaternion q2 = newVal.transform_.getRotation(); 16 | transform_.setRotation(q1.slerp(q2, current_weight).normalized()); 17 | } 18 | } 19 | 20 | -------------------------------------------------------------------------------- /fiducial_vlam/src/vloc_context.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "vloc_context.hpp" 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | 6 | namespace fiducial_vlam 7 | { 8 | void VlocContext::load_parameters() 9 | { 10 | #undef CXT_MACRO_MEMBER 11 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOAD_PARAMETER(node_, (*this), n, t, d) 12 | CXT_MACRO_INIT_PARAMETERS(VLOC_ALL_PARAMS, validate_parameters) 13 | 14 | #undef CXT_MACRO_MEMBER 15 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_PARAMETER_CHANGED(n, t) 16 | CXT_MACRO_REGISTER_PARAMETERS_CHANGED(node_, (*this), VLOC_ALL_PARAMS, validate_parameters) 17 | 18 | RCLCPP_INFO(node_.get_logger(), "VlocNode Parameters"); 19 | 20 | #undef CXT_MACRO_MEMBER 21 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_PARAMETER(RCLCPP_INFO, node_.get_logger(), (*this), n, t, d) 22 | VLOC_ALL_PARAMS 23 | } 24 | 25 | void VlocContext::validate_parameters() 26 | { 27 | t_camera_base_ = TransformWithCovariance(TransformWithCovariance::mu_type{ 28 | t_camera_base_x_, t_camera_base_y_, t_camera_base_z_, 29 | t_camera_base_roll_, t_camera_base_pitch_, t_camera_base_yaw_}); 30 | } 31 | } 32 | 33 | -------------------------------------------------------------------------------- /fiducial_vlam/src/vloc_main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "fiducial_vlam/fiducial_vlam.hpp" 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | 6 | int main(int argc, char **argv) 7 | { 8 | // Force flush of the stdout buffer 9 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 10 | 11 | // Init ROS 12 | rclcpp::init(argc, argv); 13 | 14 | // Create single-threaded executor 15 | rclcpp::executors::SingleThreadedExecutor executor; 16 | 17 | // Create and add node 18 | rclcpp::NodeOptions options{}; 19 | options.use_intra_process_comms(false); 20 | auto node = fiducial_vlam::vloc_node_factory(options); 21 | 22 | executor.add_node(node); 23 | 24 | // Spin until rclcpp::ok() returns false 25 | executor.spin(); 26 | 27 | // Shut down ROS 28 | rclcpp::shutdown(); 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /fiducial_vlam/src/vloc_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | 6 | #include "fiducial_math.hpp" 7 | #include "map.hpp" 8 | #include "observation.hpp" 9 | #include "vloc_context.hpp" 10 | 11 | #include "cv_bridge/cv_bridge.h" 12 | #include "nav_msgs/msg/odometry.hpp" 13 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 14 | #include "tf2_msgs/msg/tf_message.hpp" 15 | 16 | namespace fiducial_vlam 17 | { 18 | 19 | // ============================================================================== 20 | // VlocNode class 21 | // ============================================================================== 22 | 23 | class VlocNode : public rclcpp::Node 24 | { 25 | VlocContext cxt_; 26 | std::unique_ptr map_{}; 27 | std::unique_ptr camera_info_{}; 28 | std::unique_ptr camera_info_msg_{}; 29 | Localizer localizer_{map_}; 30 | std_msgs::msg::Header::_stamp_type last_image_stamp_{}; 31 | 32 | rclcpp::Publisher::SharedPtr observations_pub_{}; 33 | rclcpp::Publisher::SharedPtr camera_pose_pub_{}; 34 | rclcpp::Publisher::SharedPtr base_pose_pub_{}; 35 | rclcpp::Publisher::SharedPtr tf_message_pub_{}; 36 | rclcpp::Publisher::SharedPtr camera_odometry_pub_{}; 37 | rclcpp::Publisher::SharedPtr base_odometry_pub_{}; 38 | rclcpp::Publisher::SharedPtr image_marked_pub_{}; 39 | 40 | rclcpp::Subscription::SharedPtr camera_info_sub_; 41 | rclcpp::Subscription::SharedPtr image_raw_sub_; 42 | rclcpp::Subscription::SharedPtr map_sub_; 43 | 44 | 45 | public: 46 | VlocNode(const rclcpp::NodeOptions &options) 47 | : Node("vloc_node", options), cxt_{*this} 48 | { 49 | // Get parameters from the command line 50 | cxt_.load_parameters(); 51 | 52 | // ROS publishers. Initialize after parameters have been loaded. 53 | observations_pub_ = create_publisher( 54 | cxt_.fiducial_observations_pub_topic_, 16); 55 | 56 | if (cxt_.publish_camera_pose_) { 57 | camera_pose_pub_ = create_publisher( 58 | cxt_.camera_pose_pub_topic_, 16); 59 | } 60 | if (cxt_.publish_base_pose_) { 61 | base_pose_pub_ = create_publisher( 62 | cxt_.base_pose_pub_topic_, 16); 63 | } 64 | if (cxt_.publish_tfs_ || cxt_.publish_tfs_per_marker_) { 65 | tf_message_pub_ = create_publisher( 66 | "/tf", 16); 67 | } 68 | if (cxt_.publish_camera_odom_) { 69 | camera_odometry_pub_ = create_publisher( 70 | cxt_.camera_odometry_pub_topic_, 16); 71 | } 72 | if (cxt_.publish_base_odom_) { 73 | base_odometry_pub_ = create_publisher( 74 | cxt_.base_odometry_pub_topic_, 16); 75 | } 76 | if (cxt_.publish_image_marked_) { 77 | image_marked_pub_ = create_publisher( 78 | cxt_.image_marked_pub_topic_, 16); 79 | } 80 | 81 | // ROS subscriptions 82 | auto camera_info_qos = cxt_.sub_camera_info_best_effort_not_reliable_ ? 83 | rclcpp::QoS{rclcpp::SensorDataQoS(rclcpp::KeepLast(1))} : 84 | rclcpp::QoS{rclcpp::ServicesQoS()}; 85 | camera_info_sub_ = create_subscription( 86 | cxt_.camera_info_sub_topic_, 87 | camera_info_qos, 88 | [this](const sensor_msgs::msg::CameraInfo::UniquePtr msg) -> void 89 | { 90 | if (!camera_info_) { 91 | camera_info_ = std::make_unique(*msg); 92 | // Save the info message because we pass it along with the observations. 93 | camera_info_msg_ = std::make_unique(*msg); 94 | } 95 | }); 96 | 97 | auto image_raw_qos = cxt_.sub_image_raw_best_effort_not_reliable_ ? 98 | rclcpp::QoS{rclcpp::SensorDataQoS(rclcpp::KeepLast(1))} : 99 | rclcpp::QoS{rclcpp::ServicesQoS()}; 100 | image_raw_sub_ = create_subscription( 101 | cxt_.image_raw_sub_topic_, 102 | image_raw_qos, 103 | [this](sensor_msgs::msg::Image::UniquePtr msg) -> void 104 | { 105 | // the stamp to use for all published messages derived from this image message. 106 | auto stamp{msg->header.stamp}; 107 | 108 | if (!camera_info_) { 109 | RCLCPP_DEBUG(get_logger(), "Ignore image message because no camera_info has been received yet."); 110 | 111 | } else if ((stamp.nanosec == 0l && stamp.sec == 0l) || stamp == last_image_stamp_) { 112 | RCLCPP_DEBUG(get_logger(), "Ignore image message because stamp is zero or the same as the previous."); 113 | 114 | } else { 115 | // rviz doesn't like it when time goes backward when a bag is played again. 116 | // The stamp_msgs_with_current_time_ parameter can help this by replacing the 117 | // image message time with the current time. 118 | stamp = cxt_.stamp_msgs_with_current_time_ ? builtin_interfaces::msg::Time(now()) : stamp; 119 | process_image(std::move(msg), stamp); 120 | } 121 | 122 | last_image_stamp_ = stamp; 123 | }); 124 | 125 | map_sub_ = create_subscription( 126 | cxt_.fiducial_map_sub_topic_, 127 | 16, 128 | [this](const fiducial_vlam_msgs::msg::Map::UniquePtr msg) -> void 129 | { 130 | map_ = std::make_unique(*msg); 131 | }); 132 | 133 | (void) camera_info_sub_; 134 | (void) image_raw_sub_; 135 | (void) map_sub_; 136 | RCLCPP_INFO(get_logger(), "vloc_node ready"); 137 | } 138 | 139 | private: 140 | void process_image(sensor_msgs::msg::Image::UniquePtr image_msg, std_msgs::msg::Header::_stamp_type stamp) 141 | { 142 | // Convert ROS to OpenCV 143 | cv_bridge::CvImagePtr gray{cv_bridge::toCvCopy(*image_msg, "mono8")}; 144 | 145 | // If we are going to publish an annotated image, make a copy of 146 | // the original message image. If no annotated image is to be published, 147 | // then just make an empty image pointer. The routines need to check 148 | // that the pointer is valid before drawing into it. 149 | cv_bridge::CvImagePtr color_marked; 150 | cv::Mat mat_with_msg_data; 151 | if (cxt_.publish_image_marked_ && 152 | count_subscribers(cxt_.image_marked_pub_topic_) > 0) { 153 | // The toCvShare only makes ConstCvImage because they don't want 154 | // to modify the original message data. I want to modify the original 155 | // data so I create another CvImage that is not const and steal the 156 | // image data. 157 | std::shared_ptr tracked_object; 158 | auto const_color_marked = cv_bridge::toCvShare(*image_msg, tracked_object); 159 | mat_with_msg_data = const_color_marked->image; // opencv does not copy the image data on assignment 160 | color_marked = cv_bridge::CvImagePtr{ 161 | new cv_bridge::CvImage{const_color_marked->header, 162 | const_color_marked->encoding, 163 | mat_with_msg_data}}; 164 | } 165 | 166 | FiducialMath fm(*camera_info_); 167 | 168 | // Detect the markers in this image and create a list of 169 | // observations. 170 | auto observations = fm.detect_markers(gray, color_marked, cxt_.corner_refinement_method_); 171 | 172 | // If there is a map, find t_map_marker for each detected 173 | // marker. The t_map_markers has an entry for each element 174 | // in observations. If the marker wasn't found in the map, then 175 | // the t_map_marker entry has is_valid() as false. 176 | // Debugging hint: If the markers in color_marked are not outlined 177 | // in green, then they haven't been detected. If the markers in 178 | // color_marked are outlined but they have no axes drawn, then vmap_node 179 | // is not running or has not been able to find the starting node. 180 | if (map_) { 181 | auto t_map_markers = map_->find_t_map_markers(observations); 182 | 183 | TransformWithCovariance t_map_camera; 184 | 185 | // Only try to determine the location if markers were detected. 186 | if (observations.size()) { 187 | 188 | // RCLCPP_INFO(get_logger(), "%i observations", observations.size()); 189 | // for (auto &obs : observations.observations()) { 190 | // RCLCPP_INFO(get_logger(), 191 | // " Marker %i, p0[%8.3f, %8.3f], p1[%8.3f, %8.3f], p2[%8.3f, %8.3f], p3[%8.3f, %8.3f]", 192 | // obs.id(), 193 | // obs.x0(), obs.y0(), obs.x1(), obs.y1(), 194 | // obs.x2(), obs.y2(), obs.x3(), obs.y3() 195 | // ); 196 | // } 197 | 198 | // Find the camera pose from the observations. 199 | t_map_camera = localizer_.simultaneous_t_map_camera(observations, t_map_markers, color_marked, fm); 200 | 201 | if (t_map_camera.is_valid()) { 202 | 203 | TransformWithCovariance t_map_base{t_map_camera.transform() * cxt_.t_camera_base_.transform()}; 204 | 205 | // Publish the camera an/or base pose in the map frame 206 | if (cxt_.publish_camera_pose_) { 207 | auto pose_msg = to_PoseWithCovarianceStamped_msg(t_map_camera, stamp, cxt_.map_frame_id_); 208 | // add some fixed variance for now. 209 | add_fixed_covariance(pose_msg.pose); 210 | camera_pose_pub_->publish(pose_msg); 211 | } 212 | if (cxt_.publish_base_pose_) { 213 | auto pose_msg = to_PoseWithCovarianceStamped_msg(t_map_base, stamp, cxt_.map_frame_id_); 214 | // add some fixed variance for now. 215 | add_fixed_covariance(pose_msg.pose); 216 | base_pose_pub_->publish(pose_msg); 217 | } 218 | 219 | // Publish odometry of the camera and/or the base. 220 | if (cxt_.publish_camera_odom_) { 221 | auto odom_msg = to_odom_message(stamp, cxt_.camera_frame_id_, t_map_camera); 222 | add_fixed_covariance(odom_msg.pose); 223 | camera_odometry_pub_->publish(odom_msg); 224 | } 225 | if (cxt_.publish_base_odom_) { 226 | auto odom_msg = to_odom_message(stamp, cxt_.base_frame_id_, t_map_base); 227 | add_fixed_covariance(odom_msg.pose); 228 | base_odometry_pub_->publish(odom_msg); 229 | } 230 | 231 | // Also publish the camera's tf 232 | if (cxt_.publish_tfs_) { 233 | auto tf_message = to_tf_message(stamp, t_map_camera, t_map_base); 234 | tf_message_pub_->publish(tf_message); 235 | } 236 | 237 | // if requested, publish the camera tf as determined from each marker. 238 | if (cxt_.publish_tfs_per_marker_) { 239 | auto t_map_cameras = localizer_.markers_t_map_cameras(observations, t_map_markers, fm); 240 | auto tf_message = to_markers_tf_message(stamp, observations, t_map_cameras); 241 | if (!tf_message.transforms.empty()) { 242 | tf_message_pub_->publish(tf_message); 243 | } 244 | } 245 | 246 | // Publish the observations 247 | auto observations_msg = observations.to_msg(stamp, image_msg->header.frame_id, *camera_info_msg_); 248 | observations_pub_->publish(observations_msg); 249 | } 250 | } 251 | } 252 | 253 | // Publish an annotated image if requested. Even if there is no map. 254 | if (color_marked) { 255 | // The marking has been happening on the original message. 256 | // Republish it now. 257 | if (cxt_.stamp_msgs_with_current_time_) { 258 | image_msg->header.stamp = now(); 259 | } 260 | image_marked_pub_->publish(std::move(image_msg)); 261 | } 262 | } 263 | 264 | nav_msgs::msg::Odometry to_odom_message(std_msgs::msg::Header::_stamp_type stamp, 265 | const std::string &child_frame_id, 266 | const TransformWithCovariance &t) 267 | { 268 | nav_msgs::msg::Odometry odom_message; 269 | 270 | odom_message.header.stamp = stamp; 271 | odom_message.header.frame_id = cxt_.map_frame_id_; 272 | odom_message.child_frame_id = child_frame_id; 273 | odom_message.pose = to_PoseWithCovariance_msg(t); 274 | return odom_message; 275 | } 276 | 277 | tf2_msgs::msg::TFMessage to_tf_message(std_msgs::msg::Header::_stamp_type stamp, 278 | const TransformWithCovariance &t_map_camera, 279 | const TransformWithCovariance &t_map_base) 280 | { 281 | tf2_msgs::msg::TFMessage tf_message; 282 | 283 | geometry_msgs::msg::TransformStamped msg; 284 | msg.header.stamp = stamp; 285 | msg.header.frame_id = cxt_.map_frame_id_; 286 | 287 | // The camera_frame_id parameter is non-empty to publish the camera tf. 288 | // The base_frame_id parameter is non-empty to publish the base tf. 289 | if (!cxt_.camera_frame_id_.empty()) { 290 | msg.child_frame_id = cxt_.camera_frame_id_; 291 | msg.transform = tf2::toMsg(t_map_camera.transform()); 292 | tf_message.transforms.emplace_back(msg); 293 | } 294 | if (!cxt_.base_frame_id_.empty()) { 295 | msg.child_frame_id = cxt_.base_frame_id_; 296 | msg.transform = tf2::toMsg(t_map_base.transform()); 297 | tf_message.transforms.emplace_back(msg); 298 | } 299 | 300 | return tf_message; 301 | } 302 | 303 | tf2_msgs::msg::TFMessage to_markers_tf_message( 304 | std_msgs::msg::Header::_stamp_type stamp, 305 | const Observations &observations, 306 | const std::vector &t_map_cameras) 307 | { 308 | tf2_msgs::msg::TFMessage tf_message; 309 | 310 | geometry_msgs::msg::TransformStamped msg; 311 | msg.header.stamp = stamp; 312 | msg.header.frame_id = cxt_.map_frame_id_; 313 | 314 | for (int i = 0; i < observations.size(); i += 1) { 315 | auto &observation = observations.observations()[i]; 316 | auto &t_map_camera = t_map_cameras[i]; 317 | 318 | if (t_map_camera.is_valid()) { 319 | 320 | if (!cxt_.camera_frame_id_.empty()) { 321 | std::ostringstream oss_child_frame_id; 322 | oss_child_frame_id << cxt_.camera_frame_id_ << "_m" << std::setfill('0') << std::setw(3) 323 | << observation.id(); 324 | msg.child_frame_id = oss_child_frame_id.str(); 325 | msg.transform = tf2::toMsg(t_map_camera.transform()); 326 | tf_message.transforms.emplace_back(msg); 327 | } 328 | } 329 | } 330 | 331 | return tf_message; 332 | } 333 | 334 | void add_fixed_covariance(geometry_msgs::msg::PoseWithCovariance &pwc) 335 | { 336 | // A hack for now. 337 | pwc.covariance[0] = 6e-3; 338 | pwc.covariance[7] = 6e-3; 339 | pwc.covariance[14] = 6e-3; 340 | pwc.covariance[21] = 2e-3; 341 | pwc.covariance[28] = 2e-3; 342 | pwc.covariance[35] = 2e-3; 343 | } 344 | }; 345 | 346 | std::shared_ptr vloc_node_factory(const rclcpp::NodeOptions &options) 347 | { 348 | return std::shared_ptr(new VlocNode(options)); 349 | } 350 | } 351 | 352 | #include "rclcpp_components/register_node_macro.hpp" 353 | 354 | RCLCPP_COMPONENTS_REGISTER_NODE(fiducial_vlam::VlocNode) 355 | -------------------------------------------------------------------------------- /fiducial_vlam/src/vmap_context.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "vmap_context.hpp" 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | 6 | namespace fiducial_vlam 7 | { 8 | void VmapContext::load_parameters() 9 | { 10 | #undef CXT_MACRO_MEMBER 11 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOAD_PARAMETER(node_, (*this), n, t, d) 12 | CXT_MACRO_INIT_PARAMETERS(VMAP_ALL_PARAMS, validate_parameters) 13 | 14 | 15 | #undef CXT_MACRO_MEMBER 16 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_PARAMETER_CHANGED(n, t) 17 | CXT_MACRO_REGISTER_PARAMETERS_CHANGED(node_, (*this), VMAP_ALL_PARAMS, validate_parameters) 18 | 19 | RCLCPP_INFO(node_.get_logger(), "VmapNode Parameters"); 20 | 21 | #undef CXT_MACRO_MEMBER 22 | #define CXT_MACRO_MEMBER(n, t, d) CXT_MACRO_LOG_PARAMETER(RCLCPP_INFO, node_.get_logger(), (*this), n, t, d) 23 | VMAP_ALL_PARAMS 24 | } 25 | 26 | void VmapContext::validate_parameters() 27 | { 28 | if (std::abs(marker_map_publish_frequency_hz_) < 1.e-10) { 29 | marker_map_publish_frequency_hz_ = 30. / 60.; 30 | } 31 | 32 | map_init_transform_ = TransformWithCovariance(TransformWithCovariance::mu_type{ 33 | map_init_pose_x_, map_init_pose_y_, map_init_pose_z_, 34 | map_init_pose_roll_, map_init_pose_pitch_, map_init_pose_yaw_}); 35 | } 36 | } 37 | -------------------------------------------------------------------------------- /fiducial_vlam/src/vmap_main.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "fiducial_vlam/fiducial_vlam.hpp" 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | 6 | int main(int argc, char **argv) 7 | { 8 | // Force flush of the stdout buffer 9 | setvbuf(stdout, NULL, _IONBF, BUFSIZ); 10 | 11 | // Init ROS 12 | rclcpp::init(argc, argv); 13 | 14 | // Create single-threaded executor 15 | rclcpp::executors::SingleThreadedExecutor executor; 16 | 17 | // Create and add node 18 | rclcpp::NodeOptions options{}; 19 | options.use_intra_process_comms(false); 20 | auto node = fiducial_vlam::vmap_node_factory(options); 21 | 22 | executor.add_node(node); 23 | 24 | // Spin until rclcpp::ok() returns false 25 | executor.spin(); 26 | 27 | // Shut down ROS 28 | rclcpp::shutdown(); 29 | 30 | return 0; 31 | } 32 | -------------------------------------------------------------------------------- /fiducial_vlam/src/vmap_node.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "rclcpp/rclcpp.hpp" 9 | 10 | #include "fiducial_math.hpp" 11 | #include "map.hpp" 12 | #include "observation.hpp" 13 | #include "vmap_context.hpp" 14 | 15 | #include "tf2_geometry_msgs/tf2_geometry_msgs.h" 16 | #include "tf2_msgs/msg/tf_message.hpp" 17 | #include "visualization_msgs/msg/marker_array.hpp" 18 | #include "yaml-cpp/yaml.h" 19 | 20 | namespace fiducial_vlam 21 | { 22 | // ============================================================================== 23 | // ToYAML class 24 | // ============================================================================== 25 | 26 | class ToYAML 27 | { 28 | const Map &map_; 29 | YAML::Emitter emitter_{}; 30 | 31 | void do_header() 32 | { 33 | emitter_ << YAML::Key << "marker_length" << YAML::Value << map_.marker_length(); 34 | } 35 | 36 | void do_marker(const Marker &marker) 37 | { 38 | emitter_ << YAML::BeginMap; 39 | emitter_ << YAML::Key << "id" << YAML::Value << marker.id(); 40 | emitter_ << YAML::Key << "u" << YAML::Value << marker.update_count(); 41 | emitter_ << YAML::Key << "f" << YAML::Value << (marker.is_fixed() ? 1 : 0); 42 | auto &c = marker.t_map_marker().transform().getOrigin(); 43 | emitter_ << YAML::Key << "xyz" << YAML::Value << YAML::Flow << YAML::BeginSeq << c.x() << c.y() << c.z() 44 | << YAML::EndSeq; 45 | double roll, pitch, yaw; 46 | marker.t_map_marker().transform().getBasis().getRPY(roll, pitch, yaw); 47 | emitter_ << YAML::Key << "rpy" << YAML::Value << YAML::Flow << YAML::BeginSeq << roll << pitch << yaw 48 | << YAML::EndSeq; 49 | emitter_ << YAML::EndMap; 50 | } 51 | 52 | void do_markers() 53 | { 54 | emitter_ << YAML::Key << "markers" << YAML::Value << YAML::BeginSeq; 55 | for (auto &marker_pair : map_.markers()) { 56 | auto &marker = marker_pair.second; 57 | do_marker(marker); 58 | } 59 | emitter_ << YAML::EndSeq; 60 | } 61 | 62 | void do_map() 63 | { 64 | emitter_ << YAML::BeginMap; 65 | do_header(); 66 | do_markers(); 67 | emitter_ << YAML::EndMap; 68 | } 69 | 70 | public: 71 | explicit ToYAML(const Map &map) 72 | : map_(map) 73 | {} 74 | 75 | void to_YAML(std::ostream &out_stream) 76 | { 77 | do_map(); 78 | out_stream << emitter_.c_str() << std::endl; 79 | } 80 | }; 81 | 82 | // ============================================================================== 83 | // FromYAML class 84 | // ============================================================================== 85 | 86 | class FromYAML 87 | { 88 | YAML::Node yaml_node_{}; 89 | std::unique_ptr map_{}; 90 | std::string error_msg_{}; 91 | 92 | 93 | bool from_marker(YAML::Node &marker_node) 94 | { 95 | auto id_node = marker_node["id"]; 96 | if (!id_node.IsScalar()) { 97 | return yaml_error("marker.id failed IsScalar()"); 98 | } 99 | auto update_count_node = marker_node["u"]; 100 | if (!update_count_node.IsScalar()) { 101 | return yaml_error("marker.update_count failed IsScalar()"); 102 | } 103 | auto is_fixed_node = marker_node["f"]; 104 | if (!is_fixed_node.IsScalar()) { 105 | return yaml_error("marker.is_fixed failed IsScalar()"); 106 | } 107 | auto xyz_node = marker_node["xyz"]; 108 | if (!xyz_node.IsSequence()) { 109 | return yaml_error("marker.xyz failed IsSequence()"); 110 | } 111 | if (xyz_node.size() != 3) { 112 | return yaml_error("marker.xyz incorrect size"); 113 | } 114 | auto rpy_node = marker_node["rpy"]; 115 | if (!rpy_node.IsSequence()) { 116 | return yaml_error("marker.rpy failed IsSequence()"); 117 | } 118 | if (rpy_node.size() != 3) { 119 | return yaml_error("marker.rpy incorrect size"); 120 | } 121 | 122 | std::array xyz_data{}; 123 | for (int i = 0; i < xyz_data.size(); i += 1) { 124 | auto i_node = xyz_node[i]; 125 | if (!i_node.IsScalar()) { 126 | return yaml_error("marker.xyz[i] failed IsScalar()"); 127 | } 128 | xyz_data[i] = i_node.as(); 129 | } 130 | std::array rpy_data{}; 131 | for (int i = 0; i < rpy_data.size(); i += 1) { 132 | auto i_node = rpy_node[i]; 133 | if (!i_node.IsScalar()) { 134 | return yaml_error("marker.rpy[i] failed IsScalar()"); 135 | } 136 | rpy_data[i] = i_node.as(); 137 | } 138 | 139 | TransformWithCovariance::mu_type mu{ 140 | xyz_data[0], 141 | xyz_data[1], 142 | xyz_data[2], 143 | rpy_data[0], 144 | rpy_data[1], 145 | rpy_data[2]}; 146 | 147 | Marker marker(id_node.as(), TransformWithCovariance(mu)); 148 | marker.set_is_fixed(is_fixed_node.as()); 149 | marker.set_update_count(update_count_node.as()); 150 | map_->add_marker(std::move(marker)); 151 | return true; 152 | } 153 | 154 | bool from_markers(YAML::Node &markers_node) 155 | { 156 | for (YAML::const_iterator it = markers_node.begin(); it != markers_node.end(); ++it) { 157 | YAML::Node marker_node = *it; 158 | if (marker_node.IsMap()) { 159 | if (from_marker(marker_node)) { 160 | continue; 161 | } 162 | return false; 163 | } 164 | return yaml_error("marker failed IsMap()"); 165 | } 166 | return true; 167 | } 168 | 169 | bool from_map() 170 | { 171 | if (yaml_node_.IsMap()) { 172 | auto marker_length_node = yaml_node_["marker_length"]; 173 | if (marker_length_node.IsScalar()) { 174 | auto marker_length = marker_length_node.as(); 175 | // create the map object now that we have the marker_length; 176 | map_ = std::make_unique(marker_length); 177 | auto markers_node = yaml_node_["markers"]; 178 | if (markers_node.IsSequence()) { 179 | return from_markers(markers_node); 180 | } 181 | return yaml_error("markers failed IsSequence()"); 182 | } 183 | return yaml_error("marker_length failed IsScalar()"); 184 | } 185 | return yaml_error("root failed IsMap()"); 186 | } 187 | 188 | bool yaml_error(const std::string &s) 189 | { 190 | return false; 191 | } 192 | 193 | public: 194 | FromYAML() = default; 195 | 196 | std::unique_ptr from_YAML(std::istream &in) 197 | { 198 | try { 199 | yaml_node_ = YAML::Load(in); 200 | error_msg_ = ""; 201 | if (!from_map() && map_) { 202 | map_.release(); 203 | } 204 | return std::move(map_); 205 | } 206 | catch (YAML::ParserException &ex) { 207 | if (map_) { 208 | map_.release(); 209 | } 210 | error_msg_ = ex.what(); 211 | } 212 | return std::move(map_); 213 | } 214 | 215 | std::string error_msg() 216 | { 217 | return error_msg_; 218 | } 219 | }; 220 | 221 | // ============================================================================== 222 | // Mapper class 223 | // ============================================================================== 224 | 225 | class Mapper 226 | { 227 | protected: 228 | const VmapContext &cxt_; 229 | const std::unique_ptr &map_; 230 | 231 | public: 232 | explicit Mapper(const VmapContext &cxt, std::unique_ptr &map) 233 | : cxt_(cxt), map_(map) 234 | { 235 | } 236 | 237 | Mapper() = delete; 238 | 239 | virtual ~Mapper() = default; 240 | 241 | virtual void 242 | update_map(const TransformWithCovariance &camera_pose_f_map, 243 | const Observations &observations, 244 | FiducialMath &fm) = 0; 245 | 246 | virtual std::unique_ptr 247 | initialize_map_from_observations(const Observations &observations, 248 | FiducialMath &fm) = 0; 249 | }; 250 | 251 | // ============================================================================== 252 | // MapperSimpleAverage class 253 | // ============================================================================== 254 | 255 | class MapperSimpleAverage : public Mapper 256 | { 257 | public: 258 | explicit MapperSimpleAverage(const VmapContext &cxt, std::unique_ptr &map) 259 | : Mapper(cxt, map) 260 | { 261 | } 262 | 263 | void 264 | update_map(const TransformWithCovariance &t_map_camera, 265 | const Observations &observations, 266 | FiducialMath &fm) override 267 | { 268 | // For all observations estimate the marker location and update the map 269 | for (auto &observation : observations.observations()) { 270 | 271 | auto t_camera_marker = fm.solve_t_camera_marker(observation, map_->marker_length()); 272 | auto t_map_marker = TransformWithCovariance(t_map_camera.transform() * t_camera_marker.transform()); 273 | 274 | // Update an existing marker or add a new one. 275 | auto marker_ptr = map_->find_marker(observation.id()); 276 | if (marker_ptr) { 277 | auto &marker = *marker_ptr; 278 | update_marker_simple_average(marker, t_map_marker); 279 | 280 | } else { 281 | map_->add_marker(Marker(observation.id(), t_map_marker)); 282 | } 283 | } 284 | } 285 | 286 | void update_marker_simple_average(Marker &existing, const TransformWithCovariance &another_twc) 287 | { 288 | if (!existing.is_fixed()) { 289 | auto t_map_marker = existing.t_map_marker(); // Make a copy 290 | auto update_count = existing.update_count(); 291 | t_map_marker.update_simple_average(another_twc, update_count); 292 | existing.set_t_map_marker(t_map_marker); 293 | existing.set_update_count(update_count + 1); 294 | } 295 | } 296 | 297 | std::unique_ptr initialize_map_from_observations(const Observations &observations, FiducialMath &fm) override 298 | { 299 | std::unique_ptr map_unique{}; 300 | if (observations.size() < 1) { 301 | return map_unique; 302 | } 303 | 304 | // Find the marker with the lowest id 305 | int min_id = std::numeric_limits::max(); 306 | const Observation *min_obs; 307 | for (auto &obs : observations.observations()) { 308 | if (obs.id() < min_id) { 309 | min_id = obs.id(); 310 | min_obs = &obs; 311 | } 312 | } 313 | 314 | // Find t_camera_marker 315 | auto t_camera_marker = fm.solve_t_camera_marker(*min_obs, cxt_.marker_length_); 316 | 317 | // And t_map_camera 318 | auto t_map_camera = cxt_.map_init_transform_; 319 | 320 | // Figure t_map_marker and add a marker to the map. 321 | auto t_map_marker = TransformWithCovariance(t_map_camera.transform() * t_camera_marker.transform()); 322 | map_unique->add_marker(Marker(min_id, std::move(t_map_marker))); 323 | 324 | return map_unique; 325 | } 326 | }; 327 | 328 | // ============================================================================== 329 | // VmapNode class 330 | // ============================================================================== 331 | 332 | class VmapNode : public rclcpp::Node 333 | { 334 | VmapContext cxt_; 335 | std::unique_ptr map_{}; 336 | Localizer localizer_{map_}; 337 | std::unique_ptr mapper_{}; 338 | 339 | int callbacks_processed_{0}; 340 | 341 | // ROS publishers 342 | rclcpp::Publisher::SharedPtr fiducial_map_pub_{}; 343 | rclcpp::Publisher::SharedPtr fiducial_markers_pub_{}; 344 | rclcpp::Publisher::SharedPtr tf_message_pub_{}; 345 | 346 | rclcpp::Subscription::SharedPtr observations_sub_{}; 347 | rclcpp::TimerBase::SharedPtr map_pub_timer_{}; 348 | 349 | public: 350 | 351 | VmapNode(const rclcpp::NodeOptions &options) 352 | : Node("vmap_node", options), cxt_{*this} 353 | { 354 | // Get parameters from the command line 355 | cxt_.load_parameters(); 356 | 357 | // Initialize the map. Load from file or otherwise. 358 | map_ = initialize_map(); 359 | 360 | // auto s = to_YAML_string(*map_, "test"); 361 | // auto m = from_YAML_string(s, "test"); 362 | 363 | // construct a map builder. 364 | mapper_ = std::make_unique(cxt_, map_); 365 | 366 | // ROS publishers. 367 | fiducial_map_pub_ = create_publisher( 368 | cxt_.fiducial_map_pub_topic_, 16); 369 | 370 | if (cxt_.publish_marker_visualizations_) { 371 | fiducial_markers_pub_ = create_publisher( 372 | cxt_.fiducial_markers_pub_topic_, 16); 373 | } 374 | 375 | if (cxt_.publish_tfs_) { 376 | tf_message_pub_ = create_publisher("tf", 16); 377 | } 378 | 379 | // ROS subscriptions 380 | // If we are not making a map, don't bother subscribing to the observations. 381 | if (cxt_.make_not_use_map_) { 382 | observations_sub_ = create_subscription( 383 | cxt_.fiducial_observations_sub_topic_, 384 | 16, 385 | [this](const fiducial_vlam_msgs::msg::Observations::UniquePtr msg) -> void 386 | { 387 | this->observations_callback(msg); 388 | }); 389 | } 390 | 391 | // Timer for publishing map info 392 | map_pub_timer_ = create_wall_timer( 393 | std::chrono::milliseconds(static_cast(1000. / cxt_.marker_map_publish_frequency_hz_)), 394 | [this]() -> void 395 | { 396 | // Only if there is a map. There might not 397 | // be a map if no markers have been observed. 398 | if (map_) { 399 | this->publish_map_and_visualization(); 400 | } 401 | }); 402 | 403 | (void) observations_sub_; 404 | (void) map_pub_timer_; 405 | RCLCPP_INFO(get_logger(), "vmap_node ready"); 406 | } 407 | 408 | private: 409 | 410 | void observations_callback(const fiducial_vlam_msgs::msg::Observations::UniquePtr &msg) 411 | { 412 | callbacks_processed_ += 1; 413 | 414 | CameraInfo ci{msg->camera_info}; 415 | FiducialMath fm{ci}; 416 | std::shared_ptr color_marked; // No image to annotate 417 | 418 | // Get observations from the message. 419 | Observations observations(*msg); 420 | 421 | // If the map has not yet been initialized, then initialize it with these observations. 422 | if (!map_) { 423 | map_ = mapper_->initialize_map_from_observations(observations, fm); 424 | } 425 | 426 | // There is nothing to do at this point unless we have more than one observation. 427 | if (observations.size() < 2) { 428 | return; 429 | } 430 | 431 | // Estimate the camera pose using the latest map estimate 432 | auto t_map_markers = map_->find_t_map_markers(observations); 433 | auto t_map_camera = localizer_.simultaneous_t_map_camera(observations, t_map_markers, color_marked, fm); 434 | 435 | // We get an invalid pose if none of the visible markers pose's are known. 436 | if (t_map_camera.is_valid()) { 437 | 438 | // Update our map with the observations 439 | mapper_->update_map(t_map_camera, observations, fm); 440 | } 441 | } 442 | 443 | tf2_msgs::msg::TFMessage to_tf_message() 444 | { 445 | auto stamp = now(); 446 | tf2_msgs::msg::TFMessage tf_message; 447 | 448 | for (auto &marker_pair: map_->markers()) { 449 | auto &marker = marker_pair.second; 450 | auto mu = marker.t_map_marker().mu(); 451 | 452 | std::ostringstream oss_child_frame_id; 453 | oss_child_frame_id << cxt_.marker_prefix_frame_id_ << std::setfill('0') << std::setw(3) << marker.id(); 454 | 455 | tf2::Quaternion q; 456 | q.setRPY(mu[3], mu[4], mu[5]); 457 | auto tf2_transform = tf2::Transform(q, tf2::Vector3(mu[0], mu[1], mu[2])); 458 | 459 | geometry_msgs::msg::TransformStamped msg; 460 | msg.header.stamp = stamp; 461 | msg.header.frame_id = cxt_.map_frame_id_; 462 | msg.child_frame_id = oss_child_frame_id.str(); 463 | msg.transform = tf2::toMsg(tf2_transform); 464 | 465 | tf_message.transforms.emplace_back(msg); 466 | } 467 | 468 | return tf_message; 469 | } 470 | 471 | visualization_msgs::msg::MarkerArray to_marker_array_msg() 472 | { 473 | visualization_msgs::msg::MarkerArray markers; 474 | for (auto &marker_pair: map_->markers()) { 475 | auto &marker = marker_pair.second; 476 | visualization_msgs::msg::Marker marker_msg; 477 | marker_msg.id = marker.id(); 478 | marker_msg.header.frame_id = cxt_.map_frame_id_; 479 | marker_msg.pose = to_Pose_msg(marker.t_map_marker()); 480 | marker_msg.type = visualization_msgs::msg::Marker::CUBE; 481 | marker_msg.action = visualization_msgs::msg::Marker::ADD; 482 | marker_msg.scale.x = 0.1; 483 | marker_msg.scale.y = 0.1; 484 | marker_msg.scale.z = 0.01; 485 | marker_msg.color.r = 1.f; 486 | marker_msg.color.g = 1.f; 487 | marker_msg.color.b = 0.f; 488 | marker_msg.color.a = 1.f; 489 | markers.markers.emplace_back(marker_msg); 490 | } 491 | return markers; 492 | } 493 | 494 | void publish_map_and_visualization() 495 | { 496 | // publish the map 497 | std_msgs::msg::Header header; 498 | header.stamp = now(); 499 | header.frame_id = cxt_.map_frame_id_; 500 | fiducial_map_pub_->publish(*map_->to_map_msg(header, map_->marker_length())); 501 | 502 | // Publish the marker Visualization 503 | if (cxt_.publish_marker_visualizations_) { 504 | fiducial_markers_pub_->publish(to_marker_array_msg()); 505 | } 506 | 507 | // Publish the transform tree 508 | if (cxt_.publish_tfs_) { 509 | tf_message_pub_->publish(to_tf_message()); 510 | } 511 | 512 | // Save the map 513 | if (cxt_.make_not_use_map_ && !cxt_.marker_map_save_full_filename_.empty()) { 514 | to_YAML_file(*map_, cxt_.marker_map_save_full_filename_); 515 | } 516 | } 517 | 518 | void to_YAML_stream(const Map &map, std::ostream &out, const std::string &filename) 519 | { 520 | ToYAML to_YAML{map}; 521 | to_YAML.to_YAML(out); 522 | } 523 | 524 | std::string to_YAML_string(const Map &map, const std::string &filename) 525 | { 526 | std::stringstream out; 527 | to_YAML_stream(map, out, filename); 528 | return out.str(); 529 | } 530 | 531 | void to_YAML_file(const Map &map, const std::string &filename) 532 | { 533 | std::ofstream out(filename); 534 | if (out) { 535 | to_YAML_stream(map, out, filename); 536 | return; 537 | } 538 | RCLCPP_INFO(get_logger(), "Config error: can not open config file for writing: '%s'", filename.c_str()); 539 | } 540 | 541 | std::unique_ptr from_YAML_stream(std::istream &in, const std::string &filename) 542 | { 543 | FromYAML from_YAML; 544 | auto map_unique = from_YAML.from_YAML(in); 545 | if (!map_unique) { 546 | // Only display an error if a filename is provided. 547 | if (!filename.empty()) { 548 | RCLCPP_INFO(get_logger(), "Config error: error parsing config file: '%s'", filename.c_str()); 549 | RCLCPP_INFO(get_logger(), "Config error: '%s'", from_YAML.error_msg().c_str()); 550 | } 551 | } 552 | return map_unique; 553 | } 554 | 555 | std::unique_ptr from_YAML_string(const std::string &yaml, const std::string &filename) 556 | { 557 | std::stringstream ss{yaml}; 558 | return from_YAML_stream(ss, filename); 559 | } 560 | 561 | std::unique_ptr from_YAML_file(const std::string &filename) 562 | { 563 | std::ifstream in; 564 | in.open(filename, std::ifstream::in); 565 | if (in.good()) { 566 | return from_YAML_stream(in, filename); 567 | } 568 | RCLCPP_INFO(get_logger(), "Config error: can not open config file for reading: '%s'", filename.c_str()); 569 | return std::unique_ptr{}; 570 | } 571 | 572 | 573 | std::unique_ptr initialize_map() 574 | { 575 | std::unique_ptr map_unique{}; 576 | 577 | // If not building a map, then load the map from a file 578 | if (!cxt_.make_not_use_map_) { 579 | RCLCPP_INFO(get_logger(), "Loading map file '%s'", cxt_.marker_map_load_full_filename_.c_str()); 580 | 581 | // load the map. 582 | map_unique = from_YAML_file(cxt_.marker_map_load_full_filename_); 583 | if (map_unique) { 584 | return map_unique; 585 | } 586 | // If an error, fall into initialize the map 587 | RCLCPP_ERROR(get_logger(), "Could not load map file"); 588 | RCLCPP_ERROR(get_logger(), "Falling into initialize map. (style: %d)", cxt_.map_init_style_); 589 | } 590 | 591 | // Building a map. Use the different styles of map initialization. 592 | // If style 2, then need to wait for an observation for initialization. 593 | if (cxt_.map_init_style_ == 2) { 594 | return map_unique; 595 | } 596 | 597 | // if Style == 0, look for a file and pull the pose from it. 598 | // If there is a problem, fall into style 1. 599 | if (cxt_.map_init_style_ == 0) { 600 | auto map_temp = from_YAML_file(cxt_.marker_map_load_full_filename_); 601 | if (map_temp) { 602 | auto marker_temp = map_temp->find_marker(cxt_.map_init_id_); 603 | if (marker_temp) { 604 | auto marker_copy = *marker_temp; 605 | marker_copy.set_is_fixed(true); 606 | map_unique = std::make_unique(cxt_.marker_length_); 607 | map_unique->add_marker(std::move(marker_copy)); 608 | return map_unique; 609 | } 610 | } 611 | } 612 | 613 | // Style 1 initialization. Get the info from parameters. 614 | map_unique = std::make_unique(cxt_.marker_length_); 615 | auto marker_new = Marker(cxt_.map_init_id_, cxt_.map_init_transform_); 616 | marker_new.set_is_fixed(true); 617 | map_unique->add_marker(std::move(marker_new)); 618 | 619 | return map_unique; 620 | } 621 | }; 622 | 623 | std::shared_ptr vmap_node_factory(const rclcpp::NodeOptions &options) 624 | { 625 | return std::shared_ptr(new VmapNode(options)); 626 | } 627 | } 628 | 629 | #include "rclcpp_components/register_node_macro.hpp" 630 | 631 | RCLCPP_COMPONENTS_REGISTER_NODE(fiducial_vlam::VmapNode) 632 | -------------------------------------------------------------------------------- /fiducial_vlam_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(fiducial_vlam_msgs) 3 | 4 | # Default to C99 5 | if(NOT CMAKE_C_STANDARD) 6 | set(CMAKE_C_STANDARD 99) 7 | endif() 8 | 9 | # Default to C++14 10 | if(NOT CMAKE_CXX_STANDARD) 11 | set(CMAKE_CXX_STANDARD 14) 12 | endif() 13 | 14 | # Emulate colcon in CLion 15 | if($ENV{CLION_IDE}) 16 | message("Running inside CLion") 17 | find_package(fastrtps_cmake_module REQUIRED) 18 | set(FastRTPS_INCLUDE_DIR "/opt/ros/bouncy/include") 19 | set(FastRTPS_LIBRARY_RELEASE "/opt/ros/bouncy/lib/libfastrtps.so") 20 | endif() 21 | 22 | # Find packages 23 | find_package(ament_cmake REQUIRED) 24 | find_package(rosidl_default_generators REQUIRED) 25 | find_package(geometry_msgs REQUIRED) 26 | find_package(sensor_msgs REQUIRED) 27 | find_package(std_msgs REQUIRED) 28 | 29 | include_directories( 30 | ${geometry_msgs_INCLUDE_DIRS} 31 | ${sensor_msgs_INCLUDE_DIRS} 32 | ${std_msgs_INCLUDE_DIRS} 33 | ) 34 | 35 | set(msg_files 36 | "msg/Map.msg" 37 | "msg/Observation.msg" 38 | "msg/Observations.msg" 39 | ) 40 | 41 | # Generate ROS interfaces 42 | rosidl_generate_interfaces( 43 | ${PROJECT_NAME} 44 | ${msg_files} 45 | DEPENDENCIES std_msgs geometry_msgs sensor_msgs 46 | ) 47 | 48 | ament_package() 49 | -------------------------------------------------------------------------------- /fiducial_vlam_msgs/msg/Map.msg: -------------------------------------------------------------------------------- 1 | # A list of the markers in the environment 2 | 3 | std_msgs/Header header 4 | 5 | # Length in meters of a side of all markers 6 | float64 marker_length 7 | 8 | # id and pose of the markers 9 | int32[] fixed_flags 10 | int32[] ids 11 | geometry_msgs/PoseWithCovariance[] poses -------------------------------------------------------------------------------- /fiducial_vlam_msgs/msg/Observation.msg: -------------------------------------------------------------------------------- 1 | # An observation of a marker. 2 | int32 id 3 | 4 | # vertices 5 | float64 x0 6 | float64 y0 7 | float64 x1 8 | float64 y1 9 | float64 x2 10 | float64 y2 11 | float64 x3 12 | float64 y3 13 | -------------------------------------------------------------------------------- /fiducial_vlam_msgs/msg/Observations.msg: -------------------------------------------------------------------------------- 1 | # An observation of several markers 2 | 3 | # the header from the image 4 | std_msgs/Header header 5 | 6 | # the CameraInfo for the camera that captured the image 7 | sensor_msgs/CameraInfo camera_info 8 | 9 | # A list of locations of marker corners in the image 10 | Observation[] observations -------------------------------------------------------------------------------- /fiducial_vlam_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | fiducial_vlam_msgs 6 | 0.1.0 7 | fiducial_vlam_msgs for ROS2 8 | 9 | Peter Mullen 10 | BSD 11 | 12 | https://github.com/ptrmu/fiducial_vlam.git 13 | https://github.com/ptrmu/fiducial_vlam/issues 14 | 15 | Clyde McQueen 16 | Peter Mullen 17 | 18 | ament_cmake 19 | rosidl_default_generators 20 | 21 | builtin_interfaces 22 | geometry_msgs 23 | sensor_msgs 24 | std_msgs 25 | 26 | rosidl_interface_packages 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /images/rviz_in_out_path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ptrmu/fiducial_vlam/e76988d50e6e18ef45be825f198b3434d21fb155/images/rviz_in_out_path.png --------------------------------------------------------------------------------