Skip to content

Commit 25c8a6b

Browse files
elsayedelsheikhelsayedelsheikh
committed
initial thoughs
Signed-off-by: ElSayed ElSheikh <elsayed.elsheikh97@gmail.com>
1 parent dc92138 commit 25c8a6b

File tree

2 files changed

+71
-174
lines changed

2 files changed

+71
-174
lines changed

‎nav2_amcl/src/amcl_node.cpp

Lines changed: 48 additions & 166 deletions
Original file line numberDiff line numberDiff line change
@@ -58,126 +58,6 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options)
5858
: nav2::LifecycleNode("amcl", "", options)
5959
{
6060
RCLCPP_INFO(get_logger(), "Creating");
61-
62-
declare_parameter(
63-
"alpha1", rclcpp::ParameterValue(0.2));
64-
65-
declare_parameter(
66-
"alpha2", rclcpp::ParameterValue(0.2));
67-
68-
declare_parameter(
69-
"alpha3", rclcpp::ParameterValue(0.2));
70-
71-
declare_parameter(
72-
"alpha4", rclcpp::ParameterValue(0.2));
73-
74-
declare_parameter(
75-
"alpha5", rclcpp::ParameterValue(0.2));
76-
77-
declare_parameter(
78-
"base_frame_id", rclcpp::ParameterValue(std::string("base_footprint")));
79-
80-
declare_parameter("beam_skip_distance", rclcpp::ParameterValue(0.5));
81-
declare_parameter("beam_skip_error_threshold", rclcpp::ParameterValue(0.9));
82-
declare_parameter("beam_skip_threshold", rclcpp::ParameterValue(0.3));
83-
declare_parameter("do_beamskip", rclcpp::ParameterValue(false));
84-
85-
declare_parameter(
86-
"global_frame_id", rclcpp::ParameterValue(std::string("map")));
87-
88-
declare_parameter(
89-
"lambda_short", rclcpp::ParameterValue(0.1));
90-
91-
declare_parameter(
92-
"laser_likelihood_max_dist", rclcpp::ParameterValue(2.0));
93-
94-
declare_parameter(
95-
"laser_max_range", rclcpp::ParameterValue(100.0));
96-
97-
declare_parameter(
98-
"laser_min_range", rclcpp::ParameterValue(-1.0));
99-
100-
declare_parameter(
101-
"laser_model_type", rclcpp::ParameterValue(std::string("likelihood_field")));
102-
103-
declare_parameter(
104-
"set_initial_pose", rclcpp::ParameterValue(false));
105-
106-
declare_parameter(
107-
"initial_pose.x", rclcpp::ParameterValue(0.0));
108-
109-
declare_parameter(
110-
"initial_pose.y", rclcpp::ParameterValue(0.0));
111-
112-
declare_parameter(
113-
"initial_pose.z", rclcpp::ParameterValue(0.0));
114-
115-
declare_parameter(
116-
"initial_pose.yaw", rclcpp::ParameterValue(0.0));
117-
118-
declare_parameter(
119-
"max_beams", rclcpp::ParameterValue(60));
120-
121-
declare_parameter(
122-
"max_particles", rclcpp::ParameterValue(2000));
123-
124-
declare_parameter(
125-
"min_particles", rclcpp::ParameterValue(500));
126-
127-
declare_parameter(
128-
"odom_frame_id", rclcpp::ParameterValue(std::string("odom")));
129-
130-
declare_parameter("pf_err", rclcpp::ParameterValue(0.05));
131-
declare_parameter("pf_z", rclcpp::ParameterValue(0.99));
132-
133-
declare_parameter(
134-
"recovery_alpha_fast", rclcpp::ParameterValue(0.0));
135-
136-
declare_parameter(
137-
"recovery_alpha_slow", rclcpp::ParameterValue(0.0));
138-
139-
declare_parameter(
140-
"resample_interval", rclcpp::ParameterValue(1));
141-
142-
declare_parameter("robot_model_type",
143-
rclcpp::ParameterValue("nav2_amcl::DifferentialMotionModel"));
144-
145-
declare_parameter(
146-
"save_pose_rate", rclcpp::ParameterValue(0.5));
147-
148-
declare_parameter("sigma_hit", rclcpp::ParameterValue(0.2));
149-
150-
declare_parameter(
151-
"tf_broadcast", rclcpp::ParameterValue(true));
152-
153-
declare_parameter(
154-
"transform_tolerance", rclcpp::ParameterValue(1.0));
155-
156-
declare_parameter(
157-
"update_min_a", rclcpp::ParameterValue(0.2));
158-
159-
declare_parameter(
160-
"update_min_d", rclcpp::ParameterValue(0.25));
161-
162-
declare_parameter("z_hit", rclcpp::ParameterValue(0.5));
163-
declare_parameter("z_max", rclcpp::ParameterValue(0.05));
164-
declare_parameter("z_rand", rclcpp::ParameterValue(0.5));
165-
declare_parameter("z_short", rclcpp::ParameterValue(0.05));
166-
167-
declare_parameter(
168-
"always_reset_initial_pose", rclcpp::ParameterValue(false));
169-
170-
declare_parameter(
171-
"scan_topic", rclcpp::ParameterValue("scan"));
172-
173-
declare_parameter(
174-
"map_topic", rclcpp::ParameterValue("map"));
175-
176-
declare_parameter(
177-
"first_map_only", rclcpp::ParameterValue(false));
178-
179-
declare_parameter(
180-
"freespace_downsampling", rclcpp::ParameterValue(false));
18161
}
18262

