From 950d76cd49e9af7286472c108f017a85124ee365 Mon Sep 17 00:00:00 2001 From: Cryst4L9527 Date: Tue, 11 Oct 2022 11:47:07 +0800 Subject: [PATCH 1/9] odom alpha restriction --- nav2_amcl/src/amcl_node.cpp | 45 ++++++++++++++++++++++++++++++++----- 1 file changed, 40 insertions(+), 5 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index a28b4927a3e..844f9842533 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1158,19 +1158,54 @@ AmclNode::dynamicParametersCallback( if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == "alpha1") { alpha1_ = parameter.as_double(); - reinit_odom = true; + //alpha restricted to be non-negative + if (alpha1_ < 0.0){ + RCLCPP_WARN( + get_logger(), "You've set alpha1 to be negative," + " this isn't allowed, so the alpha1 will be set to be zero."); + alpha1_ = 0.0; + } + else reinit_odom = true; } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); - reinit_odom = true; + //alpha restricted to be non-negative + if (alpha2_ < 0.0){ + RCLCPP_WARN( + get_logger(), "You've set alpha2 to be negative," + " this isn't allowed, so the alpha2 will be set to be zero."); + alpha2_ = 0.0; + } + else reinit_odom = true; } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); - reinit_odom = true; + //alpha restricted to be non-negative + if (alpha3_ < 0.0){ + RCLCPP_WARN( + get_logger(), "You've set alpha3 to be negative," + " this isn't allowed, so the alpha3 will be set to be zero."); + alpha3_ = 0.0; + } + else reinit_odom = true; } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); - reinit_odom = true; + //alpha restricted to be non-negative + if (alpha4_ < 0.0){ + RCLCPP_WARN( + get_logger(), "You've set alpha4 to be negative," + " this isn't allowed, so the alpha4 will be set to be zero."); + alpha4_ = 0.0; + } + else reinit_odom = true; } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); - reinit_odom = true; + //alpha restricted to be non-negative + if (alpha5_ < 0.0){ + RCLCPP_WARN( + get_logger(), "You've set alpha5 to be negative," + " this isn't allowed, so the alpha5 will be set to be zero."); + alpha5_ = 0.0; + } + else reinit_odom = true; } else if (param_name == "beam_skip_distance") { beam_skip_distance_ = parameter.as_double(); reinit_laser = true; From b67393821a25750bf55b7a6cd2d213727847d8d7 Mon Sep 17 00:00:00 2001 From: Cryst4L9527 Date: Tue, 11 Oct 2022 13:35:58 +0800 Subject: [PATCH 2/9] odom alpha code style --- nav2_amcl/src/amcl_node.cpp | 25 ++++++++++--------------- 1 file changed, 10 insertions(+), 15 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 844f9842533..4e3db1faa00 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1159,53 +1159,48 @@ AmclNode::dynamicParametersCallback( if (param_name == "alpha1") { alpha1_ = parameter.as_double(); //alpha restricted to be non-negative - if (alpha1_ < 0.0){ + if (alpha1_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha1 to be negative," " this isn't allowed, so the alpha1 will be set to be zero."); alpha1_ = 0.0; - } - else reinit_odom = true; + } else if (alpha1_ > 0.0) reinit_odom = true; } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); //alpha restricted to be non-negative - if (alpha2_ < 0.0){ + if (alpha2_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha2 to be negative," " this isn't allowed, so the alpha2 will be set to be zero."); alpha2_ = 0.0; - } - else reinit_odom = true; + } else if (alpha2_ > 0.0) reinit_odom = true; } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); //alpha restricted to be non-negative - if (alpha3_ < 0.0){ + if (alpha3_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha3 to be negative," " this isn't allowed, so the alpha3 will be set to be zero."); alpha3_ = 0.0; - } - else reinit_odom = true; + } else if (alpha3_ > 0.0) reinit_odom = true; } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); //alpha restricted to be non-negative - if (alpha4_ < 0.0){ + if (alpha4_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha4 to be negative," " this isn't allowed, so the alpha4 will be set to be zero."); alpha4_ = 0.0; - } - else reinit_odom = true; + } else if (alpha4_ > 0.0) reinit_odom = true; } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); //alpha restricted to be non-negative - if (alpha5_ < 0.0){ + if (alpha5_ < 0.0) { RCLCPP_WARN( get_logger(), "You've set alpha5 to be negative," " this isn't allowed, so the alpha5 will be set to be zero."); alpha5_ = 0.0; - } - else reinit_odom = true; + } else if (alpha5_ > 0.0) reinit_odom = true; } else if (param_name == "beam_skip_distance") { beam_skip_distance_ = parameter.as_double(); reinit_laser = true; From ec5adb55b295dfcf400dd326bafcf897bfeca359 Mon Sep 17 00:00:00 2001 From: Cryst4L9527 Date: Tue, 11 Oct 2022 14:57:08 +0800 Subject: [PATCH 3/9] odom alpha code style --- nav2_amcl/src/amcl_node.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 4e3db1faa00..735c7d95a3d 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1164,7 +1164,8 @@ AmclNode::dynamicParametersCallback( get_logger(), "You've set alpha1 to be negative," " this isn't allowed, so the alpha1 will be set to be zero."); alpha1_ = 0.0; - } else if (alpha1_ > 0.0) reinit_odom = true; + } + reinit_odom = true; } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); //alpha restricted to be non-negative @@ -1173,7 +1174,8 @@ AmclNode::dynamicParametersCallback( get_logger(), "You've set alpha2 to be negative," " this isn't allowed, so the alpha2 will be set to be zero."); alpha2_ = 0.0; - } else if (alpha2_ > 0.0) reinit_odom = true; + } + reinit_odom = true; } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); //alpha restricted to be non-negative @@ -1182,7 +1184,8 @@ AmclNode::dynamicParametersCallback( get_logger(), "You've set alpha3 to be negative," " this isn't allowed, so the alpha3 will be set to be zero."); alpha3_ = 0.0; - } else if (alpha3_ > 0.0) reinit_odom = true; + } + reinit_odom = true; } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); //alpha restricted to be non-negative @@ -1191,7 +1194,8 @@ AmclNode::dynamicParametersCallback( get_logger(), "You've set alpha4 to be negative," " this isn't allowed, so the alpha4 will be set to be zero."); alpha4_ = 0.0; - } else if (alpha4_ > 0.0) reinit_odom = true; + } + reinit_odom = true; } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); //alpha restricted to be non-negative @@ -1200,7 +1204,8 @@ AmclNode::dynamicParametersCallback( get_logger(), "You've set alpha5 to be negative," " this isn't allowed, so the alpha5 will be set to be zero."); alpha5_ = 0.0; - } else if (alpha5_ > 0.0) reinit_odom = true; + } + reinit_odom = true; } else if (param_name == "beam_skip_distance") { beam_skip_distance_ = parameter.as_double(); reinit_laser = true; From d639b1ad14b231f162fbd1a4f8662c9922298a92 Mon Sep 17 00:00:00 2001 From: Cryst4L9527 Date: Tue, 11 Oct 2022 15:47:38 +0800 Subject: [PATCH 4/9] odom alpha code style --- nav2_amcl/src/amcl_node.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 735c7d95a3d..2e9730ae56b 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1164,7 +1164,7 @@ AmclNode::dynamicParametersCallback( get_logger(), "You've set alpha1 to be negative," " this isn't allowed, so the alpha1 will be set to be zero."); alpha1_ = 0.0; - } + } reinit_odom = true; } else if (param_name == "alpha2") { alpha2_ = parameter.as_double(); @@ -1174,7 +1174,7 @@ AmclNode::dynamicParametersCallback( get_logger(), "You've set alpha2 to be negative," " this isn't allowed, so the alpha2 will be set to be zero."); alpha2_ = 0.0; - } + } reinit_odom = true; } else if (param_name == "alpha3") { alpha3_ = parameter.as_double(); @@ -1184,7 +1184,7 @@ AmclNode::dynamicParametersCallback( get_logger(), "You've set alpha3 to be negative," " this isn't allowed, so the alpha3 will be set to be zero."); alpha3_ = 0.0; - } + } reinit_odom = true; } else if (param_name == "alpha4") { alpha4_ = parameter.as_double(); @@ -1194,7 +1194,7 @@ AmclNode::dynamicParametersCallback( get_logger(), "You've set alpha4 to be negative," " this isn't allowed, so the alpha4 will be set to be zero."); alpha4_ = 0.0; - } + } reinit_odom = true; } else if (param_name == "alpha5") { alpha5_ = parameter.as_double(); @@ -1204,7 +1204,7 @@ AmclNode::dynamicParametersCallback( get_logger(), "You've set alpha5 to be negative," " this isn't allowed, so the alpha5 will be set to be zero."); alpha5_ = 0.0; - } + } reinit_odom = true; } else if (param_name == "beam_skip_distance") { beam_skip_distance_ = parameter.as_double(); From 1c0827dfd3f049f8741a1dd6d20b99d004e1184f Mon Sep 17 00:00:00 2001 From: Cryst4L9527 Date: Wed, 12 Oct 2022 15:24:53 +0800 Subject: [PATCH 5/9] map-size restriction --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index c9ca90ad075..6763a43b378 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -659,9 +659,21 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter if (name == "width") { resize_map = true; map_width_meters_ = parameter.as_int(); + if(map_width_meters_ <= 0) { + RCLCPP_WARN( + get_logger(), "You've set width of map to be negative," + " this isn't allowed, so the width will be set to be default value 5."); + map_width_meters_ = 5; + } } else if (name == "height") { resize_map = true; map_height_meters_ = parameter.as_int(); + if(map_height_meters_ <= 0) { + RCLCPP_WARN( + get_logger(), "You've set height of map to be negative," + " this isn't allowed, so the height will be set to be default value 5."); + map_height_meters_ = 5; + } } } else if (type == ParameterType::PARAMETER_STRING) { if (name == "footprint") { From 2005e28c0bb324365c8fe81dc388da3d14c65c0d Mon Sep 17 00:00:00 2001 From: Cryst4L9527 Date: Wed, 12 Oct 2022 16:52:42 +0800 Subject: [PATCH 6/9] map-size code style --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 6763a43b378..77c31f46363 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -659,7 +659,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter if (name == "width") { resize_map = true; map_width_meters_ = parameter.as_int(); - if(map_width_meters_ <= 0) { + if (map_width_meters_ <= 0) { RCLCPP_WARN( get_logger(), "You've set width of map to be negative," " this isn't allowed, so the width will be set to be default value 5."); @@ -668,7 +668,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter } else if (name == "height") { resize_map = true; map_height_meters_ = parameter.as_int(); - if(map_height_meters_ <= 0) { + if (map_height_meters_ <= 0) { RCLCPP_WARN( get_logger(), "You've set height of map to be negative," " this isn't allowed, so the height will be set to be default value 5."); From 79ecfb0dea62e5b1f8d84c83c63668172f701f83 Mon Sep 17 00:00:00 2001 From: Cryst4L9527 Date: Thu, 13 Oct 2022 11:36:17 +0800 Subject: [PATCH 7/9] map-size rejection --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 28 +++++++++++++------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 77c31f46363..a1ac8d094c6 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -657,22 +657,22 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter } } else if (type == ParameterType::PARAMETER_INTEGER) { if (name == "width") { - resize_map = true; - map_width_meters_ = parameter.as_int(); - if (map_width_meters_ <= 0) { - RCLCPP_WARN( - get_logger(), "You've set width of map to be negative," - " this isn't allowed, so the width will be set to be default value 5."); - map_width_meters_ = 5; + if (parameter.as_int() > 0) { + resize_map = true; + map_width_meters_ = parameter.as_int(); + } else { + RCLCPP_ERROR( + get_logger(), "You try to set width of map to be negative or zero," + " this isn't allowed, please give a positive value."); } } else if (name == "height") { - resize_map = true; - map_height_meters_ = parameter.as_int(); - if (map_height_meters_ <= 0) { - RCLCPP_WARN( - get_logger(), "You've set height of map to be negative," - " this isn't allowed, so the height will be set to be default value 5."); - map_height_meters_ = 5; + if (parameter.as_int() > 0) { + resize_map = true; + map_height_meters_ = parameter.as_int(); + } else { + RCLCPP_ERROR( + get_logger(), "You try to set height of map to be negative or zero," + " this isn't allowed, please give a positive value."); } } } else if (type == ParameterType::PARAMETER_STRING) { From e8d73d5f5ed087dcc7f54abce5e23d20fdbbbe91 Mon Sep 17 00:00:00 2001 From: Cryst4L9527 Date: Thu, 13 Oct 2022 11:45:50 +0800 Subject: [PATCH 8/9] map-size codestyle --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index a1ac8d094c6..2aec111d3ff 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -661,7 +661,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter resize_map = true; map_width_meters_ = parameter.as_int(); } else { - RCLCPP_ERROR( + RCLCPP_ERROR( get_logger(), "You try to set width of map to be negative or zero," " this isn't allowed, please give a positive value."); } @@ -670,7 +670,7 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter resize_map = true; map_height_meters_ = parameter.as_int(); } else { - RCLCPP_ERROR( + RCLCPP_ERROR( get_logger(), "You try to set height of map to be negative or zero," " this isn't allowed, please give a positive value."); } From 44c7dbfc4ba693f01895385a45fec920c562e362 Mon Sep 17 00:00:00 2001 From: Cryst4L9527 Date: Fri, 14 Oct 2022 09:53:02 +0800 Subject: [PATCH 9/9] map-size return false --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 2aec111d3ff..50e74c511b2 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -664,6 +664,8 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter RCLCPP_ERROR( get_logger(), "You try to set width of map to be negative or zero," " this isn't allowed, please give a positive value."); + result.successful = false; + return result; } } else if (name == "height") { if (parameter.as_int() > 0) { @@ -673,6 +675,8 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter RCLCPP_ERROR( get_logger(), "You try to set height of map to be negative or zero," " this isn't allowed, please give a positive value."); + result.successful = false; + return result; } } } else if (type == ParameterType::PARAMETER_STRING) {