diff --git a/cfg/AMCL.cfg b/cfg/AMCL.cfg index 823e5ca1..5efbbb7b 100755 --- a/cfg/AMCL.cfg +++ b/cfg/AMCL.cfg @@ -3,12 +3,14 @@ PACKAGE = 'badger_amcl' from math import pi -from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, int_t, double_t, str_t, bool_t +from dynamic_reconfigure.parameter_generator_catkin import (ParameterGenerator, int_t, + double_t, str_t, bool_t) gen = ParameterGenerator() -map_type_enum = gen.enum([ gen.const("OccupancyMap", int_t, 2, "Use a static occupancy map"), - gen.const("OctoMap", int_t, 3, "Use a static OctoMap") +map_type_enum = gen.enum([ + gen.const("OccupancyMap", int_t, 2, "Use a static occupancy map"), + gen.const("OctoMap", int_t, 3, "Use a static OctoMap") ], "Type of static map to use for localization") gen.add("map_type", int_t, 0, "Type of static map to use", 3, edit_method=map_type_enum) @@ -16,108 +18,176 @@ gen.add("map_type", int_t, 0, "Type of static map to use", 3, edit_method=map_ty gen.add("min_particles", int_t, 0, "Minimum allowed number of particles.", 100, 0, 1000) gen.add("max_particles", int_t, 0, "Mamimum allowed number of particles.", 5000, 0, 10000) -gen.add("kld_err", double_t, 0, "Maximum error between the true distribution and the estimated distribution.", .01, 0, 1) -gen.add("kld_z", double_t, 0, "Upper standard normal quantile for (1 - p), where p is the probability that the error on the estimated distrubition will be less than kld_err.", .99, 0, 1) - -gen.add("update_min_d", double_t, 0, "Translational movement required before performing a filter update.", .2, 0, 5) -gen.add("update_min_a", double_t, 0, "Rotational movement required before performing a filter update.", pi/6, 0, 2*pi) - -gen.add("resample_interval", int_t, 0, "Number of filter updates required before resampling.", 2, 0, 20) - -rmt = gen.enum([gen.const("multinomial_const", str_t, "multinomial", "Use multinomial resampling"), gen.const("systematic_const", str_t, "systematic", "Use systematic sampling.")], "Resample Models") -gen.add("resample_model_type", str_t, 0, "Which resample model to use, either multinomial (default), or systematic.", "multinomial", edit_method=rmt) - -gen.add("transform_tolerance", double_t, 0, "Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.", .1, 0, 2) - -gen.add("recovery_alpha_slow", double_t, 0, "Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001.", 0, 0, .5) -gen.add("recovery_alpha_fast", double_t, 0, "Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1.", 0, 0, 1) - -gen.add("uniform_pose_starting_weight_threshold", double_t, 0, "When adding uniform poses, attempt to pick a pose with at least this sample weight according to the sensor model.", 0.0, 0.0, 10.0) -gen.add("uniform_pose_deweight_multiplier", double_t, 0, "When adding uniform poses, deweight uniform_pose_starting_weight_threshold by this multiplier for each try. This guarantees that we will eventually find a pose.", 0.0, 0.0, 1.0) - -gen.add("global_localization_alpha_slow", double_t, 0, "During global localization, override recovery alpha_slow to this value. A good value might be 0.001.", 0, 0, .5) -gen.add("global_localization_alpha_fast", double_t, 0, "During global localization, override recovery alpha_fast to this value. A good value might be 0.1.", 0, 0, 1) - -gen.add("do_beamskip", bool_t, 0, "When true skips scans when a scan doesnt work for a majority of particles", False) -gen.add("beam_skip_distance", double_t, 0, "Distance from a valid map point before scan is considered invalid", 0, 2, 0.5) -gen.add("beam_skip_threshold", double_t, 0, "Ratio of samples for which the scans are valid to consider as valid scan", 0, 1, 0.3) - -gen.add("tf_broadcast", bool_t, 0, "When true (the default), publish results via TF. When false, do not.", True) +gen.add("kld_err", double_t, 0, "Maximum error between the true distribution and the estimated " + "distribution.", .01, 0, 1) +gen.add("kld_z", double_t, 0, "Upper standard normal quantile for (1 - p), where p is the " + "probability that the error on the estimated distrubition will be less than kld_err.", + .99, 0, 1) + +gen.add("update_min_d", double_t, 0, "Translational movement required before performing a filter " + "update.", .2, 0, 5) +gen.add("update_min_a", double_t, 0, "Rotational movement required before performing a filter " + "update.", pi / 6, 0, 2 * pi) + +gen.add("resample_interval", int_t, 0, "Number of filter updates required before resampling.", + 2, 0, 20) + +rmt = gen.enum([ + gen.const("multinomial_const", str_t, "multinomial", "Use multinomial resampling"), + gen.const("systematic_const", str_t, "systematic", "Use systematic sampling.") + ], "Resample Models") + +gen.add("resample_model_type", str_t, 0, "Which resample model to use, either multinomial " + "(default), or systematic.", "multinomial", edit_method=rmt) + +gen.add("transform_tolerance", double_t, 0, "Time with which to post-date the transform that is " + "published, to indicate that this transform is valid into the future.", .1, 0, 2) + +gen.add("recovery_alpha_slow", double_t, 0, "Exponential decay rate for the slow average weight " + "filter, used in deciding when to recover by adding random poses. A good value might be " + "0.001.", 0, 0, .5) +gen.add("recovery_alpha_fast", double_t, 0, "Exponential decay rate for the fast average weight " + "filter, used in deciding when to recover by adding random poses. A good value might be " + "0.1.", 0, 0, 1) + +gen.add("uniform_pose_starting_weight_threshold", double_t, 0, "When adding uniform poses, " + "attempt to pick a pose with at least this sample weight according to the sensor model.", + 0.0, 0.0, 10.0) +gen.add("uniform_pose_deweight_multiplier", double_t, 0, "When adding uniform poses, deweight " + "uniform_pose_starting_weight_threshold by this multiplier for each try. This guarantees " + "that we will eventually find a pose.", 0.0, 0.0, 1.0) + +gen.add("global_localization_alpha_slow", double_t, 0, "During global localization, override " + "recovery alpha_slow to this value. A good value might be 0.001.", 0, 0, .5) +gen.add("global_localization_alpha_fast", double_t, 0, "During global localization, override " + "recovery alpha_fast to this value. A good value might be 0.1.", 0, 0, 1) + +gen.add("do_beamskip", bool_t, 0, "When true skips scans when a scan doesnt work for a majority " + "of particles", False) +gen.add("beam_skip_distance", double_t, 0, "Distance from a valid map point before scan is " + "considered invalid", 0, 2, 0.5) +gen.add("beam_skip_threshold", double_t, 0, "Ratio of samples for which the scans are valid " + "to consider as valid scan", 0, 1, 0.3) + +gen.add("tf_broadcast", bool_t, 0, "When true (the default), publish results via TF. " + "When false, do not.", True) gen.add("tf_reverse", bool_t, 0, "When set to true, reverse published TF.", False) -gen.add("gui_publish_rate", double_t, 0, "Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable.", -1, -1, 100) -gen.add("transform_publish_rate", double_t, 0, "Rate (Hz) at which to publish the transform between map and odom to tf.", 50.0, 0.1, 100.0) -gen.add("save_pose_to_file_rate", double_t, 0, "Maximum rate (Hz) at which to store the last estimated pose and covariance to file. This saved pose will be used on subsequent runs to initialize the filter if the param server does not have the parameters stored. 0.0 to disable.", 0.1, 0.0, 10.0) - -gen.add("use_map_topic", bool_t, 0, "When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.", False) -gen.add("first_map_only", bool_t, 0, "When set to true, AMCL will only use the first map it subscribes to, rather than updating each time a new one is received.", False) -gen.add("wait_for_occupancy_map", bool_t, 0, "When set to true and map type is OctoMap, AMCL will wait for the occupancy map message before creating the OctoMap", True) +gen.add("gui_publish_rate", double_t, 0, "Maximum rate (Hz) at which scans and paths are " + "published for visualization, -1.0 to disable.", -1, -1, 100) +gen.add("transform_publish_rate", double_t, 0, "Rate (Hz) at which to publish the transform " + "between map and odom to tf.", 50.0, 0.1, 100.0) +gen.add("save_pose_to_file_rate", double_t, 0, "Maximum rate (Hz) at which to store the last " + "estimated pose and covariance to file. This saved pose will be used on subsequent runs " + "to initialize the filter if the param server does not have the parameters stored. 0.0 " + "to disable.", 0.1, 0.0, 10.0) + +gen.add("use_map_topic", bool_t, 0, "When set to true, AMCL will subscribe to the map topic " + "rather than making a service call to receive its map.", False) +gen.add("first_map_only", bool_t, 0, "When set to true, AMCL will only use the first map it " + "subscribes to, rather than updating each time a new one is received.", False) +gen.add("wait_for_occupancy_map", bool_t, 0, "When set to true and map type is OctoMap, AMCL will " + "wait for the occupancy map message before creating the OctoMap", True) # Model Parameters -gen.add("laser_min_range", double_t, 0, "Minimum scan range to be considered; -1.0 will cause the scanner's reported minimum range to be used.", -1, -1, 1000) -gen.add("laser_max_range", double_t, 0, "Maximum scan range to be considered; -1.0 will cause the scanner's reported maximum range to be used.", -1, -1, 1000) +gen.add("laser_min_range", double_t, 0, "Minimum scan range to be considered; " + "-1.0 will cause the scanner's reported minimum range to be used.", -1, -1, 1000) +gen.add("laser_max_range", double_t, 0, "Maximum scan range to be considered; " + "-1.0 will cause the scanner's reported maximum range to be used.", -1, -1, 1000) -gen.add("laser_max_beams", int_t, 0, "How many evenly-spaced beams in each scan to be used when updating the filter.", 30, 0, 100) +gen.add("laser_max_beams", int_t, 0, "How many evenly-spaced beams in each scan to be used when " + "updating the filter.", 30, 0, 100) gen.add("laser_z_hit", double_t, 0, "Mixture weight for the z_hit part of the model.", .95, 0, 10) -gen.add("laser_z_short", double_t, 0, "Mixture weight for the z_short part of the model.", .1, 0, 10) +gen.add("laser_z_short", double_t, 0, "Mixture weight for the z_short part of the model.", + .1, 0, 10) gen.add("laser_z_max", double_t, 0, "Mixture weight for the z_max part of the model.", .05, 0, 10) -gen.add("laser_z_rand", double_t, 0, "Mixture weight for the z_rand part of the model.", .05, 0, 10) - -gen.add("laser_gompertz_a", double_t, 0, "Gompertz a coefficient for gompertz sample weight function", 1.0, 0.0, 10.0) -gen.add("laser_gompertz_b", double_t, 0, "Gompertz b coefficient for gompertz sample weight function", 1.0, 0.0, 10.0) -gen.add("laser_gompertz_c", double_t, 0, "Gompertz c coefficient for gompertz sample weight function", 1.0, 0.0, 10.0) -gen.add("laser_gompertz_input_shift", double_t, 0, "Shift input value to gompertz function (after input scaling)", 0.0, -10.0, 10.0) -gen.add("laser_gompertz_input_scale", double_t, 0, "Scale input value to gompertz function (before input shifting)", 1.0, 0.0, 10.0) -gen.add("laser_gompertz_output_shift", double_t, 0, "Shift output value of gompertz function", 0.0, -10.0, 10.0) - -# There is no option for output scale since the output will just be normalized by the particle filter - -gen.add("laser_sigma_hit", double_t, 0, "Standard deviation for Gaussian model used in z_hit part of the model.", .2, 0, 10) -gen.add("laser_lambda_short", double_t, 0, "Exponential decay parameter for z_short part of model.", .1, 0, 10) -gen.add("laser_likelihood_max_dist", double_t, 0, "Maximum distance to do obstacle inflation on map, for use in likelihood_field model.", 2, 0, 20) -gen.add("laser_off_map_factor", double_t, 0, "Factor applied to particle weights out of the map bounds.", 1.0, 0.0, 1.0) -gen.add("laser_non_free_space_factor", double_t, 0, "Factor applied ot particle weights not in free space.", 1.0, 0.0, 1.0) -gen.add("laser_non_free_space_radius", double_t, 0, "Radius used to interpolate laser_non_free_space_factor near non free space.", 0.0, 0.0, 10.0) -gen.add("global_localization_laser_off_map_factor", double_t, 0, "During global localization, Factor applied to particle weights out of the map bounds.", 1.0, 0.0, 1.0) -gen.add("global_localization_laser_non_free_space_factor", double_t, 0, "During global localization, override factor applied ot particle weights not in free space.", 1.0, 0.0, 1.0) +gen.add("laser_z_rand", double_t, 0, "Mixture weight for the z_rand part of the model.", + .05, 0, 10) + +gen.add("laser_gompertz_a", double_t, 0, "Gompertz a coefficient for gompertz sample weight " + "function", 1.0, 0.0, 10.0) +gen.add("laser_gompertz_b", double_t, 0, "Gompertz b coefficient for gompertz sample weight " + "function", 1.0, 0.0, 10.0) +gen.add("laser_gompertz_c", double_t, 0, "Gompertz c coefficient for gompertz sample weight " + "function", 1.0, 0.0, 10.0) +gen.add("laser_gompertz_input_shift", double_t, 0, "Shift input value to gompertz function " + "(after input scaling)", 0.0, -10.0, 10.0) +gen.add("laser_gompertz_input_scale", double_t, 0, "Scale input value to gompertz function " + "(before input shifting)", 1.0, 0.0, 10.0) +gen.add("laser_gompertz_output_shift", double_t, 0, "Shift output value of gompertz " + "function", 0.0, -10.0, 10.0) + +# There is no option for output scale since the output will just be normalized by the +# particle filter + +gen.add("laser_sigma_hit", double_t, 0, "Standard deviation for Gaussian model used in z_hit " + "part of the model.", .2, 0, 10) +gen.add("laser_lambda_short", double_t, 0, "Exponential decay parameter for z_short part " + "of model.", .1, 0, 10) +gen.add("laser_likelihood_max_dist", double_t, 0, "Maximum distance to do obstacle inflation on " + "map, for use in likelihood_field model.", 2, 0, 20) +gen.add("laser_off_map_factor", double_t, 0, "Factor applied to particle weights out of the map " + "bounds.", 1.0, 0.0, 1.0) +gen.add("laser_non_free_space_factor", double_t, 0, "Factor applied ot particle weights not " + "in free space.", 1.0, 0.0, 1.0) +gen.add("laser_non_free_space_radius", double_t, 0, "Radius used to interpolate " + "laser_non_free_space_factor near non free space.", 0.0, 0.0, 10.0) +gen.add("global_localization_laser_off_map_factor", double_t, 0, "During global localization, " + "Factor applied to particle weights out of the map bounds.", 1.0, 0.0, 1.0) +gen.add("global_localization_laser_non_free_space_factor", double_t, 0, "During global " + "localization, override factor applied ot particle weights not in free space.", + 1.0, 0.0, 1.0) lmt = gen.enum([ gen.const("beam_const", str_t, "beam", "Use beam laser model"), gen.const("likelihood_field_const", str_t, "likelihood_field", "Use likelihood_field laser model"), - gen.const("likelihood_field_prob_const", str_t, - "likelihood_field_prob", + gen.const("likelihood_field_prob_const", str_t, "likelihood_field_prob", "Use probablistic likelihood field model"), - gen.const("likelihood_field_gompertz_const", str_t, - "likelihood_field_gompertz", - "Use likelihood field model with gompertz sample weighting"), + gen.const("likelihood_field_gompertz_const", str_t, "likelihood_field_gompertz", + "Use likelihood field model with gompertz sample weighting") ], "Laser Scanner Models") -gen.add("laser_model_type", str_t, 0, "Which laser sensor model to use.", "likelihood_field", edit_method=lmt) +gen.add("laser_model_type", str_t, 0, "Which laser sensor model to use.", "likelihood_field", + edit_method=lmt) # Odometry Model Parameters -odt = gen.enum([gen.const("diff_const", str_t, "diff", "Use diff odom model"), - gen.const("omni_const", str_t, "omni", "Use omni odom model"), - gen.const("diff_corrected_const", str_t, "diff-corrected", "Use corrected diff odom model"), - gen.const("omni_corrected_const", str_t, "omni-corrected", "Use corrected omni odom model"), - gen.const("gaussian_const", str_t, "gaussian", "Use gaussian model")], - "Odom Models") -gen.add("odom_model_type", str_t, 0, "Which model to use, diff, omni, diff-corrected, omni-corrected, or guassian", "diff", edit_method=odt) - -gen.add("odom_alpha1", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion.", .2, 0, 10) -gen.add("odom_alpha2", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the translational component of the robot's motion.", .2, 0, 10) -gen.add("odom_alpha3", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion.", .2, 0, 10) -gen.add("odom_alpha4", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion.", .2, 0, 10) -gen.add("odom_alpha5", double_t, 0, "Specified the expected noise in odometry's sideways translation estimate from the sideways translational component of the robot's motion.", .2, 0, 10) +odt = gen.enum([ + gen.const("diff_const", str_t, "diff", "Use diff odom model"), + gen.const("omni_const", str_t, "omni", "Use omni odom model"), + gen.const("diff_corrected_const", str_t, "diff-corrected", + "Use corrected diff odom model"), + gen.const("omni_corrected_const", str_t, "omni-corrected", + "Use corrected omni odom model"), + gen.const("gaussian_const", str_t, "gaussian", "Use gaussian model") + ], "Odom Models") +gen.add("odom_model_type", str_t, 0, "Which model to use, diff, omni, diff-corrected, " + "omni-corrected, or guassian", "diff", edit_method=odt) + +gen.add("odom_alpha1", double_t, 0, "Specifies the expected noise in odometry's rotation " + "estimate from the rotational component of the robot's motion.", .2, 0, 10) +gen.add("odom_alpha2", double_t, 0, "Specifies the expected noise in odometry's rotation " + "estimate from the translational component of the robot's motion.", .2, 0, 10) +gen.add("odom_alpha3", double_t, 0, "Specifies the expected noise in odometry's translation " + "estimate from the translational component of the robot's motion.", .2, 0, 10) +gen.add("odom_alpha4", double_t, 0, "Specifies the expected noise in odometry's translation " + "estimate from the rotational component of the robot's motion.", .2, 0, 10) +gen.add("odom_alpha5", double_t, 0, "Specified the expected noise in odometry's sideways " + "translation estimate from the sideways translational component of the robot's motion.", + .2, 0, 10) gen.add("odom_frame_id", str_t, 0, "Which frame to use for odometry.", "odom") gen.add("base_frame_id", str_t, 0, "Which frame to use for the robot base.", "base_link") -gen.add("global_frame_id", str_t, 0, "The name of the coordinate frame published by the localization system.", "map") +gen.add("global_frame_id", str_t, 0, "The name of the coordinate frame published by the " + "localization system.", "map") -gen.add("off_object_penalty_factor", double_t, 0, "Penalty factor for points that miss an object on the static map.", 1000.0, 0.0, 100000.0) +gen.add("off_object_penalty_factor", double_t, 0, "Penalty factor for points that miss an object " + "on the static map.", 1000.0, 0.0, 100000.0) gen.add("restore_defaults", bool_t, 0, "Retsore the default configuration", False) -gen.add("save_pose", bool_t, 0, "If the node should save the pose to the param server and to a persistent file.", True) -gen.add("saved_pose_filepath", str_t, 0, "Path of file to store saved poses.", "badger_amcl_saved_pose.yaml") +gen.add("save_pose", bool_t, 0, "If the node should save the pose to the param server and to a " + "persistent file.", True) +gen.add("saved_pose_filepath", str_t, 0, "Path of file to store saved poses.", + "badger_amcl_saved_pose.yaml") exit(gen.generate(PACKAGE, "badger_amcl", "AMCL")) diff --git a/include/amcl/map/map.h b/include/amcl/map/map.h index 9a5478bc..041baca4 100644 --- a/include/amcl/map/map.h +++ b/include/amcl/map/map.h @@ -32,7 +32,7 @@ namespace badger_amcl class Map { public: - Map(double resolution); + explicit Map(double resolution); virtual ~Map() = default; // Convert from map index to world coords @@ -52,6 +52,6 @@ class Map double max_distance_to_object_; std::atomic distances_lut_created_; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_MAP_MAP_H diff --git a/include/amcl/map/occupancy_map.h b/include/amcl/map/occupancy_map.h index ac9a4b64..8aa3ef76 100644 --- a/include/amcl/map/occupancy_map.h +++ b/include/amcl/map/occupancy_map.h @@ -54,7 +54,7 @@ class CachedDistanceOccupancyMap class OccupancyMap : public Map { public: - OccupancyMap(double resolution); + explicit OccupancyMap(double resolution); virtual ~OccupancyMap() = default; // Convert from map index to world coords virtual void convertMapToWorld(const std::vector& map_coords, @@ -105,7 +105,7 @@ class OccupancyMap : public Map { OccupancyMapCellData() = delete; OccupancyMap* occ_map; - OccupancyMapCellData(OccupancyMap* o_map) : occ_map(o_map) {} + explicit OccupancyMapCellData(OccupancyMap* o_map) : occ_map(o_map) {} int i, j; int src_i, src_j; inline bool operator<(const OccupancyMapCellData& b) const @@ -121,6 +121,6 @@ class OccupancyMap : public Map std::vector map_vec_; std::vector world_vec_; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_MAP_OCCUPANCY_MAP_H diff --git a/include/amcl/map/octomap.h b/include/amcl/map/octomap.h index 60a24965..84facc7c 100644 --- a/include/amcl/map/octomap.h +++ b/include/amcl/map/octomap.h @@ -58,7 +58,7 @@ struct Index3 { class OctoMap : public Map { public: - OctoMap(double resolution); + explicit OctoMap(double resolution); OctoMap(double resolution, bool publish_distances_lut); virtual ~OctoMap() = default; virtual void initFromOctree(std::shared_ptr octree, double max_distance_to_object); @@ -119,6 +119,6 @@ class OctoMap : public Map ros::Publisher distances_lut_pub_; bool publish_distances_lut_; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_MAP_OCTOMAP_H diff --git a/include/amcl/node/node.h b/include/amcl/node/node.h index d0021114..1eab0af1 100644 --- a/include/amcl/node/node.h +++ b/include/amcl/node/node.h @@ -226,6 +226,6 @@ class Node std::vector> free_space_indices_; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_NODE_NODE_H diff --git a/include/amcl/node/node_2d.h b/include/amcl/node/node_2d.h index 9ce97f37..ded93722 100644 --- a/include/amcl/node/node_2d.h +++ b/include/amcl/node/node_2d.h @@ -80,7 +80,8 @@ class Node2D : public NodeND void updateScannerPose(const tf2::Transform& scanner_pose, int scanner_index); void initLatestScanData(const sensor_msgs::LaserScanConstPtr& planar_scan, int scanner_index); bool getAngleStats(const sensor_msgs::LaserScanConstPtr& planar_scan, double* angle_min, double* angle_increment); - void updateLatestScanData(const sensor_msgs::LaserScanConstPtr& planar_scan, double angle_min, double angle_increment); + void updateLatestScanData(const sensor_msgs::LaserScanConstPtr& planar_scan, + double angle_min, double angle_increment); bool updatePf(const sensor_msgs::LaserScanConstPtr& planar_scan, int scanner_index, bool* resampled); bool resamplePf(const sensor_msgs::LaserScanConstPtr& planar_scan); @@ -134,6 +135,6 @@ class Node2D : public NodeND bool global_localization_active_; }; -} // namespace amcl +} // namespace badger_amcl -#endif // AMCL_NODE_NODE_2D_H +#endif // AMCL_NODE_NODE_2D_H diff --git a/include/amcl/node/node_3d.h b/include/amcl/node/node_3d.h index d20ce152..e8fe0d35 100644 --- a/include/amcl/node/node_3d.h +++ b/include/amcl/node/node_3d.h @@ -142,6 +142,6 @@ class Node3D : public NodeND bool global_localization_active_; }; -} // namespace amcl +} // namespace badger_amcl -#endif // AMCL_NODE_NODE_3D_H +#endif // AMCL_NODE_NODE_3D_H diff --git a/include/amcl/node/node_nd.h b/include/amcl/node/node_nd.h index 58124c56..cf5eb315 100644 --- a/include/amcl/node/node_nd.h +++ b/include/amcl/node/node_nd.h @@ -36,6 +36,6 @@ class NodeND virtual double scorePose(const Eigen::Vector3d& p) = 0; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_NODE_NODE_ND_H diff --git a/include/amcl/pf/particle_filter.h b/include/amcl/pf/particle_filter.h index 04c52145..ee014c71 100644 --- a/include/amcl/pf/particle_filter.h +++ b/include/amcl/pf/particle_filter.h @@ -45,7 +45,6 @@ struct PFSample // Weight for this pose double weight; - }; // Information for a cluster of samples @@ -63,7 +62,6 @@ struct PFCluster // Workspace double m[4], c[2][2]; - }; // Information for a set of samples @@ -183,6 +181,6 @@ class ParticleFilter bool converged_; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_PF_PARTICLE_FILTER_H diff --git a/include/amcl/pf/pdf_gaussian.h b/include/amcl/pf/pdf_gaussian.h index 87341ea7..51c48f8c 100644 --- a/include/amcl/pf/pdf_gaussian.h +++ b/include/amcl/pf/pdf_gaussian.h @@ -59,6 +59,6 @@ class PDFGaussian Eigen::Vector3d cd_; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_PF_PDF_GAUSSIAN_H diff --git a/include/amcl/pf/pf_kdtree.h b/include/amcl/pf/pf_kdtree.h index 168e5c53..33d96574 100644 --- a/include/amcl/pf/pf_kdtree.h +++ b/include/amcl/pf/pf_kdtree.h @@ -37,7 +37,6 @@ struct PFKDTreeNode double value; int cluster; struct PFKDTreeNode* children[2]; - }; class PFKDTree @@ -64,6 +63,6 @@ class PFKDTree int leaf_count_; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_PF_PF_KDTREE_H diff --git a/include/amcl/sensors/odom.h b/include/amcl/sensors/odom.h index bacb9957..d1874b4e 100644 --- a/include/amcl/sensors/odom.h +++ b/include/amcl/sensors/odom.h @@ -80,6 +80,6 @@ class Odom : public Sensor double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_SENSORS_ODOM_H diff --git a/include/amcl/sensors/planar_scanner.h b/include/amcl/sensors/planar_scanner.h index 257a9701..e12c8db7 100644 --- a/include/amcl/sensors/planar_scanner.h +++ b/include/amcl/sensors/planar_scanner.h @@ -167,6 +167,6 @@ class PlanarScanner : public Sensor std::vector world_vec_; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_SENSORS_PLANAR_SCANNER_H diff --git a/include/amcl/sensors/point_cloud_scanner.h b/include/amcl/sensors/point_cloud_scanner.h index 0aad25b0..21fdfa7b 100644 --- a/include/amcl/sensors/point_cloud_scanner.h +++ b/include/amcl/sensors/point_cloud_scanner.h @@ -21,6 +21,8 @@ #define AMCL_SENSORS_POINT_CLOUD_SCANNER_H #include +#include +#include #include #include @@ -119,6 +121,6 @@ class PointCloudScanner : public Sensor std::vector world_vec_; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_SENSORS_POINT_CLOUD_SCANNER_H diff --git a/include/amcl/sensors/sensor.h b/include/amcl/sensors/sensor.h index 36b3c7dd..37fb679f 100644 --- a/include/amcl/sensors/sensor.h +++ b/include/amcl/sensors/sensor.h @@ -35,12 +35,12 @@ class Sensor virtual ~Sensor() = default; virtual bool updateAction(std::shared_ptr pf, - std::shared_ptr data) { return false; }; + std::shared_ptr data) { return false; } // Update the filter based on the sensor model. Returns true if the // filter has been updated. virtual bool updateSensor(std::shared_ptr pf, - std::shared_ptr data) { return false; }; + std::shared_ptr data) { return false; } }; // Base class for all AMCL sensor measurements @@ -51,6 +51,6 @@ class SensorData virtual ~SensorData() = default; }; -} // namespace amcl +} // namespace badger_amcl #endif // AMCL_SENSORS_SENSOR_H diff --git a/src/amcl/map/map.cpp b/src/amcl/map/map.cpp index 386e9dd2..7384c569 100644 --- a/src/amcl/map/map.cpp +++ b/src/amcl/map/map.cpp @@ -40,4 +40,4 @@ bool Map::isDistancesLUTCreated() return distances_lut_created_; } -} // namespace amcl +} // namespace badger_amcl diff --git a/src/amcl/map/occupancy_map.cpp b/src/amcl/map/occupancy_map.cpp index a7b63bb1..cae015bb 100644 --- a/src/amcl/map/occupancy_map.cpp +++ b/src/amcl/map/occupancy_map.cpp @@ -138,7 +138,7 @@ CachedDistanceOccupancyMap::CachedDistanceOccupancyMap(double resolution, double void OccupancyMap::updateDistancesLUT(double max_distance_to_object) { max_distance_to_object_ = max_distance_to_object; - if(max_distance_to_object_ == 0.0) + if (max_distance_to_object_ == 0.0) { ROS_DEBUG("Failed to update distances lut, max distance to object is 0"); return; @@ -215,7 +215,7 @@ void OccupancyMap::updateNode(int i, int j, const OccupancyMapCellData& current_ std::vector& marked) { unsigned int index = computeCellIndex(i, j); - if (not marked.at(index)) + if (!marked.at(index)) { marked.at(index) = enqueue(i, j, current_cell.src_i, current_cell.src_j, q); } @@ -276,7 +276,7 @@ double OccupancyMap::calcRange(double ox, double oy, double oa, double max_range x1 = map_vec_[0]; y1 = map_vec_[1]; - if (x0 == x1 and y0 == y1) + if (x0 == x1 && y0 == y1) return max_range; if (std::abs(y1 - y0) > std::abs(x1 - x0)) @@ -363,4 +363,4 @@ double OccupancyMap::calcRange(double ox, double oy, double oa, double max_range return max_range; } -} //namespace amcl +} // namespace badger_amcl diff --git a/src/amcl/map/octomap.cpp b/src/amcl/map/octomap.cpp index df23ad49..e60434c6 100644 --- a/src/amcl/map/octomap.cpp +++ b/src/amcl/map/octomap.cpp @@ -111,13 +111,13 @@ void OctoMap::convertWorldToMap(const std::vector& world_coords, std::ve // returns true if all coordinates are within the represented map bool OctoMap::isPoseValid(const int i, const int j) { - return (i <= cropped_max_cells_[0] and i >= cropped_min_cells_[0] - and j <= cropped_max_cells_[1] and j >= cropped_min_cells_[1]); + return (i <= cropped_max_cells_[0] && i >= cropped_min_cells_[0] + && j <= cropped_max_cells_[1] && j >= cropped_min_cells_[1]); } bool OctoMap::isVoxelValid(const int i, const int j, const int k) { - return isPoseValid(i, j) and k <= cropped_max_cells_[2] and k >= cropped_min_cells_[2]; + return isPoseValid(i, j) && k <= cropped_max_cells_[2] && k >= cropped_min_cells_[2]; } double OctoMap::getMaxDistanceToObject() @@ -229,7 +229,7 @@ void OctoMap::iterateObstacleCells(CellDataQueue& q) i = map_coords[0]; j = map_coords[1]; k = map_coords[2]; - if(!isVoxelValid(i, j, k)) + if (!isVoxelValid(i, j, k)) continue; setDistanceToObject(i, j, k, 0.0); source.v[0] = i; @@ -239,7 +239,7 @@ void OctoMap::iterateObstacleCells(CellDataQueue& q) } } - while(!ordering_queue.empty()) + while (!ordering_queue.empty()) { source = ordering_queue.top(); ordering_queue.pop(); @@ -319,7 +319,7 @@ void OctoMap::setDistanceToObject(int i, int j, int k, double d) int k_shifted = k - cropped_min_cells_[2]; uint32_t pose_index = makePoseIndex(i_shifted, j_shifted); uint32_t distances_lut_start_index = pose_indices_[pose_index]; - if(distances_lut_start_index == 0) + if (distances_lut_start_index == 0) { distances_lut_start_index = distance_ratios_.size(); pose_indices_[pose_index] = distances_lut_start_index; @@ -337,7 +337,7 @@ double OctoMap::getDistanceToObject(int i, int j, int k) { // Checking if distances lut is created first will prevent checking validity while creating distances lut. // The distances lut container is assumed to not send invalid coordinates and checking every time is inefficient. - if(distances_lut_created_ and !isVoxelValid(i, j, k)) + if (distances_lut_created_ && !isVoxelValid(i, j, k)) return max_distance_to_object_; int i_shifted = i - cropped_min_cells_[0]; int j_shifted = j - cropped_min_cells_[1]; @@ -365,14 +365,14 @@ void OctoMap::publishDistancesLUT() std::vector world_coords(3); int count = 0; int max_count = 1000000; - for(int i = cropped_min_cells_[0]; i <= cropped_max_cells_[0]; i++) + for (int i = cropped_min_cells_[0]; i <= cropped_max_cells_[0]; i++) { - for(int j = cropped_min_cells_[1]; j <= cropped_max_cells_[1]; j++) + for (int j = cropped_min_cells_[1]; j <= cropped_max_cells_[1]; j++) { - for(int k = cropped_min_cells_[2]; k <= cropped_max_cells_[2]; k++) + for (int k = cropped_min_cells_[2]; k <= cropped_max_cells_[2]; k++) { double d = getDistanceToObject(i, j, k); - if(count < max_count and d < max_distance_to_object_) + if (count < max_count && d < max_distance_to_object_) { map_coords[0] = i; map_coords[1] = j; @@ -394,4 +394,4 @@ void OctoMap::publishDistancesLUT() ROS_INFO_STREAM("Publishing cloud of size: " << cloud->points.size()); } -} // namespace amcl +} // namespace badger_amcl diff --git a/src/amcl/node/node.cpp b/src/amcl/node/node.cpp index 9938548d..31482767 100644 --- a/src/amcl/node/node.cpp +++ b/src/amcl/node/node.cpp @@ -150,18 +150,18 @@ Node::Node() default_cov_vals_[COVARIANCE_AA] = (M_PI / 12.0) * (M_PI / 12.0); loadPose(); - if(odom_integrator_enabled_); + if (odom_integrator_enabled_) { std::string odom_integrator_topic = "odom"; odom_integrator_sub_ = nh_.subscribe(odom_integrator_topic, 20, &Node::integrateOdom, this); absolute_motion_pub_ = nh_.advertise("amcl_absolute_motion", 20, false); } - if(map_type_ == 2) + if (map_type_ == 2) { node_ = std::make_shared(this, configuration_mutex_); } - if(map_type_ == 3) + if (map_type_ == 3) { node_ = std::make_shared(this, configuration_mutex_); } @@ -305,11 +305,11 @@ bool Node::updatePf(const ros::Time& t, std::vector& scanners_update, int if (getOdomPose(t, &pose)) { Eigen::Vector3d delta; - if(odom_init_) + if (odom_init_) { computeDelta(pose, &delta); setScannersUpdateFlags(delta, scanners_update, force_update); - if(scanners_update.at(scanner_index)) + if (scanners_update.at(scanner_index)) { updateOdom(pose, delta); } @@ -472,7 +472,7 @@ bool Node::loadPoseFromFile() xx = config["pose"]["covariance"][COVARIANCE_XX].as(); yy = config["pose"]["covariance"][COVARIANCE_YY].as(); aa = config["pose"]["covariance"][COVARIANCE_AA].as(); - if(config["header"]["on_exit"]) + if (config["header"]["on_exit"]) on_exit = config["header"]["on_exit"].as(); else // assume pose was saved on exit if flag does not exist @@ -483,10 +483,10 @@ bool Node::loadPoseFromFile() ROS_WARN_STREAM("Exception while loading pose from file. Failed to parse saved pose. " << e.what()); return false; } - if (std::isnan(pose_x) or std::isnan(pose_y) - or std::isnan(orientation_x) or std::isnan(orientation_y) - or std::isnan(orientation_z) or std::isnan(orientation_w) - or std::isnan(xx) or std::isnan(yy) or std::isnan(aa)) + if (std::isnan(pose_x) || std::isnan(pose_y) + || std::isnan(orientation_x) || std::isnan(orientation_y) + || std::isnan(orientation_z) || std::isnan(orientation_w) + || std::isnan(xx) || std::isnan(yy) || std::isnan(aa)) { ROS_WARN("Failed to parse saved YAML pose. NAN value read from file."); return false; @@ -500,7 +500,7 @@ bool Node::loadPoseFromFile() init_pose_[0] = pose_x; init_pose_[1] = pose_y; init_pose_[2] = yaw; - if(on_exit) + if (on_exit) { init_cov_[0] = xx; init_cov_[1] = yy; @@ -519,7 +519,7 @@ YAML::Node Node::loadYamlFromFile() { YAML::Node node = YAML::LoadFile(saved_pose_filepath_); std::string key = node.begin()->first.as(); - if (key.compare("header") == 0 or key.compare("pose") == 0) + if (key.compare("header") == 0 || key.compare("pose") == 0) { ROS_DEBUG("YAML c++ style, returning node"); return node; @@ -633,13 +633,13 @@ void Node::savePoseToFile(const geometry_msgs::PoseWithCovarianceStamped& latest void Node::initFromNewMap(std::shared_ptr new_map, bool use_initial_pose) { map_ = new_map; - if(not use_initial_pose) + if (!use_initial_pose) return; // Create the particle filter uniform_pose_generator_fn_ = std::bind(&Node::uniformPoseGenerator, this); pf_ = std::make_shared(min_particles_, max_particles_, alpha_slow_, alpha_fast_, - global_localization_convergence_threshold_,uniform_pose_generator_fn_); + global_localization_convergence_threshold_, uniform_pose_generator_fn_); pf_->setPopulationSizeParameters(pf_err_, pf_z_); pf_->setResampleModel(resample_model_type_); @@ -648,9 +648,9 @@ void Node::initFromNewMap(std::shared_ptr new_map, bool use_initial_pose) pf_init_pose_mean[1] = init_pose_[1]; pf_init_pose_mean[2] = init_pose_[2]; Eigen::Matrix3d pf_init_pose_cov; - for(int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { - for(int j = 0; j < 3; j++) + for (int j = 0; j < 3; j++) { pf_init_pose_cov(i, j) = 0.0; } @@ -886,7 +886,7 @@ void Node::publishTransform(const ros::TimerEvent& event) bool Node::getLatestTf(tf2::Transform* latest_tf) { std::lock_guard tfl(tf_mutex_); - if(latest_tf_valid_) + if (latest_tf_valid_) { *latest_tf = latest_tf_; return true; @@ -919,7 +919,7 @@ void Node::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedCon std::lock_guard cfl(configuration_mutex_); geometry_msgs::PoseWithCovarianceStamped msg(*msg_ptr); resolveFrameId(msg); - if(checkInitialPose(msg)) + if (checkInitialPose(msg)) { std::vector cov_vals(36, 0.0); setCovarianceVals(msg, &cov_vals); @@ -998,8 +998,10 @@ void Node::setScannersUpdateFlags(const Eigen::Vector3d& delta, std::vector>( new message_filters::Subscriber(nh_, scan_topic_, 1)); scan_filter_ = std::unique_ptr>( - new tf2_ros::MessageFilter(*scan_sub_.get(), tf_buffer_, node_->getOdomFrameId(), 1, nh_)); + new tf2_ros::MessageFilter(*scan_sub_.get(), tf_buffer_, + node_->getOdomFrameId(), 1, nh_)); scan_filter_->registerCallback(std::bind(&Node2D::scanReceived, this, std::placeholders::_1)); // 15s timer to warn on lack of receipt of planar scans, #5209 @@ -254,7 +255,7 @@ void Node2D::initFromNewMap() ROS_INFO("Done initializing likelihood field model."); } scanner_.setMapFactors(off_map_factor_, non_free_space_factor_, non_free_space_radius_); - node_->initFromNewMap(map_, not first_map_received_); + node_->initFromNewMap(map_, !first_map_received_); pf_ = node_->getPfPtr(); } @@ -340,22 +341,22 @@ void Node2D::updateFreeSpaceIndices() void Node2D::scanReceived(const sensor_msgs::LaserScanConstPtr& planar_scan) { latest_scan_received_ts_ = ros::Time::now(); - if(!isMapInitialized()) + if (!isMapInitialized()) return; - if(!global_localization_active_) + if (!global_localization_active_) deactivateGlobalLocalizationParams(); ros::Time stamp = planar_scan->header.stamp; int scanner_index = getFrameToScannerIndex(planar_scan->header.frame_id); - if(scanner_index >= 0) + if (scanner_index >= 0) { bool force_publication = false, resampled = false, success; success = updateNodePf(stamp, scanner_index, &force_publication); - if(scanners_update_.at(scanner_index)) - success = success and updateScanner(planar_scan, scanner_index, &resampled); - if(force_publication or resampled) - success = success and resamplePose(stamp); + if (scanners_update_.at(scanner_index)) + success = success && updateScanner(planar_scan, scanner_index, &resampled); + if (force_publication || resampled) + success = success && resamplePose(stamp); } } @@ -370,18 +371,18 @@ bool Node2D::updateScanner(const sensor_msgs::LaserScanConstPtr& planar_scan, initLatestScanData(planar_scan, scanner_index); double angle_min, angle_increment; bool success = true; - if(getAngleStats(planar_scan, &angle_min, &angle_increment)) + if (getAngleStats(planar_scan, &angle_min, &angle_increment)) { ROS_DEBUG("Planar scanner %d angles in base frame: min: %.3f inc: %.3f", scanner_index, angle_min, angle_increment); updateLatestScanData(planar_scan, angle_min, angle_increment); scanners_[scanner_index]->updateSensor(pf_, std::dynamic_pointer_cast(latest_scan_data_)); scanners_update_.at(scanner_index) = false; - if(!(++resample_count_ % resample_interval_)) + if (!(++resample_count_ % resample_interval_)) { resampleParticles(); *resampled = true; } - if(!force_update_) + if (!force_update_) node_->publishParticleCloud(); } else @@ -403,7 +404,7 @@ bool Node2D::isMapInitialized() ROS_DEBUG("PF is null"); return false; } - if (not map_->isDistancesLUTCreated()) + if (!map_->isDistancesLUTCreated()) { ROS_DEBUG("Distances not yet created"); return false; @@ -433,7 +434,7 @@ int Node2D::getFrameToScannerIndex(const std::string& scanner_frame_id) { tf2::Transform scanner_pose; initFrameToScanner(scanner_frame_id, &scanner_pose, &scanner_index); - if(scanner_index >= 0) + if (scanner_index >= 0) { frame_to_scanner_[scanner_frame_id] = scanner_index; updateScannerPose(scanner_pose, scanner_index); @@ -494,7 +495,8 @@ void Node2D::initLatestScanData(const sensor_msgs::LaserScanConstPtr& planar_sca latest_scan_data_->range_count_ = planar_scan->ranges.size(); } -bool Node2D::getAngleStats(const sensor_msgs::LaserScanConstPtr& planar_scan, double* angle_min, double* angle_increment) +bool Node2D::getAngleStats(const sensor_msgs::LaserScanConstPtr& planar_scan, + double* angle_min, double* angle_increment) { // To account for the planar scanners that are mounted upside-down, we determine the // min, max, and increment angles of the scanner in the base frame. @@ -509,7 +511,8 @@ bool Node2D::getAngleStats(const sensor_msgs::LaserScanConstPtr& planar_scan, do bool success = true; try { - geometry_msgs::TransformStamped t = tf_buffer_.lookupTransform(node_->getBaseFrameId(), planar_scan->header.frame_id, + geometry_msgs::TransformStamped t = tf_buffer_.lookupTransform(node_->getBaseFrameId(), + planar_scan->header.frame_id, planar_scan->header.stamp, ros::Duration(5.0)); tf2::doTransform(min_q_msg, min_q_msg, t); tf2::doTransform(inc_q_msg, inc_q_msg, t); @@ -519,7 +522,7 @@ bool Node2D::getAngleStats(const sensor_msgs::LaserScanConstPtr& planar_scan, do ROS_WARN_STREAM("Unable to transform min/max planar scanner angles into base frame: " << e.what()); success = false; } - if(success) + if (success) { tf2::fromMsg(min_q_msg, min_q); tf2::fromMsg(inc_q_msg, inc_q); @@ -575,7 +578,7 @@ bool Node2D::resamplePose(const ros::Time& stamp) Eigen::Vector3d max_pose; getMaxWeightPose(&max_weight, &max_pose); bool success = true; - if(max_weight > 0.0) + if (max_weight > 0.0) success = updatePose(max_pose, stamp); else { @@ -643,7 +646,7 @@ bool Node2D::updatePose(const Eigen::Vector3d& max_pose, const ros::Time& stamp) success = false; } - if(success) + if (success) { tf2::Transform odom_to_map_transform; tf2::fromMsg(odom_to_map_msg, odom_to_map_transform); @@ -676,4 +679,4 @@ void Node2D::globalLocalizationCallback() global_localization_active_ = true; } -} // namespace amcl +} // namespace badger_amcl diff --git a/src/amcl/node/node_3d.cpp b/src/amcl/node/node_3d.cpp index 037725aa..c1c25d6c 100644 --- a/src/amcl/node/node_3d.cpp +++ b/src/amcl/node/node_3d.cpp @@ -178,7 +178,7 @@ void Node3D::reconfigure(AMCLConfig& config) void Node3D::occupancyMapMsgReceived(const nav_msgs::OccupancyGridConstPtr& msg) { std::lock_guard cfl(configuration_mutex_); - if(not wait_for_occupancy_map_ or (first_map_only_ && first_occupancy_map_received_)) + if (!wait_for_occupancy_map_ || (first_map_only_ && first_occupancy_map_received_)) return; first_occupancy_map_received_ = true; @@ -189,7 +189,7 @@ void Node3D::occupancyMapMsgReceived(const nav_msgs::OccupancyGridConstPtr& msg) occupancy_map_min_ = {0.0, 0.0}; occupancy_map_max_ = {size_vec[0] * resolution, size_vec[1] * resolution}; occupancy_bounds_received_ = true; - if(first_octomap_received_) + if (first_octomap_received_) { map_->setMapBounds(occupancy_map_min_, occupancy_map_max_); updateFreeSpaceIndices(); @@ -239,16 +239,16 @@ void Node3D::initFromNewMap() ROS_INFO("Done initializing likelihood (gompertz) field model."); } scanner_.setMapFactors(off_map_factor_, non_free_space_factor_, non_free_space_radius_); - node_->initFromNewMap(map_, not first_octomap_received_); + node_->initFromNewMap(map_, !first_octomap_received_); pf_ = node_->getPfPtr(); // if we are using both maps as bounds // and the occupancy map has already arrived - if (wait_for_occupancy_map_ and occupancy_bounds_received_) + if (wait_for_occupancy_map_ && occupancy_bounds_received_) { map_->setMapBounds(occupancy_map_min_, occupancy_map_max_); updateFreeSpaceIndices(); } - else if(!wait_for_occupancy_map_) + else if (!wait_for_occupancy_map_) { map_->updateDistancesLUT(); updateFreeSpaceIndices(); @@ -305,7 +305,7 @@ double Node3D::scorePose(const Eigen::Vector3d& p) void Node3D::updateFreeSpaceIndices() { - // TODO: update free space indices with initialized 2D map + // TODO(anyone): update free space indices with initialized 2D map // Index of free space // Must be calculated after the distances lut is set std::vector> fsi; @@ -320,7 +320,7 @@ void Node3D::updateFreeSpaceIndices() void Node3D::scanReceived(const sensor_msgs::PointCloud2ConstPtr& point_cloud_scan) { latest_scan_received_ts_ = ros::Time::now(); - if(!isMapInitialized()) + if (!isMapInitialized()) return; if (!global_localization_active_) @@ -328,14 +328,14 @@ void Node3D::scanReceived(const sensor_msgs::PointCloud2ConstPtr& point_cloud_sc ros::Time stamp = point_cloud_scan->header.stamp; int scanner_index = getFrameToScannerIndex(point_cloud_scan->header.frame_id); - if(scanner_index >= 0) + if (scanner_index >= 0) { bool force_publication = false, resampled = false, success; success = updateNodePf(stamp, scanner_index, &force_publication); - if(scanners_update_.at(scanner_index)) + if (scanners_update_.at(scanner_index)) updateScanner(point_cloud_scan, scanner_index, &resampled); - if(force_publication or resampled) - success = success and resamplePose(stamp); + if (force_publication || resampled) + success = success && resamplePose(stamp); } } @@ -355,12 +355,12 @@ void Node3D::updateScanner(const sensor_msgs::PointCloud2ConstPtr& point_cloud_s scanners_[scanner_index]->updateSensor(pf_, std::dynamic_pointer_cast( latest_scan_data_)); scanners_update_.at(scanner_index) = false; - if(!(++resample_count_ % resample_interval_)) + if (!(++resample_count_ % resample_interval_)) { resampleParticles(); *resampled = true; } - if(!force_update_) + if (!force_update_) node_->publishParticleCloud(); } @@ -376,7 +376,7 @@ bool Node3D::isMapInitialized() ROS_DEBUG("PF is null"); return false; } - if (not map_->isDistancesLUTCreated()) + if (!map_->isDistancesLUTCreated()) { ROS_DEBUG("Distances not yet created"); return false; @@ -404,10 +404,10 @@ int Node3D::getFrameToScannerIndex(const std::string& scanner_frame_id) if (frame_to_scanner_.find(scanner_frame_id) == frame_to_scanner_.end()) { scanner_index = initFrameToScanner(); - if(scanner_index >= 0) + if (scanner_index >= 0) { geometry_msgs::Transform scanner_to_footprint_tf; - if(getFootprintToFrameTransform(scanner_frame_id, &scanner_to_footprint_tf)) + if (getFootprintToFrameTransform(scanner_frame_id, &scanner_to_footprint_tf)) { frame_to_scanner_[scanner_frame_id] = scanner_index; scanners_[scanner_index]->setPointCloudScannerToFootprintTF(scanner_to_footprint_tf); @@ -498,7 +498,7 @@ bool Node3D::resamplePose(const ros::Time& stamp) Eigen::Vector3d max_pose; getMaxWeightPose(&max_weight, &max_pose); bool success = true; - if(max_weight > 0.0) + if (max_weight > 0.0) success = updatePose(max_pose, stamp); else { @@ -566,7 +566,7 @@ bool Node3D::updatePose(const Eigen::Vector3d& max_pose, const ros::Time& stamp) success = false; } - if(success) + if (success) { tf2::Transform odom_to_map_transform; tf2::fromMsg(odom_to_map_msg, odom_to_map_transform); @@ -599,4 +599,4 @@ void Node3D::globalLocalizationCallback() global_localization_active_ = true; } -} // namespace amcl +} // namespace badger_amcl diff --git a/src/amcl/pf/particle_filter.cpp b/src/amcl/pf/particle_filter.cpp index 83bd8bb5..9416bf6f 100644 --- a/src/amcl/pf/particle_filter.cpp +++ b/src/amcl/pf/particle_filter.cpp @@ -367,7 +367,7 @@ double ParticleFilter::resampleMultinomial(double w_diff) set_b = sets_[(current_set_ + 1) % 2]; // Build up cumulative probability table for resampling. - // TODO: Replace this with a more efficient procedure + // TODO(anyone): Replace this with a more efficient procedure // (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html) c = std::vector(set_a->sample_count + 1); c[0] = 0.0; @@ -522,7 +522,7 @@ void ParticleFilter::computeClusterStatsForSet(std::shared_ptr set) { PFSample* sample = &(set->samples[i]); int cidx = getClusterIndexOfSampleInSet(set, sample); - if(cidx >= 0) + if (cidx >= 0) { addSampleStatsToCluster(sample, &(set->clusters[cidx])); addSampleStatsToSet(sample, &weight, m, c); @@ -672,4 +672,4 @@ bool ParticleFilter::isConverged() return converged_; } -} // namespace amcl +} // namespace badger_amcl diff --git a/src/amcl/pf/pdf_gaussian.cpp b/src/amcl/pf/pdf_gaussian.cpp index 3ed4feda..de50c443 100644 --- a/src/amcl/pf/pdf_gaussian.cpp +++ b/src/amcl/pf/pdf_gaussian.cpp @@ -126,4 +126,4 @@ void PDFGaussian::decompose(const Eigen::Matrix3d& m, Eigen::Matrix3d* r, Eigen: } } -} // namespace amcl +} // namespace badger_amcl diff --git a/src/amcl/pf/pf_kdtree.cpp b/src/amcl/pf/pf_kdtree.cpp index 9f90b79a..9a432028 100644 --- a/src/amcl/pf/pf_kdtree.cpp +++ b/src/amcl/pf/pf_kdtree.cpp @@ -195,7 +195,7 @@ void PFKDTree::clusterNode(PFKDTreeNode* node) bool PFKDTree::equals(int key_a[], int key_b[]) { - return (key_a[0] == key_b[0]) and (key_a[1] == key_b[1]) and (key_a[2] == key_b[2]); + return (key_a[0] == key_b[0]) && (key_a[1] == key_b[1]) && (key_a[2] == key_b[2]); } -} // namspace amcl +} // namespace badger_amcl diff --git a/src/amcl/sensors/odom.cpp b/src/amcl/sensors/odom.cpp index d7ebb82b..f1c77193 100644 --- a/src/amcl/sensors/odom.cpp +++ b/src/amcl/sensors/odom.cpp @@ -320,4 +320,4 @@ double Odom::angleDiff(double a, double b) return angles::shortest_angular_distance(b, a); } -} // namespace amcl +} // namespace badger_amcl diff --git a/src/amcl/sensors/planar_scanner.cpp b/src/amcl/sensors/planar_scanner.cpp index 96311af1..445a0d0e 100644 --- a/src/amcl/sensors/planar_scanner.cpp +++ b/src/amcl/sensors/planar_scanner.cpp @@ -216,7 +216,7 @@ double PlanarScanner::calcBeamModel(std::shared_ptr data, if (obs_range < data->range_max_) pz += z_rand_ * 1.0 / data->range_max_; - // TODO: outlier rejection for short readings + // TODO(anyone): outlier rejection for short readings ROS_ASSERT(pz <= 1.0); ROS_ASSERT(pz >= 0.0); @@ -304,14 +304,14 @@ double PlanarScanner::calcLikelihoodFieldModel(std::shared_ptr data, // Part 2: random measurements pz += z_rand_ * z_rand_mult; - // TODO: outlier rejection for short readings + // TODO(anyone): outlier rejection for short readings ROS_ASSERT(pz <= 1.0); ROS_ASSERT(pz >= 0.0); // p *= pz; // here we have an ad-hoc weighting scheme for combining beam probs // works well, though... - // TODO: investigate schemes for combining beam probs + // TODO(anyone): investigate schemes for combining beam probs p += pz * pz * pz; } @@ -461,7 +461,7 @@ double PlanarScanner::calcLikelihoodFieldModelProb(std::shared_ptr d ROS_ASSERT(pz <= 1.0); ROS_ASSERT(pz >= 0.0); - // TODO: outlier rejection for short readings + // TODO(anyone): outlier rejection for short readings if (!do_beamskip) { @@ -700,4 +700,4 @@ Eigen::Vector3d PlanarScanner::coordAdd(const Eigen::Vector3d& a, const Eigen::V return c; } -} // namespace amcl +} // namespace badger_amcl diff --git a/src/amcl/sensors/point_cloud_scanner.cpp b/src/amcl/sensors/point_cloud_scanner.cpp index 85d50bba..9c91606b 100644 --- a/src/amcl/sensors/point_cloud_scanner.cpp +++ b/src/amcl/sensors/point_cloud_scanner.cpp @@ -264,4 +264,4 @@ int PointCloudScanner::getMaxBeams() return max_beams_; } -} // namspace amcl +} // namespace badger_amcl diff --git a/test/test_badger_amcl.cpp b/test/test_badger_amcl.cpp index 670be8fa..b338155d 100644 --- a/test/test_badger_amcl.cpp +++ b/test/test_badger_amcl.cpp @@ -145,9 +145,9 @@ TEST(TestBadgerAmcl, testOccupancyMapDistances) { unsigned int index = occupancy_map.computeCellIndex(x, y); badger_amcl::MapCellState state; - if (x == 1 and y > 2 and y < 12) + if (x == 1 && y > 2 && y < 12) state = badger_amcl::MapCellState::CELL_UNKNOWN; - else if (x > 4 and x < 14 and (y == 10 or y == 15)) + else if (x > 4 && x < 14 && (y == 10 || y == 15)) state = badger_amcl::MapCellState::CELL_OCCUPIED; else state = badger_amcl::MapCellState::CELL_FREE; @@ -156,10 +156,10 @@ TEST(TestBadgerAmcl, testOccupancyMapDistances) } } EXPECT_TRUE(occupancy_map.isValid({0, 0})); - EXPECT_TRUE(not occupancy_map.isValid({-1, 5})); + EXPECT_TRUE(!occupancy_map.isValid({-1, 5})); EXPECT_TRUE(occupancy_map.isValid({99, 149})); - EXPECT_TRUE(not occupancy_map.isValid({100, 150})); - EXPECT_TRUE(not occupancy_map.isValid({149, 99})); + EXPECT_TRUE(!occupancy_map.isValid({100, 150})); + EXPECT_TRUE(!occupancy_map.isValid({149, 99})); occupancy_map.updateDistancesLUT(0.3); EXPECT_EQ(occupancy_map.getCellState(0, 0), badger_amcl::MapCellState::CELL_FREE); EXPECT_EQ(occupancy_map.getCellState(1, 3), badger_amcl::MapCellState::CELL_UNKNOWN);