diff --git a/CMakeLists.txt b/CMakeLists.txt index a32e159..02e2e3e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,8 +13,8 @@ find_package(tf2_ros REQUIRED) find_package(tf2 REQUIRED) find_package(libpointmatcher_ros REQUIRED) find_package(rosidl_default_generators REQUIRED) -find_package(libpointmatcher REQUIRED) -find_package(norlab_icp_mapper REQUIRED) +find_package(libpointmatcher CONFIG 1.4.2 REQUIRED) +find_package(norlab_icp_mapper CONFIG 2.0.0 REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "srv/SaveTrajectory.srv" diff --git a/README.md b/README.md index c0eea1a..fcbe6bd 100644 --- a/README.md +++ b/README.md @@ -1,55 +1,40 @@ # norlab_icp_mapper_ros -A bridge between norlab_icp_mapper and ROS. +A bridge between [norlab_icp_mapper](https://github.com/norlab-ulaval/norlab_icp_mapper/) and ROS. +Check the [mapper's documentation](https://norlab-icp-mapper.readthedocs.io/en/latest/UsingInRos/) for a detailed guide. ## Node Parameters -| Name | Description | Possible values | Default Value | -| :-------------------------------: | :---------------------------------------------------------------------------------------------------------: | :------------------------------: | :--------------------------------------------------------: | -| odom_frame | Frame used for odometry. | Any string | "odom" | -| robot_frame | Frame centered on the robot. | Any string | "base_link" | -| initial_map_file_name | Path of the file from which the initial map is loaded. | Any file path | "" | -| initial_robot_pose | Transformation matrix in homogeneous coordinates describing the initial pose of the robot in the map frame. | Any matrix of dimension 3 or 4 | "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]" | -| final_map_file_name | Path of the file in which the final map is saved when is_online is false. | Any file path | "map.vtk" | -| final_trajectory_file_name | Path of the file in which the final trajectory is saved when is_online is false. | Any file path | "trajectory.vtk" | -| icp_config | Path of the file containing the libpointmatcher icp config. | Any file path | "" | -| input_filters_config | Path of the file containing the filters applied to the sensor points. | Any file path | "" | -| map_post_filters_config | Path of the file containing the filters applied to the map after the update. | Any file path | "" | -| map_update_condition | Condition for map update. | {"overlap", "delay", "distance"} | "overlap" | -| map_update_overlap | Overlap between sensor and map points under which the map is updated. | [0, 1] | 0.9 | -| map_update_delay | Delay since last map update over which the map is updated (in seconds). | [0, ∞) | 1.0 | -| map_update_distance | Euclidean distance from last map update over which the map is updated (in meters). | [0, ∞) | 0.5 | -| map_publish_rate | Rate at which the map is published (in Hertz). It can be slower depending on the map update rate. | (0, ∞) | 10.0 | -| map_tf_publish_rate | Rate at which the map tf is published (in Hertz). | (0, ∞) | 10.0 | -| max_idle_time | Delay to wait being idle before shutting down ROS when is_online is false (in seconds). | [0, ∞) | 10.0 | -| min_dist_new_point | Distance from current map points under which a new point is not added to the map (in meters). | [0, ∞) | 0.03 | -| sensor_max_range | Maximum reading distance of the laser (in meters). | [0, ∞) | 80.0 | -| prior_dynamic | A priori probability of points being dynamic. | [0, 1] | 0.6 | -| threshold_dynamic | Probability at which a point is considered permanently dynamic. | [0, 1] | 0.9 | -| beam_half_angle | Half angle of the cones formed by the sensor laser beams (in rad). | [0, π/2] | 0.01 | -| epsilon_a | Error proportional to the sensor distance. | [0, ∞) | 0.01 | -| epsilon_d | Fix error on the sensor distance (in meters). | [0, ∞) | 0.01 | -| alpha | Probability of staying static given that the point was static. | [0, 1] | 0.8 | -| beta | Probability of staying dynamic given that the point was dynamic. | [0, 1] | 0.99 | -| is_3D | true when a 3D sensor is used, false when a 2D sensor is used. | {true, false} | true | -| is_online | true when online mapping is wanted, false otherwise. | {true, false} | true | -| compute_prob_dynamic | true when computation of probability of points being dynamic is wanted, false otherwise. | {true, false} | false | -| is_mapping | true when map updates are wanted, false when only localization is wanted. | {true, false} | true | -| save_map_cells_on_hard_drive | true when map cell storage on hard drive is wanted, false when map cell storage in RAM is wanted. | {true, false} | true | -| publish_tfs_between_registrations | When false, the map tf is published only after registration. Otherwise with map_tf_publish_rate. | {true, false} | true | +| Name | Description | Possible values | Default Value | +|:---------------------------------:|:-----------------------------------------------------------------------------------------------------------:|:------------------------------:|:----------------------------------------------------------:| +| odom_frame | Frame used for odometry. | Any string | "odom" | +| robot_frame | Frame centered on the robot. | Any string | "base_link" | +| mapping_config | Path to the file containing the mapping config. | Any file path | "" | +| initial_map_file_name | Path of the file from which the initial map is loaded. | Any file path | "" | +| initial_robot_pose | Transformation matrix in homogeneous coordinates describing the initial pose of the robot in the map frame. | Any matrix of dimension 3 or 4 | "[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]" | +| final_map_file_name | Path of the file in which the final map is saved when is_online is false. | Any file path | "map.vtk" | +| final_trajectory_file_name | Path of the file in which the final trajectory is saved when is_online is false. | Any file path | "trajectory.vtk" | +| map_publish_rate | Rate at which the map is published (in Hertz). It can be slower depending on the map update rate. | (0, ∞) | 10.0 | +| map_tf_publish_rate | Rate at which the map tf is published (in Hertz). | (0, ∞) | 10.0 | +| max_idle_time | Delay to wait being idle before shutting down ROS when is_online is false (in seconds). | [0, ∞) | 10.0 | +| is_mapping | true when map updates are wanted, false when only localization is wanted. | {true, false} | true | +| is_online | true when online mapping is wanted, false otherwise. | {true, false} | true | +| is_3D | true when a 3D sensor is used, false when a 2D sensor is used. | {true, false} | true | +| save_map_cells_on_hard_drive | true when map cell storage on hard drive is wanted, false when map cell storage in RAM is wanted. | {true, false} | true | +| publish_tfs_between_registrations | When false, the map tf is published only after registration. Otherwise with map_tf_publish_rate. | {true, false} | true | ## Node Topics | Name | Description | -| :-------: | :-------------------------------------------------: | +|:---------:|:---------------------------------------------------:| | points_in | Topic from which the input points are retrieved. | | map | Topic in which the map is published. | | icp_odom | Topic in which the corrected odometry is published. | ## Node Services | Name | Description | Parameter Name | Parameter Description | -| :----------------: | :---------------------------: | :------------: | :------------------------------------------------: | +|:------------------:|:-----------------------------:|:--------------:|:--------------------------------------------------:| | save_map | Saves the current map. | filename | Path of the file in which the map is saved. | | save_trajectory | Saves the current trajectory. | filename | Path of the file in which the trajectory is saved. | -| reload_yaml_config | Reload all YAML config files. | | | +| reload_yaml_config | Reload the YAML config file. | | | ## Mapper Node Graph diff --git a/package.xml b/package.xml index 463b3d2..36352b6 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ norlab_icp_mapper_ros - 0.0.1 + 2.0.0 A bridge between norlab_icp_mapper and ROS. Simon-Pierre Deschenes new BSD diff --git a/src/NodeParameters.cpp b/src/NodeParameters.cpp index 87c5f33..a95e256 100644 --- a/src/NodeParameters.cpp +++ b/src/NodeParameters.cpp @@ -13,33 +13,17 @@ void NodeParameters::declareParameters(rclcpp::Node& node) { node.declare_parameter("odom_frame", "odom"); node.declare_parameter("robot_frame", "base_link"); + node.declare_parameter("mapping_config", ""); node.declare_parameter("initial_map_file_name", ""); node.declare_parameter("initial_robot_pose", ""); node.declare_parameter("final_map_file_name", "map.vtk"); node.declare_parameter("final_trajectory_file_name", "trajectory.vtk"); - node.declare_parameter("icp_config", ""); - node.declare_parameter("input_filters_config", ""); - node.declare_parameter("map_post_filters_config", ""); - node.declare_parameter("map_update_condition", "overlap"); - node.declare_parameter("map_update_overlap", 0.9); - node.declare_parameter("map_update_delay", 1); - node.declare_parameter("map_update_distance", 0.5); node.declare_parameter("map_publish_rate", 10); node.declare_parameter("map_tf_publish_rate", 10); node.declare_parameter("max_idle_time", 10); - node.declare_parameter("min_dist_new_point", 0.03); - node.declare_parameter("sensor_max_range", 80); - node.declare_parameter("prior_dynamic", 0.6); - node.declare_parameter("threshold_dynamic", 0.9); - node.declare_parameter("beam_half_angle", 0.01); - node.declare_parameter("epsilon_a", 0.01); - node.declare_parameter("epsilon_d", 0.01); - node.declare_parameter("alpha", 0.8); - node.declare_parameter("beta", 0.99); node.declare_parameter("is_3D", true); - node.declare_parameter("is_online", true); - node.declare_parameter("compute_prob_dynamic", false); node.declare_parameter("is_mapping", true); + node.declare_parameter("is_online", true); node.declare_parameter("save_map_cells_on_hard_drive", true); node.declare_parameter("publish_tfs_between_registrations", true); } @@ -48,33 +32,17 @@ void NodeParameters::retrieveParameters(rclcpp::Node& node) { node.get_parameter("odom_frame", odomFrame); node.get_parameter("robot_frame", robotFrame); + node.get_parameter("mapping_config", mappingConfig); node.get_parameter("initial_map_file_name", initialMapFileName); node.get_parameter("initial_robot_pose", initialRobotPoseString); node.get_parameter("final_map_file_name", finalMapFileName); node.get_parameter("final_trajectory_file_name", finalTrajectoryFileName); - node.get_parameter("icp_config", icpConfig); - node.get_parameter("input_filters_config", inputFiltersConfig); - node.get_parameter("map_post_filters_config", mapPostFiltersConfig); - node.get_parameter("map_update_condition", mapUpdateCondition); - node.get_parameter("map_update_overlap", mapUpdateOverlap); - node.get_parameter("map_update_delay", mapUpdateDelay); - node.get_parameter("map_update_distance", mapUpdateDistance); node.get_parameter("map_publish_rate", mapPublishRate); node.get_parameter("map_tf_publish_rate", mapTfPublishRate); node.get_parameter("max_idle_time", maxIdleTime); - node.get_parameter("min_dist_new_point", minDistNewPoint); - node.get_parameter("sensor_max_range", sensorMaxRange); - node.get_parameter("prior_dynamic", priorDynamic); - node.get_parameter("threshold_dynamic", thresholdDynamic); - node.get_parameter("beam_half_angle", beamHalfAngle); - node.get_parameter("epsilon_a", epsilonA); - node.get_parameter("epsilon_d", epsilonD); - node.get_parameter("alpha", alpha); - node.get_parameter("beta", beta); node.get_parameter("is_3D", is3D); - node.get_parameter("is_online", isOnline); - node.get_parameter("compute_prob_dynamic", computeProbDynamic); node.get_parameter("is_mapping", isMapping); + node.get_parameter("is_online", isOnline); node.get_parameter("save_map_cells_on_hard_drive", saveMapCellsOnHardDrive); node.get_parameter("publish_tfs_between_registrations", publishTfsBetweenRegistrations); } @@ -108,56 +76,16 @@ void NodeParameters::validateParameters() const trajectoryOfs.close(); } - if(!icpConfig.empty()) + if(!mappingConfig.empty()) { - std::ifstream ifs(icpConfig.c_str()); + std::ifstream ifs(mappingConfig.c_str()); if(!ifs.good()) { - throw std::runtime_error("Invalid icp config file: " + icpConfig); + throw std::runtime_error("Invalid mapping config file: " + mappingConfig); } ifs.close(); } - if(!inputFiltersConfig.empty()) - { - std::ifstream ifs(inputFiltersConfig.c_str()); - if(!ifs.good()) - { - throw std::runtime_error("Invalid input filters config file: " + inputFiltersConfig); - } - ifs.close(); - } - - if(!mapPostFiltersConfig.empty()) - { - std::ifstream ifs(mapPostFiltersConfig.c_str()); - if(!ifs.good()) - { - throw std::runtime_error("Invalid map post filters config file: " + mapPostFiltersConfig); - } - ifs.close(); - } - - if(mapUpdateCondition != "overlap" && mapUpdateCondition != "delay" && mapUpdateCondition != "distance") - { - throw std::runtime_error("Invalid map update condition: " + mapUpdateCondition); - } - - if(mapUpdateOverlap < 0 || mapUpdateOverlap > 1) - { - throw std::runtime_error("Invalid map update overlap: " + std::to_string(mapUpdateOverlap)); - } - - if(mapUpdateDelay < 0) - { - throw std::runtime_error("Invalid map update delay: " + std::to_string(mapUpdateDelay)); - } - - if(mapUpdateDistance < 0) - { - throw std::runtime_error("Invalid map update distance: " + std::to_string(mapUpdateDistance)); - } - if(mapPublishRate <= 0) { throw std::runtime_error("Invalid map publish rate: " + std::to_string(mapPublishRate)); @@ -178,51 +106,6 @@ void NodeParameters::validateParameters() const } } - if(minDistNewPoint < 0) - { - throw std::runtime_error("Invalid minimum distance of new point: " + std::to_string(minDistNewPoint)); - } - - if(sensorMaxRange < 0) - { - throw std::runtime_error("Invalid sensor max range: " + std::to_string(sensorMaxRange)); - } - - if(priorDynamic < 0 || priorDynamic > 1) - { - throw std::runtime_error("Invalid prior dynamic: " + std::to_string(priorDynamic)); - } - - if(thresholdDynamic < 0 || thresholdDynamic > 1) - { - throw std::runtime_error("Invalid threshold dynamic: " + std::to_string(thresholdDynamic)); - } - - if(beamHalfAngle < 0 || beamHalfAngle > M_PI_2) - { - throw std::runtime_error("Invalid beam half angle: " + std::to_string(beamHalfAngle)); - } - - if(epsilonA < 0) - { - throw std::runtime_error("Invalid epsilon a: " + std::to_string(epsilonA)); - } - - if(epsilonD < 0) - { - throw std::runtime_error("Invalid epsilon d: " + std::to_string(epsilonD)); - } - - if(alpha < 0 || alpha > 1) - { - throw std::runtime_error("Invalid alpha: " + std::to_string(alpha)); - } - - if(beta < 0 || beta > 1) - { - throw std::runtime_error("Invalid beta: " + std::to_string(beta)); - } - if(!isMapping && initialMapFileName.empty()) { throw std::runtime_error("is mapping is set to false, but initial map file name was not specified."); diff --git a/src/NodeParameters.h b/src/NodeParameters.h index 1482032..745b2ef 100644 --- a/src/NodeParameters.h +++ b/src/NodeParameters.h @@ -18,34 +18,18 @@ class NodeParameters public: std::string odomFrame; std::string robotFrame; + std::string mappingConfig; std::string initialMapFileName; std::string initialRobotPoseString; PM::TransformationParameters initialRobotPose; std::string finalMapFileName; std::string finalTrajectoryFileName; - std::string icpConfig; - std::string inputFiltersConfig; - std::string mapPostFiltersConfig; - std::string mapUpdateCondition; - float mapUpdateOverlap; - float mapUpdateDelay; - float mapUpdateDistance; float mapPublishRate; float mapTfPublishRate; float maxIdleTime; - float minDistNewPoint; - float sensorMaxRange; - float priorDynamic; - float thresholdDynamic; - float beamHalfAngle; - float epsilonA; - float epsilonD; - float alpha; - float beta; - bool is3D; - bool isOnline; - bool computeProbDynamic; + bool is3D; bool isMapping; + bool isOnline; bool saveMapCellsOnHardDrive; bool publishTfsBetweenRegistrations; diff --git a/src/mapper_node.cpp b/src/mapper_node.cpp index 227355a..c6543f9 100644 --- a/src/mapper_node.cpp +++ b/src/mapper_node.cpp @@ -23,14 +23,8 @@ class MapperNode : public rclcpp::Node transformation = PM::get().TransformationRegistrar.create("RigidTransformation"); - mapper = std::unique_ptr(new norlab_icp_mapper::Mapper(params->inputFiltersConfig, params->icpConfig, - params->mapPostFiltersConfig, params->mapUpdateCondition, - params->mapUpdateOverlap, params->mapUpdateDelay, - params->mapUpdateDistance, params->minDistNewPoint, - params->sensorMaxRange, params->priorDynamic, params->thresholdDynamic, - params->beamHalfAngle, params->epsilonA, params->epsilonD, params->alpha, - params->beta, params->is3D, params->isOnline, params->computeProbDynamic, - params->isMapping, params->saveMapCellsOnHardDrive)); + mapper = std::make_unique(params->mappingConfig, params->is3D, params->isOnline, + params->isMapping, params->saveMapCellsOnHardDrive); if(!params->initialMapFileName.empty()) { @@ -335,7 +329,7 @@ class MapperNode : public rclcpp::Node void reloadYamlConfigCallback(const std::shared_ptr req, std::shared_ptr res) { RCLCPP_INFO(this->get_logger(), "Reloading YAML config"); - mapper->loadYamlConfig(params->inputFiltersConfig, params->icpConfig, params->mapPostFiltersConfig); + mapper->loadYamlConfig(params->mappingConfig); } void saveMapCallback(const std::shared_ptr req, std::shared_ptr res)