18363
AmclNode::~AmclNode()
@@ -1013,52 +893,54 @@ AmclNode::initParameters()
1013893
double save_pose_rate;
1014894
double tmp_tol;
1015895

1016-
get_parameter("alpha1", alpha1_);
1017-
get_parameter("alpha2", alpha2_);
1018-
get_parameter("alpha3", alpha3_);
1019-
get_parameter("alpha4", alpha4_);
1020-
get_parameter("alpha5", alpha5_);
1021-
get_parameter("base_frame_id", base_frame_id_);
1022-
get_parameter("beam_skip_distance", beam_skip_distance_);
1023-
get_parameter("beam_skip_error_threshold", beam_skip_error_threshold_);
1024-
get_parameter("beam_skip_threshold", beam_skip_threshold_);
1025-
get_parameter("do_beamskip", do_beamskip_);
1026-
get_parameter("global_frame_id", global_frame_id_);
1027-
get_parameter("lambda_short", lambda_short_);
1028-
get_parameter("laser_likelihood_max_dist", laser_likelihood_max_dist_);
1029-
get_parameter("laser_max_range", laser_max_range_);
1030-
get_parameter("laser_min_range", laser_min_range_);
1031-
get_parameter("laser_model_type", sensor_model_type_);
1032-
get_parameter("set_initial_pose", set_initial_pose_);
1033-
get_parameter("initial_pose.x", initial_pose_x_);
1034-
get_parameter("initial_pose.y", initial_pose_y_);
1035-
get_parameter("initial_pose.z", initial_pose_z_);
1036-
get_parameter("initial_pose.yaw", initial_pose_yaw_);
1037-
get_parameter("max_beams", max_beams_);
1038-
get_parameter("max_particles", max_particles_);
1039-
get_parameter("min_particles", min_particles_);
1040-
get_parameter("odom_frame_id", odom_frame_id_);
1041-
get_parameter("pf_err", pf_err_);
1042-
get_parameter("pf_z", pf_z_);
1043-
get_parameter("recovery_alpha_fast", alpha_fast_);
1044-
get_parameter("recovery_alpha_slow", alpha_slow_);
1045-
get_parameter("resample_interval", resample_interval_);
1046-
get_parameter("robot_model_type", robot_model_type_);
1047-
get_parameter("save_pose_rate", save_pose_rate);
1048-
get_parameter("sigma_hit", sigma_hit_);
1049-
get_parameter("tf_broadcast", tf_broadcast_);
1050-
get_parameter("transform_tolerance", tmp_tol);
1051-
get_parameter("update_min_a", a_thresh_);
1052-
get_parameter("update_min_d", d_thresh_);
1053-
get_parameter("z_hit", z_hit_);
1054-
get_parameter("z_max", z_max_);
1055-
get_parameter("z_rand", z_rand_);
1056-
get_parameter("z_short", z_short_);
1057-
get_parameter("first_map_only", first_map_only_);
1058-
get_parameter("always_reset_initial_pose", always_reset_initial_pose_);
1059-
get_parameter("scan_topic", scan_topic_);
1060-
get_parameter("map_topic", map_topic_);
1061-
get_parameter("freespace_downsampling", freespace_downsampling_);
896+
alpha1_ = this->declare_or_get_parameter("alpha1", 0.2);
897+
alpha2_ = this->declare_or_get_parameter("alpha2", 0.2);
898+
alpha3_ = this->declare_or_get_parameter("alpha3", 0.2);
899+
alpha4_ = this->declare_or_get_parameter("alpha4", 0.2);
900+
alpha5_ = this->declare_or_get_parameter("alpha5", 0.2);
901+
base_frame_id_ = this->declare_or_get_parameter("base_frame_id", std::string{"base_footprint"});
902+
beam_skip_distance_ = this->declare_or_get_parameter("beam_skip_distance", 0.5);
903+
beam_skip_error_threshold_ = this->declare_or_get_parameter("beam_skip_error_threshold", 0.9);
904+
beam_skip_threshold_ = this->declare_or_get_parameter("beam_skip_threshold", 0.3);
905+
do_beamskip_ = this->declare_or_get_parameter("do_beamskip", false);
906+
global_frame_id_ = this->declare_or_get_parameter("global_frame_id", std::string{"map"});
907+
lambda_short_ = this->declare_or_get_parameter("lambda_short", 0.1);
908+
laser_likelihood_max_dist_ = this->declare_or_get_parameter("laser_likelihood_max_dist", 2.0);
909+
laser_max_range_ = this->declare_or_get_parameter("laser_max_range", 100.0);
910+
laser_min_range_ = this->declare_or_get_parameter("laser_min_range", -1.0);
911+
sensor_model_type_ = this->declare_or_get_parameter(
912+
"laser_model_type", std::string{"likelihood_field"});
913+
set_initial_pose_ = this->declare_or_get_parameter("set_initial_pose", false);
914+
initial_pose_x_ = this->declare_or_get_parameter("initial_pose.x", 0.0);
915+
initial_pose_y_ = this->declare_or_get_parameter("initial_pose.y", 0.0);
916+
initial_pose_z_ = this->declare_or_get_parameter("initial_pose.z", 0.0);
917+
initial_pose_yaw_ = this->declare_or_get_parameter("initial_pose.yaw", 0.0);
918+
max_beams_ = this->declare_or_get_parameter("max_beams", 60);
919+
max_particles_ = this->declare_or_get_parameter("max_particles", 2000);
920+
min_particles_ = this->declare_or_get_parameter("min_particles", 500);
921+
odom_frame_id_ = this->declare_or_get_parameter("odom_frame_id", std::string{"odom"});
922+
pf_err_ = this->declare_or_get_parameter("pf_err", 0.05);
923+
pf_z_ = this->declare_or_get_parameter("pf_z", 0.99);
924+
alpha_fast_ = this->declare_or_get_parameter("recovery_alpha_fast", 0.0);
925+
alpha_slow_ = this->declare_or_get_parameter("recovery_alpha_slow", 0.0);
926+
resample_interval_ = this->declare_or_get_parameter("resample_interval", 1);
927+
robot_model_type_ = this->declare_or_get_parameter(
928+
"robot_model_type", std::string{"nav2_amcl::DifferentialMotionModel"});
929+
save_pose_rate = this->declare_or_get_parameter("save_pose_rate", 0.5);
930+
sigma_hit_ = this->declare_or_get_parameter("sigma_hit", 0.2);
931+
tf_broadcast_ = this->declare_or_get_parameter("tf_broadcast", true);
932+
tmp_tol = this->declare_or_get_parameter("transform_tolerance", 1.0);
933+
a_thresh_ = this->declare_or_get_parameter("update_min_a", 0.2);
934+
d_thresh_ = this->declare_or_get_parameter("update_min_d", 0.25);
935+
z_hit_ = this->declare_or_get_parameter("z_hit", 0.5);
936+
z_max_ = this->declare_or_get_parameter("z_max", 0.05);
937+
z_rand_ = this->declare_or_get_parameter("z_rand", 0.5);
938+
z_short_ = this->declare_or_get_parameter("z_short", 0.05);
939+
first_map_only_ = this->declare_or_get_parameter("first_map_only", false);
940+
always_reset_initial_pose_ = this->declare_or_get_parameter("always_reset_initial_pose", false);
941+
scan_topic_ = this->declare_or_get_parameter("scan_topic", std::string{"scan"});
942+
map_topic_ = this->declare_or_get_parameter("map_topic", std::string{"map"});
943+
freespace_downsampling_ = this->declare_or_get_parameter("freespace_downsampling", false);
1062944

