### Implement a UFOMap Publisher Source: https://github.com/unknownfreeoccupied/ufomap/wiki/ROS-Tutorials Example of initializing a colored UFOMap and publishing it as a ROS message. ```cpp // We will publish colored UFOMaps in this tutorial #include // UFOMap ROS msg #include // To convert between UFO and ROS #include #include #include int main(int argc, char *argv[]) { ros::init(argc, argv, "ufomap_publisher"); ros::NodeHandle nh; ros::Publisher map_pub = nh.advertise("map", 10); // Create a colored UFOMap ufo::map::OccupancyMapColor map(0.1); // If the UFOMap should be compressed using LZ4. // Good if you are sending the UFOMap between computers. bool compress = false; // Lowest depth to publish. // Higher value means less data to transfer, good in // situation where the data rate is low. // Many nodes do not require detailed maps as well. ufo::map::DepthType pub_depth = 0; ros::Rate rate(10); while (ros::ok()) { // TODO: Integrate data into map // This is the UFOMap message object. ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped); // Convert UFOMap to ROS message if (ufomap_msgs::ufoToMsg(map, msg->map, compress, pub_depth)) { // Conversion was successful msg->header.stamp = ros::Time::now(); msg->header.frame_id = "map"; map_pub.publish(msg); } rate.sleep(); } return 0; } ``` -------------------------------- ### Build UFOMap Workspace and Source Source: https://github.com/unknownfreeoccupied/ufomap/wiki/Setup Compile your Catkin workspace after cloning UFOMap and then source the setup file to make UFOMap available in your ROS environment. ```bash # Install all dependencies. Not really needed since we already installed TBB above rosdep install --from-paths . --ignore-src -r -y # Build your workspace catkin build # Source your workspace source ../devel/setup.bash ``` -------------------------------- ### Install Launch Files Directory Source: https://github.com/unknownfreeoccupied/ufomap/blob/master/ufomap_ros/ufomap_mapping/CMakeLists.txt Installs a directory containing launch files to the package's share destination, specifically under a 'launch' subdirectory. This is common for ROS packages. ```cmake install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch ) ``` -------------------------------- ### Install System Dependencies for UFOMap Source: https://github.com/unknownfreeoccupied/ufomap/wiki/Setup Install the TBB development library, which is a required dependency for UFOMap's C++17 execution policies. ```bash sudo apt install libtbb-dev ``` -------------------------------- ### Initialize and Configure Catkin Workspace Source: https://github.com/unknownfreeoccupied/ufomap/wiki/Setup Initialize a Catkin workspace and configure it for building with CMake release optimizations. This setup is for ROS integration. ```bash mkdir -p ~/catkin_ws/src cd ~/catkin_ws catkin init # Init Catkin workspace catkin config --extend /opt/ros/noetic # exchange noetic for your ros distro if necessary catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release # To enable release mode compiler optimzations ``` -------------------------------- ### Install Header Directory Source: https://github.com/unknownfreeoccupied/ufomap/blob/master/ufomap_ros/ufomap_mapping/CMakeLists.txt Installs a directory of header files to the package's include destination. Only files matching the '*.h' pattern are installed, excluding version control directories. ```cmake install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" ) ``` -------------------------------- ### UFOMap Collision Checking Example Source: https://context7.com/unknownfreeoccupied/ufomap/llms.txt Demonstrates initializing an OccupancyMapColor, defining robot and goal positions, and performing collision checks using a sphere and an oriented bounding box. Shows how to check against occupied, unknown, or combined space at different depths. ```cpp int main(int argc, char *argv[]) { double resolution = 0.02; // 2 cm voxel size ufo::map::OccupancyMapColor map(resolution); // ... fill map with data ... ufo::math::Vector3 robot_pos(0.0, 0.0, 0.0); ufo::math::Vector3 goal(10.0, 5.0, 0.0); double robot_radius = 0.5; // Check collision with sphere at robot position ufo::geometry::Sphere robot_sphere(robot_pos, robot_radius); // Check against occupied space at finest resolution (depth 0 = 2 cm) if (isInCollision(map, robot_sphere, true, false, false, 0)) { std::cout << "Collision with occupied space!" << std::endl; } // Check against unknown space at depth 1 (4 cm) if (isInCollision(map, robot_sphere, false, false, true, 1)) { std::cout << "Collision with unknown space!" << std::endl; } // Check against both occupied AND unknown at depth 2 (8 cm) if (isInCollision(map, robot_sphere, true, false, true, 2)) { std::cout << "Collision with occupied or unknown!" << std::endl; } // Check path clearance with oriented bounding box ufo::math::Vector3 direction = goal - robot_pos; ufo::math::Vector3 center = robot_pos + (direction / 2.0); double distance = direction.norm(); direction /= distance; double yaw = -atan2(direction[1], direction[0]); double pitch = -asin(direction[2]); ufo::geometry::OBB path_obb(center, ufo::math::Vector3(distance / 2.0, robot_radius, robot_radius), ufo::math::Quaternion(0, pitch, yaw)); if (!isInCollision(map, path_obb, true, false, true, 3)) { std::cout << "Path to goal is clear!" << std::endl; } // Available bounding volumes: // ufo::geometry::AABB - Axis-aligned bounding box // ufo::geometry::OBB - Oriented bounding box // ufo::geometry::Sphere - Sphere // ufo::geometry::Frustum - View frustum (camera simulation) // ufo::geometry::LineSegment - Line segment // ufo::geometry::Plane - Plane // ufo::geometry::Ray - Ray // ufo::geometry::Point - Point return 0; } ``` -------------------------------- ### CMakeLists.txt for ufomap_srvs Source: https://github.com/unknownfreeoccupied/ufomap/blob/master/ufomap_ros/ufomap_srvs/CMakeLists.txt Configures the catkin package, generates service messages, and installs header files. ```cmake cmake_minimum_required(VERSION 3.0.2) project(ufomap_srvs) find_package(catkin REQUIRED COMPONENTS message_generation ufomap_msgs ) add_service_files( FILES ClearVolume.srv GetMap.srv Reset.srv SaveMap.srv ) generate_messages( DEPENDENCIES ufomap_msgs ) catkin_package( CATKIN_DEPENDS message_runtime ufomap_msgs ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" ) ``` -------------------------------- ### Install Executable Target Source: https://github.com/unknownfreeoccupied/ufomap/blob/master/ufomap_ros/ufomap_mapping/CMakeLists.txt Installs an executable target to the specified runtime destination within the package structure. Ensure the target name matches the executable built. ```cmake install(TARGETS ${PROJECT_NAME}_server_node RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) ``` -------------------------------- ### Implement a UFOMap Subscriber Source: https://github.com/unknownfreeoccupied/ufomap/wiki/ROS-Tutorials Example of receiving a UFOMap ROS message and converting it back into a local UFOMap object. ```cpp // We will subscribe to colored UFOMaps in this tutorial #include // UFOMap ROS msg #include // To convert between UFO and ROS #include #include #include // Create a colored UFOMap ufo::map::OccupancyMapColor map(0.1); void mapCallback(ufomap_msgs::UFOMapStamped::ConstPtr const& msg) { // Convert ROS message to UFOMap if (ufomap_msgs::msgToUfo(msg->map, map)) { // Conversion was successful ROS_INFO("UFOMap conversion successful"); } else { ROS_WARN("UFOMap conversion failed"); } } int main(int argc, char *argv[]) { ros::init(argc, argv, "ufomap_subscriber"); ros::NodeHandle nh; ros::Subscriber map_sub = nh.subscribe("map", 10, mapCallback); ros::spin(); return 0; } ``` -------------------------------- ### Configure ROS Package Build with CMake Source: https://github.com/unknownfreeoccupied/ufomap/blob/master/ufomap_ros/ufomap_ros/CMakeLists.txt This CMakeLists.txt file sets up a ROS package, defining C++ standard, finding dependencies, creating a static library, and specifying installation targets. ```cmake cmake_minimum_required(VERSION 3.8.2) project(ufomap_ros) set(CMAKE_CXX_STANDARD 17) find_package(catkin REQUIRED COMPONENTS geometry_msgs roscpp sensor_msgs ) find_package(ufomap REQUIRED) catkin_package( INCLUDE_DIRS include ${catkin_INCLUDE_DIRS} LIBRARIES ${PROJECT_NAME} ${catkin_LIBRARIES} CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs ) include_directories( include ${catkin_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} STATIC src/conversions.cpp ) # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES} UFO::Map ) install(TARGETS ${PROJECT_NAME} ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE ) ``` -------------------------------- ### Initialize Geometry Types in C++ Source: https://context7.com/unknownfreeoccupied/ufomap/llms.txt Demonstrates instantiation of various geometric primitives and combining them into a BoundingVolume. Ensure all relevant headers from ufo/geometry are included. ```cpp #include #include #include #include #include #include #include #include int main() { // Axis-Aligned Bounding Box (AABB) ufo::math::Vector3 min_pt(-1.0, -1.0, 0.0); ufo::math::Vector3 max_pt(1.0, 1.0, 2.0); ufo::geometry::AABB aabb(min_pt, max_pt); // Alternative: center + half_size ufo::geometry::AABB aabb2(ufo::geometry::Point(0, 0, 1), 1.0); // Cube // Sphere ufo::geometry::Point center(0.0, 0.0, 0.0); double radius = 0.5; ufo::geometry::Sphere sphere(center, radius); // Oriented Bounding Box (OBB) ufo::math::Vector3 obb_center(0.0, 0.0, 0.0); ufo::math::Vector3 half_extents(1.0, 0.5, 0.3); ufo::math::Quaternion rotation(0.707, 0.0, 0.707, 0.0); // 90 deg rotation ufo::geometry::OBB obb(obb_center, half_extents, rotation); // Line Segment ufo::geometry::Point start(0.0, 0.0, 0.0); ufo::geometry::Point end(5.0, 5.0, 0.0); ufo::geometry::LineSegment line(start, end); // Ray (origin + direction) ufo::geometry::Point origin(0.0, 0.0, 0.0); ufo::math::Vector3 direction(1.0, 0.0, 0.0); ufo::geometry::Ray ray(origin, direction); // Plane (normal + distance from origin) ufo::math::Vector3 normal(0.0, 0.0, 1.0); double distance = 0.0; ufo::geometry::Plane plane(normal, distance); // Frustum (camera view volume) // Defined by position, direction, vertical FOV, aspect ratio, near/far planes ufo::geometry::Frustum frustum( ufo::geometry::Point(0, 0, 0), // position ufo::geometry::Point(1, 0, 0), // target ufo::geometry::Point(0, 0, 1), // up 60.0, // vertical FOV (degrees) 1.333, // aspect ratio 0.1, // near plane 10.0 // far plane ); // Combine multiple volumes ufo::geometry::BoundingVolume combined; combined.add(aabb); combined.add(sphere); // combined now matches union of both volumes return 0; } ``` -------------------------------- ### Create UFOMap Instances Source: https://context7.com/unknownfreeoccupied/ufomap/llms.txt Initialize standard or colored occupancy maps using resolution and octree depth parameters. Disable automatic pruning for thread-safe access. ```cpp #include #include int main(int argc, char *argv[]) { // Basic map creation with 10 cm voxel resolution double resolution = 0.1; ufo::map::OccupancyMap simple_map(resolution); // Full configuration with all parameters ufo::map::DepthType depth_levels = 16; // Octree depth [2, 21] bool automatic_pruning = false; // Disable for thread-safe access double occupied_thres = 0.5; // Occupied classification threshold double free_thres = 0.5; // Free classification threshold double prob_hit = 0.7; // Occupancy increase on hit (0.5, 1] double prob_miss = 0.4; // Occupancy decrease on miss [0, 0.5) double clamping_thres_min = 0.1192; // Min clamping threshold double clamping_thres_max = 0.971; // Max clamping threshold ufo::map::OccupancyMap map(resolution, depth_levels, automatic_pruning, occupied_thres, free_thres, prob_hit, prob_miss, clamping_thres_min, clamping_thres_max); // Create a colored UFOMap for RGB point clouds ufo::map::OccupancyMapColor colored_map(resolution, depth_levels, automatic_pruning); // Maximum representable volume: resolution * 2^depth_levels meters per dimension // With resolution=0.1 and depth_levels=16: 0.1 * 65536 = 6553.6 meters return 0; } ``` -------------------------------- ### Load UFOMap from file in C++ Source: https://github.com/unknownfreeoccupied/ufomap/wiki/Tutorials Demonstrates reading a map file type and initializing either an OccupancyMap or OccupancyMapColor object, optionally using std::variant for polymorphic storage. ```cpp #include #include #include int main(int argc, char *argv[]) { std::string map_type = ufo::map::OccupancyMap::readType("some_ufomap_file.um"); if ("occupancy_map" == map_type) { ufo::map::OccupancyMap map("some_ufomap_file.um"); } else if ("occupancy_map_color" == map_type) { ufo::map::OccupancyMapColor map("some_ufomap_file.um"); } // // You can also use std::variant // // Create a UFOMap variant // The first is std::monostate so we do not have to initialize it. std::variant map; if ("occupancy_map" == map_type) { map.emplace("some_ufomap_file.um"); } else if ("occupancy_map_color" == map_type) { map.emplace("some_ufomap_file.um"); } // Use std::visit to access map return 0; } ``` -------------------------------- ### Create UFOMap with Custom Parameters Source: https://github.com/unknownfreeoccupied/ufomap/wiki/Tutorials Initializes an OccupancyMap with specified resolution, depth levels, pruning, occupancy thresholds, and hit/miss probabilities. Use this for fine-grained control over map properties. ```cpp #include #include int main(int argc, char *argv[]) { // 10 cm voxel size. double resolution = 0.1; // Number of levels for the octree. Has to be [2, 21]. // This determines the maximum volume that can be represented by the map // resolution * 2^(depth_levels) meter in each dimension. ufo::map::DepthType depth_levels = 16; // Default: 16 // If automatic pruning should be enabled/disabled. // If disabled it is possible to safely integrate and access data // at the same time. Can manually prune the tree. bool automatic_pruning = false; // Default: true // Occupied threshold. Voxels with an occupancy value // strictly above this will be classified as occupied. double occupied_thres = 0.5; // Default: 0.5 // Free threshold. Voxels with an occupancy value // strictly below this will be classified as free. double free_thres = 0.5; // Default: 0.5 // Voxels with an occupancy value free_thres <= v <= occupied_thres // will be classified as unknown. // How much the occupancy value of a voxel should be increased when "hit". // This value should be (0.5, 1]. double prob_hit = 0.7; // Default: 0.7 // How much the occupancy value of a voxel should be decreased when "miss". // This value should be [0, 0.5). double prob_miss = 0.4; // Default: 0.4 // Minimum and maximum clamping thresholds. These are used to increased // the amount the can be pruned. A voxel with with an occupancy value // clamping_thres_max < v < clamping_thres_min will be clamped to either // clamping_thres_min or clamping_thres_max. double clamping_thres_min = 0.1192; // Default: 0.1192 double clamping_thres_max = 0.971; // Default: 0.971 // To create a UFOMap ufo::map::OccupancyMap map(resolution, depth_levels, automatic_pruning, occupied_thres, free_thres, prob_hit, prob_miss, clamping_thres_min, clamping_thres_max); // We can also use the default parameter values. You only have to specify a resolution, // if you do not wish to change anything else. ufo::map::OccupancyMap map_2(resolution); // To create a colored UFOMap without automatic pruning ufo::map::OccupancyMapColor map_colored(resolution, depth_levels, automatic_pruning); return 0; } ``` -------------------------------- ### Save and Load UFOMap Files Source: https://context7.com/unknownfreeoccupied/ufomap/llms.txt Demonstrates writing maps to disk with optional LZ4 compression and region filtering, alongside flexible loading using type detection or std::variant. ```cpp #include #include #include int main(int argc, char *argv[]) { ufo::map::OccupancyMapColor map(0.1); // ... fill map with data ... // Save complete map map.write("map.um"); // Save with LZ4 compression map.write("map_compressed.um", true); // Save compressed with reduced depth (faster, smaller file) ufo::map::DepthType save_depth = 3; // Save down to depth 3 only map.write("map_coarse.um", true, save_depth); // Save only a region using bounding volume ufo::geometry::Sphere region(ufo::geometry::Point(0.0, 0.0, 0.0), 5.0); map.write("map_region.um", region); map.write("map_region_compressed.um", region, true); map.write("map_region_coarse.um", region, true, 2); // Load map - determine type first std::string filename = "map.um"; std::string map_type = ufo::map::OccupancyMap::readType(filename); if ("occupancy_map" == map_type) { ufo::map::OccupancyMap loaded_map(filename); // Use loaded_map... } else if ("occupancy_map_color" == map_type) { ufo::map::OccupancyMapColor loaded_color_map(filename); // Use loaded_color_map... } // Using std::variant for flexible loading std::variant variant_map; if ("occupancy_map" == map_type) { variant_map.emplace(filename); } else if ("occupancy_map_color" == map_type) { variant_map.emplace(filename); } // Access via std::visit std::visit([](auto& m) { if constexpr (!std::is_same_v, std::monostate>) { std::cout << "Resolution: " << m.getResolution() << std::endl; } }, variant_map); return 0; } ``` -------------------------------- ### Configure ROS Mapping Server Source: https://context7.com/unknownfreeoccupied/ufomap/llms.txt Launch file configuration for the ufomap_server_node, including map resolution, sensor model parameters, and robot clearing settings. ```xml ``` -------------------------------- ### Configure CMake Build System Source: https://context7.com/unknownfreeoccupied/ufomap/llms.txt CMake configuration for standalone C++ projects and ROS catkin packages using UFOMap. ```cmake # CMakeLists.txt for standalone C++ project cmake_minimum_required(VERSION 3.10) project(my_ufomap_project) set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(ufomap REQUIRED) add_executable(my_app src/main.cpp) target_link_libraries(my_app UFO::Map) ``` ```cmake # CMakeLists.txt for ROS catkin package cmake_minimum_required(VERSION 3.10) project(my_ros_package) find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs tf2_ros ufomap_msgs ufomap_ros ) find_package(ufomap REQUIRED) catkin_package() include_directories(${catkin_INCLUDE_DIRS}) add_executable(mapper_node src/mapper_node.cpp) target_link_libraries(mapper_node ${catkin_LIBRARIES} UFO::Map ) ``` -------------------------------- ### Configure CMakeLists.txt and package.xml for UFOMap Source: https://github.com/unknownfreeoccupied/ufomap/wiki/ROS-Tutorials Required configuration to link UFOMap libraries and declare dependencies in ROS packages. ```cmake find_package(ufomap REQUIRED) target_link_libraries(YOUR_NODE ${catkin_LIBRARIES} UFO::Map ) ``` ```xml ufomap ufomap ``` ```cmake find_package(catkin REQUIRED COMPONENTS roscpp ufomap_msgs ufomap_ros ) ``` ```xml ufomap_msgs ufomap_ros ufomap_msgs ufomap_ros ``` -------------------------------- ### Perform K-Nearest Neighbor Search in C++ Source: https://context7.com/unknownfreeoccupied/ufomap/llms.txt Use beginNNLeaves to iterate over voxels by distance. Supports filtering by occupancy type and restricting searches to specific bounding volumes or spheres. ```cpp #include #include #include #include int main(int argc, char *argv[]) { double resolution = 0.02; // 2 cm voxel size ufo::map::OccupancyMapColor map(resolution); // ... fill map with data ... ufo::math::Vector3 position(0.0, 0.0, 0.0); // Find closest occupied voxel at finest resolution for (auto it = map.beginNNLeaves(position, true, false, false, false, 0), it_end = map.endNNLeaves(); it != it_end; ++it) { std::cout << "Closest occupied: " << it.getX() << ", " << it.getY() << ", " << it.getZ() << " at distance " << it.getDistance() << " m" << std::endl; break; // Only need the first (closest) } // Find 3 closest occupied or unknown voxels int count = 3; for (auto it = map.beginNNLeaves(position, true, false, true, false, 0), it_end = map.endNNLeaves(); it != it_end && count > 0; ++it, --count) { std::cout << "Position: " << it.getX() << ", " << it.getY() << ", " << it.getZ() << std::endl; std::cout << "Distance: " << it.getDistance() << " m" << std::endl; if (it.isOccupied()) { std::cout << "Type: Occupied" << std::endl; } else if (it.isUnknown()) { std::cout << "Type: Unknown" << std::endl; } } // Search within a bounding volume (exclude floor) ufo::math::Vector3 min_pt(-10.0, -10.0, 0.5); // Above floor ufo::math::Vector3 max_pt(10.0, 10.0, 3.0); ufo::geometry::AABB search_region(min_pt, max_pt); for (auto it = map.beginNNLeaves(search_region, position, true, false, true, false, 0), it_end = map.endNNLeaves(); it != it_end; ++it) { std::cout << "Closest obstacle above floor at " << it.getDistance() << " m" << std::endl; break; } // Search within a sphere radius ufo::geometry::Sphere search_sphere(position, 2.0); // 2m radius for (auto it = map.beginNNLeaves(search_sphere, position, false, true, false, false, 0), it_end = map.endNNLeaves(); it != it_end; ++it) { std::cout << "Closest free space within 2m: " << it.getDistance() << " m" << std::endl; break; } return 0; } ``` -------------------------------- ### Save UFOMap to File Source: https://github.com/unknownfreeoccupied/ufomap/wiki/Tutorials Demonstrates saving the entire map or spatially filtered subsets to disk, with options for compression and octree depth limits. ```cpp #include #include #include int main(int argc, char *argv[]) { // 10 cm voxel size. double resolution = 0.1; // Create a colored UFOMap ufo::map::OccupancyMapColor map(resolution); // FILL IN WITH INFORMATION // To save the whole map to file map.write("whole_map.um"); // To save the whole map compressed to file map.write("whole_map_compressed.um", true); // To save the whole map compressed down to the third level of the octree map.write("whole_map_compressed_3_level.um", true, 3); // To save only the part of the map that interesects with a sphere with // 1 meter radius at [0, 0, 0] ufo::geometry::Sphere sphere(ufo::geometry::Point(0.0, 0.0, 0.0), 1.0); map.write("1m_center_map.um", sphere); // and compressed map.write("1m_center_map_compressed.um", sphere, true); // and only down to second level of the octree map.write("1m_center_map_compressed_2_level.um", sphere, true, 2); return 0; } ``` -------------------------------- ### Publish Full UFOMap on New Subscription Source: https://github.com/unknownfreeoccupied/ufomap/wiki/Advanced-ROS-Tutorials This C++ code snippet sets up a ROS node to publish a UFOMap. It includes a callback function `mapConnectCallback` that is triggered when a new node subscribes to the 'map' topic. This callback converts the UFOMap to a ROS message and publishes the entire map to the new subscriber. ```cpp // We will publish colored UFOMaps in this tutorial #include // UFOMap ROS msg #include // To convert between UFO and ROS #include #include #include // Function that publishes the whole map to new nodes that subscribe to the map void mapConnectCallback(ros::SingleSubscriberPublisher const &pub) { ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped); // Convert UFOMap to ROS, with compression and at depth 0. You can of course change these. if (ufomap_msgs::ufoToMsg(map, msg->map, true, 0)) { // Conversion was successful msg->header.stamp = ros::Time::now(); msg->header.frame_id = "map"; // Publish the whole map to the specific node that just subscribed pub.publish(msg); } } ``` ```cpp int main(int argc, char *argv[]) { ros::init(argc, argv, "ufomap_publisher"); ros::NodeHandle nh; // Advance publisher. ros::Publisher map_pub = nh.advertise("map", 10, &mapConnectCallback, ros::SubscriberStatusCallback(), ros::VoidConstPtr()); // Create a colored UFOMap ufo::map::OccupancyMapColor map(0.1); // Enable min/max change detection. This allows us to get an // axis-aligned bounding box of the updated region. map.enableMinMaxChangeDetection(true); // If the UFOMap should be compressed using LZ4. // Good if you are sending the UFOMap between computers. bool compress = false; // Lowest depth to publish. // Higher value means less data to transfer, good in // situation where the data rate is low. // Many nodes do not require detailed maps as well. ufo::map::DepthType pub_depth = 0; ros::Rate rate(10); while (ros::ok()) { // TODO: Integrate data into map // Get axis-aligned bounding box of update part of the map. ufo::geometry::AABB updated_aabb(map.minChange(), map.maxChange()); // This is the UFOMap message object. ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped); // Convert UFOMap to ROS message. Here we add the axis-aligned bounding box, // which tells us what part of the UFOMap we want to publish. if (ufomap_msgs::ufoToMsg(map, msg->map, updated_aabb, compress, pub_depth)) { // Conversion was successful msg->header.stamp = ros::Time::now(); msg->header.frame_id = "map"; map_pub.publish(msg); // This line was missing in the original code, added for completeness // Reset min/max change detection so we get correct axis-aligned bounding box // next time we publish. map.resetMinMaxChangeDetection(); } rate.sleep(); } return 0; } ``` -------------------------------- ### Publishing Updated Map Regions in C++ Source: https://github.com/unknownfreeoccupied/ufomap/wiki/Advanced-ROS-Tutorials Demonstrates enabling change detection, calculating the AABB of updates, and converting the map to a ROS message for publication. ```cpp // We will publish colored UFOMaps in this tutorial #include // UFOMap ROS msg #include // To convert between UFO and ROS #include #include #include int main(int argc, char *argv[]) { ros::init(argc, argv, "ufomap_publisher"); ros::NodeHandle nh; ros::Publisher map_pub = nh.advertise("map", 10); // Create a colored UFOMap ufo::map::OccupancyMapColor map(0.1); // Enable min/max change detection. This allows us to get an // axis-aligned bounding box of the updated region. map.enableMinMaxChangeDetection(true); // If the UFOMap should be compressed using LZ4. // Good if you are sending the UFOMap between computers. bool compress = false; // Lowest depth to publish. // Higher value means less data to transfer, good in // situation where the data rate is low. // Many nodes do not require detailed maps as well. ufo::map::DepthType pub_depth = 0; ros::Rate rate(10); while (ros::ok()) { // TODO: Integrate data into map // Get axis-aligned bounding box of update part of the map. ufo::geometry::AABB updated_aabb(map.minChange(), map.maxChange()); // This is the UFOMap message object. ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped); // Convert UFOMap to ROS message. Here we add the axis-aligned bounding box, // which tells us what part of the UFOMap we want to publish. if (ufomap_msgs::ufoToMsg(map, msg->map, updated_aabb, compress, pub_depth)) { // Conversion was successful msg->header.stamp = ros::Time::now(); msg->header.frame_id = "map"; map_pub.publish(msg); // Reset min/max change detection so we get correct axis-aligned bounding box // next time we publish. map.resetMinMaxChangeDetection(); } rate.sleep(); } return 0; } ``` -------------------------------- ### Publish UFOMap at Different Depths Source: https://github.com/unknownfreeoccupied/ufomap/wiki/Advanced-ROS-Tutorials This C++ code demonstrates publishing a UFOMap at multiple depths. It creates publishers for different topic names based on depth and only publishes when nodes subscribe. It uses `ufomap_msgs::ufoToMsg` for conversion and `ros::advertise` with a callback for dynamic publishing. ```cpp // We will publish colored UFOMaps in this tutorial #include // UFOMap ROS msg #include // To convert between UFO and ROS #include #include #include #include // Function that publishes the whole map to new nodes that subscribe to the map. // d is the depth of the map we should publish. void mapConnectCallback(ros::SingleSubscriberPublisher const &pub, int d) { ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped); // Convert UFOMap to ROS, with compression and at depth d. You can of course change these. if (ufomap_msgs::ufoToMsg(map, msg->map, true, d)) { // Conversion was successful msg->header.stamp = ros::Time::now(); msg->header.frame_id = "map"; // Publish the whole map to the specific node that just subscribed pub.publish(msg); } } int main(int argc, char *argv[]) { ros::init(argc, argv, "ufomap_publisher"); ros::NodeHandle nh; size_t max_depth_to_pub = 4; // Create a number of publishers. std::vector map_pubs(max_depth_to_pub); // Construct publishers. for (int d = 0; d < map_pubs.size(); ++d) { std::string final_topic = d == 0 ? "map" : "map_depth_" + std::to_string(d); // Advance publisher map_pubs[d] = nh.advertise(final_topic, 10, boost::bind(&mapConnectCallback, _1, d), ros::SubscriberStatusCallback(), ros::VoidConstPtr()); } // Create a colored UFOMap ufo::map::OccupancyMapColor map(0.1); // Enable min/max change detection. This allows us to get an // axis-aligned bounding box of the updated region. map.enableMinMaxChangeDetection(true); // If the UFOMap should be compressed using LZ4. // Good if you are sending the UFOMap between computers. bool compress = false; ros::Rate rate(10); while (ros::ok()) { // TODO: Integrate data into map // Get axis-aligned bounding box of update part of the map. ufo::geometry::AABB updated_aabb(map.minChange(), map.maxChange()); for (int d = 0; d < map_pubs.size(); ++d) { if (!map_pubs[d] || (0 >= map_pubs[d].getNumSubscribers() && !map_pubs[d].isLatched())) { // This publisher is broken(?) // There are no subscribers on this topic and the topic is not latched continue; } // This is the UFOMap message object. ufomap_msgs::UFOMapStamped::Ptr msg(new ufomap_msgs::UFOMapStamped); // Convert UFOMap to ROS message. Here we add the axis-aligned bounding box, // which tells us what part of the UFOMap we want to publish. if (ufomap_msgs::ufoToMsg(map, msg->map, updated_aabb, compress, d)) { // Conversion was successful msg->header.stamp = ros::Time::now(); msg->header.frame_id = "map"; map_pubs[d].publish(msg); } } // Reset min/max change detection so we get correct axis-aligned bounding box // next time we publish. map.resetMinMaxChangeDetection(); rate.sleep(); } return 0; } ``` -------------------------------- ### Clone UFOMap Repository Source: https://github.com/unknownfreeoccupied/ufomap/wiki/Setup Clone the UFOMap repository into your Catkin workspace's src directory using either SSH or HTTPS. ```bash cd ~/catkin_ws/src git clone git@github.com:UnknownFreeOccupied/ufomap.git ``` ```bash cd ~/catkin_ws/src git clone https://github.com/UnknownFreeOccupied/ufomap.git ``` -------------------------------- ### Dynamic Reconfigure API Source: https://github.com/unknownfreeoccupied/ufomap/blob/master/ufomap_ros/ufomap_mapping/README.md The ufomap_server provides a dynamic_reconfigure interface to modify various settings in real-time. ```APIDOC ## Dynamic Reconfigure API ufomap_server offers a [dynamic_reconfigure](http://wiki.ros.org/dynamic_reconfigure) interface to change a number of settings on the fly. ``` -------------------------------- ### Integrate Point Cloud into Non-Colored UFOMap Source: https://github.com/unknownfreeoccupied/ufomap/wiki/Tutorials Integrates a point cloud into a standard OccupancyMap. Ensure the point cloud is transformed to the correct frame before integration. Uses discrete insertion for efficiency. ```cpp // For non-colored UFOMap #include // For colored UFOMap #include int main(int argc, char *argv[]) { // 10 cm voxel size. double resolution = 0.1; // Maximum range to integrate, in meters. // Set to negative value to ignore maximum range. double max_range = 7.0; // The depth at which free space should be cleared. // A higher value significantly increases the integration speed // for smaller voxel sizes. ufo::map::DepthType integration_depth = 1; // Will free space at resolution * 2^(integration_depth) voxel size. // Some translation [x, y, z] ufo::math::Vector3 translation(0.0, 0.0, 0.0); // Some rotation (w, x, y, z) ufo::math::Quaternion rotation(1.0, 0.0, 0.0, 0.0); ufo::math::Pose6 frame_origin(translation, rotation); ufo::math::Vector3 sensor_origin(translation); // Create a UFOMap ufo::map::OccupancyMap map(resolution); // Point cloud ufo::map::PointCloud cloud; // Fill point cloud cloud.resize(1000); for (ufo::map::Point3& point : cloud) { point.x() = 1024 * rand () / (RAND_MAX + 1.0); point.y() = 1024 * rand () / (RAND_MAX + 1.0); point.z() = 1024 * rand () / (RAND_MAX + 1.0); } // Specify if the point cloud should be transformed in parallel or not. bool parallel = true; // Transform point cloud to correct frame cloud.transform(frame_origin, parallel); // Integrate point cloud into UFOMap map.insertPointCloudDiscrete(sensor_origin, cloud, max_range, integration_depth); return 0; } ```