1063945
save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
1064946
transform_tolerance_ = tf2::durationFromSec(tmp_tol);

‎nav2_ros_common/include/nav2_ros_common/lifecycle_node.hpp

Lines changed: 23 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -67,14 +67,9 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
6767
rclcpp::Parameter(
6868
bond::msg::Constants::DISABLE_HEARTBEAT_TIMEOUT_PARAM, true));
6969

70-
nav2::declare_parameter_if_not_declared(
71-
this, "bond_heartbeat_period", rclcpp::ParameterValue(0.1));
72-
this->get_parameter("bond_heartbeat_period", bond_heartbeat_period);
73-
74-
bool autostart_node = false;
75-
nav2::declare_parameter_if_not_declared(
76-
this, "autostart_node", rclcpp::ParameterValue(false));
77-
this->get_parameter("autostart_node", autostart_node);
70+
bond_heartbeat_period = this->declare_or_get_parameter<double>("bond_heartbeat_period", 0.1);
71+
72+
bool autostart_node = this->declare_or_get_parameter("autostart_node", false);
7873
if (autostart_node) {
7974
autostart();
8075
}
@@ -108,6 +103,26 @@ class LifecycleNode : public rclcpp_lifecycle::LifecycleNode
108103
}
109104
}
110105

106+
/**
107+
* @brief Declares or gets a parameter.
108+
* If the parameter is already declared, returns its value;
109+
* otherwise declares it and returns the default value.
110+
* @param parameter_name Name of the parameter
111+
* @param default_value Default value of the parameter
112+
* @param parameter_descriptor Optional parameter descriptor
113+
* @return The value of the param from the override if existent, otherwise the default value.
114+
*/
115+
template<typename ParamType>
116+
inline ParamType declare_or_get_parameter(
117+
const std::string & parameter_name,
118+
const ParamType & default_value,
119+
const ParameterDescriptor & parameter_descriptor = ParameterDescriptor())
120+
{
121+
return nav2::declare_or_get_parameter(
122+
shared_from_this(), parameter_name,
123+
default_value, parameter_descriptor);
124+
}
125+
111126
/**
112127
* @brief Create a subscription to a topic using Nav2 QoS profiles and SubscriptionOptions
113128
* @param topic_name Name of topic

0 commit comments

Comments
 (0)