amcl is a probabilistic localization system for a robot moving in
diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp
index 2e9730ae56..4afae2a122 100644
--- a/nav2_amcl/src/amcl_node.cpp
+++ b/nav2_amcl/src/amcl_node.cpp
@@ -146,11 +146,11 @@ AmclNode::AmclNode(const rclcpp::NodeOptions & options)
add_parameter(
"max_particles", rclcpp::ParameterValue(2000),
- "Minimum allowed number of particles");
+ "Maximum allowed number of particles");
add_parameter(
"min_particles", rclcpp::ParameterValue(500),
- "Maximum allowed number of particles");
+ "Minimum allowed number of particles");
add_parameter(
"odom_frame_id", rclcpp::ParameterValue(std::string("odom")),
diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt
index 7a73c2da59..944ca7bce4 100644
--- a/nav2_behavior_tree/CMakeLists.txt
+++ b/nav2_behavior_tree/CMakeLists.txt
@@ -129,6 +129,9 @@ list(APPEND plugin_libs nav2_distance_traveled_condition_bt_node)
add_library(nav2_initial_pose_received_condition_bt_node SHARED plugins/condition/initial_pose_received_condition.cpp)
list(APPEND plugin_libs nav2_initial_pose_received_condition_bt_node)
+add_library(nav2_is_battery_charging_condition_bt_node SHARED plugins/condition/is_battery_charging_condition.cpp)
+list(APPEND plugin_libs nav2_is_battery_charging_condition_bt_node)
+
add_library(nav2_is_battery_low_condition_bt_node SHARED plugins/condition/is_battery_low_condition.cpp)
list(APPEND plugin_libs nav2_is_battery_low_condition_bt_node)
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
index 45363b7588..5701c59d73 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_node.hpp
@@ -194,6 +194,9 @@ class BtActionNode : public BT::ActionNodeBase
// user defined callback
on_tick();
+ if (!should_send_goal_) {
+ return BT::NodeStatus::FAILURE;
+ }
send_new_goal();
}
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
index a5fe6e0021..e050ab3038 100644
--- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp
@@ -39,13 +39,16 @@ class BtServiceNode : public BT::ActionNodeBase
public:
/**
* @brief A nav2_behavior_tree::BtServiceNode constructor
- * @param service_node_name Service name this node creates a client for
+ * @param service_node_name BT node name
* @param conf BT node configuration
+ * @param service_name Optional service name this node creates a client for instead of from input port
*/
BtServiceNode(
const std::string & service_node_name,
- const BT::NodeConfiguration & conf)
- : BT::ActionNodeBase(service_node_name, conf), service_node_name_(service_node_name)
+ const BT::NodeConfiguration & conf,
+ const std::string & service_name = "")
+ : BT::ActionNodeBase(service_node_name, conf), service_name_(service_name), service_node_name_(
+ service_node_name)
{
node_ = config().blackboard->template get("node");
callback_group_ = node_->create_callback_group(
@@ -128,7 +131,17 @@ class BtServiceNode : public BT::ActionNodeBase
BT::NodeStatus tick() override
{
if (!request_sent_) {
+ // reset the flag to send the request or not,
+ // allowing the user the option to set it in on_tick
+ should_send_request_ = true;
+
+ // user defined callback, may modify "should_send_request_".
on_tick();
+
+ if (!should_send_request_) {
+ return BT::NodeStatus::FAILURE;
+ }
+
future_result_ = service_client_->async_send_request(request_).share();
sent_time_ = node_->now();
request_sent_ = true;
@@ -240,6 +253,9 @@ class BtServiceNode : public BT::ActionNodeBase
std::shared_future future_result_;
bool request_sent_{false};
rclcpp::Time sent_time_;
+
+ // Can be set in on_tick or on_wait_for_result to indicate if a request should be sent.
+ bool should_send_request_;
};
} // namespace nav2_behavior_tree
diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp
new file mode 100644
index 0000000000..37a53b41e1
--- /dev/null
+++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp
@@ -0,0 +1,81 @@
+// Copyright (c) 2023 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_
+#define NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_
+
+#include
+#include
+#include
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/battery_state.hpp"
+#include "behaviortree_cpp_v3/condition_node.h"
+
+namespace nav2_behavior_tree
+{
+
+/**
+ * @brief A BT::ConditionNode that listens to a battery topic and
+ * returns SUCCESS when battery is charging and FAILURE otherwise
+ */
+class IsBatteryChargingCondition : public BT::ConditionNode
+{
+public:
+ /**
+ * @brief A constructor for nav2_behavior_tree::IsBatteryChargingCondition
+ * @param condition_name Name for the XML tag for this node
+ * @param conf BT node configuration
+ */
+ IsBatteryChargingCondition(
+ const std::string & condition_name,
+ const BT::NodeConfiguration & conf);
+
+ IsBatteryChargingCondition() = delete;
+
+ /**
+ * @brief The main override required by a BT action
+ * @return BT::NodeStatus Status of tick execution
+ */
+ BT::NodeStatus tick() override;
+
+ /**
+ * @brief Creates list of BT ports
+ * @return BT::PortsList Containing node-specific ports
+ */
+ static BT::PortsList providedPorts()
+ {
+ return {
+ BT::InputPort(
+ "battery_topic", std::string("/battery_status"), "Battery topic")
+ };
+ }
+
+private:
+ /**
+ * @brief Callback function for battery topic
+ * @param msg Shared pointer to sensor_msgs::msg::BatteryState message
+ */
+ void batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg);
+
+ rclcpp::CallbackGroup::SharedPtr callback_group_;
+ rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
+ rclcpp::Subscription::SharedPtr battery_sub_;
+ std::string battery_topic_;
+ bool is_battery_charging_;
+};
+
+} // namespace nav2_behavior_tree
+
+#endif // NAV2_BEHAVIOR_TREE__PLUGINS__CONDITION__IS_BATTERY_CHARGING_CONDITION_HPP_
diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml
index a70000e176..a225ea03d2 100644
--- a/nav2_behavior_tree/nav2_tree_nodes.xml
+++ b/nav2_behavior_tree/nav2_tree_nodes.xml
@@ -46,7 +46,7 @@
- Service name to cancel the assisted teleop behavior
+ Server name to cancel the assisted teleop behavior
Server timeout
@@ -113,7 +113,7 @@
Path to follow
- Goal checker
+ Goal checker
Server name
Server timeout
Follow Path error code
@@ -227,6 +227,10 @@
Bool if check based on voltage or total %
+
+ Topic for battery info
+
+
Distance to check if passed
reference frame to check in
diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml
index 68e00ae4d8..30442368aa 100644
--- a/nav2_behavior_tree/package.xml
+++ b/nav2_behavior_tree/package.xml
@@ -2,7 +2,7 @@
nav2_behavior_tree
- 1.1.6
+ 1.1.7
TODO
Michael Jeronimo
Carlos Orduno
diff --git a/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
new file mode 100644
index 0000000000..c9c05f6875
--- /dev/null
+++ b/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
@@ -0,0 +1,66 @@
+// Copyright (c) 2023 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include
+
+#include "nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp"
+
+namespace nav2_behavior_tree
+{
+
+IsBatteryChargingCondition::IsBatteryChargingCondition(
+ const std::string & condition_name,
+ const BT::NodeConfiguration & conf)
+: BT::ConditionNode(condition_name, conf),
+ battery_topic_("/battery_status"),
+ is_battery_charging_(false)
+{
+ getInput("battery_topic", battery_topic_);
+ auto node = config().blackboard->get("node");
+ callback_group_ = node->create_callback_group(
+ rclcpp::CallbackGroupType::MutuallyExclusive,
+ false);
+ callback_group_executor_.add_callback_group(callback_group_, node->get_node_base_interface());
+
+ rclcpp::SubscriptionOptions sub_option;
+ sub_option.callback_group = callback_group_;
+ battery_sub_ = node->create_subscription(
+ battery_topic_,
+ rclcpp::SystemDefaultsQoS(),
+ std::bind(&IsBatteryChargingCondition::batteryCallback, this, std::placeholders::_1),
+ sub_option);
+}
+
+BT::NodeStatus IsBatteryChargingCondition::tick()
+{
+ callback_group_executor_.spin_some();
+ if (is_battery_charging_) {
+ return BT::NodeStatus::SUCCESS;
+ }
+ return BT::NodeStatus::FAILURE;
+}
+
+void IsBatteryChargingCondition::batteryCallback(sensor_msgs::msg::BatteryState::SharedPtr msg)
+{
+ is_battery_charging_ =
+ (msg->power_supply_status == sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING);
+}
+
+} // namespace nav2_behavior_tree
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+ factory.registerNodeType("IsBatteryCharging");
+}
diff --git a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt
index c6edc8dcd7..c7a5ba1238 100644
--- a/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt
+++ b/nav2_behavior_tree/test/plugins/condition/CMakeLists.txt
@@ -34,6 +34,10 @@ ament_add_gtest(test_condition_is_stuck test_is_stuck.cpp)
target_link_libraries(test_condition_is_stuck nav2_is_stuck_condition_bt_node)
ament_target_dependencies(test_condition_is_stuck ${dependencies})
+ament_add_gtest(test_condition_is_battery_charging test_is_battery_charging.cpp)
+target_link_libraries(test_condition_is_battery_charging nav2_is_battery_charging_condition_bt_node)
+ament_target_dependencies(test_condition_is_battery_charging ${dependencies})
+
ament_add_gtest(test_condition_is_battery_low test_is_battery_low.cpp)
target_link_libraries(test_condition_is_battery_low nav2_is_battery_low_condition_bt_node)
ament_target_dependencies(test_condition_is_battery_low ${dependencies})
diff --git a/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp
new file mode 100644
index 0000000000..46af6d5c6e
--- /dev/null
+++ b/nav2_behavior_tree/test/plugins/condition/test_is_battery_charging.cpp
@@ -0,0 +1,130 @@
+// Copyright (c) 2023 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include
+#include
+#include
+#include
+#include
+
+#include "sensor_msgs/msg/battery_state.hpp"
+
+#include "utils/test_behavior_tree_fixture.hpp"
+#include "nav2_behavior_tree/plugins/condition/is_battery_charging_condition.hpp"
+
+class IsBatteryChargingConditionTestFixture : public ::testing::Test
+{
+public:
+ static void SetUpTestCase()
+ {
+ node_ = std::make_shared("test_is_battery_charging");
+ factory_ = std::make_shared();
+
+ config_ = new BT::NodeConfiguration();
+
+ // Create the blackboard that will be shared by all of the nodes in the tree
+ config_->blackboard = BT::Blackboard::create();
+ // Put items on the blackboard
+ config_->blackboard->set(
+ "node",
+ node_);
+
+ factory_->registerNodeType("IsBatteryCharging");
+
+ battery_pub_ = node_->create_publisher(
+ "/battery_status",
+ rclcpp::SystemDefaultsQoS());
+ }
+
+ static void TearDownTestCase()
+ {
+ delete config_;
+ config_ = nullptr;
+ battery_pub_.reset();
+ node_.reset();
+ factory_.reset();
+ }
+
+protected:
+ static rclcpp::Node::SharedPtr node_;
+ static BT::NodeConfiguration * config_;
+ static std::shared_ptr factory_;
+ static rclcpp::Publisher::SharedPtr battery_pub_;
+};
+
+rclcpp::Node::SharedPtr IsBatteryChargingConditionTestFixture::node_ = nullptr;
+BT::NodeConfiguration * IsBatteryChargingConditionTestFixture::config_ = nullptr;
+std::shared_ptr IsBatteryChargingConditionTestFixture::factory_ = nullptr;
+rclcpp::Publisher::SharedPtr
+IsBatteryChargingConditionTestFixture::battery_pub_ = nullptr;
+
+TEST_F(IsBatteryChargingConditionTestFixture, test_behavior_power_supply_status)
+{
+ std::string xml_txt =
+ R"(
+
+
+
+
+ )";
+
+ auto tree = factory_->createTreeFromText(xml_txt, config_->blackboard);
+
+ sensor_msgs::msg::BatteryState battery_msg;
+ battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN;
+ battery_pub_->publish(battery_msg);
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ rclcpp::spin_some(node_);
+ EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+ battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_CHARGING;
+ battery_pub_->publish(battery_msg);
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ rclcpp::spin_some(node_);
+ EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::SUCCESS);
+
+ battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_DISCHARGING;
+ battery_pub_->publish(battery_msg);
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ rclcpp::spin_some(node_);
+ EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+ battery_msg.power_supply_status =
+ sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_NOT_CHARGING;
+ battery_pub_->publish(battery_msg);
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ rclcpp::spin_some(node_);
+ EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+
+ battery_msg.power_supply_status = sensor_msgs::msg::BatteryState::POWER_SUPPLY_STATUS_FULL;
+ battery_pub_->publish(battery_msg);
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ rclcpp::spin_some(node_);
+ EXPECT_EQ(tree.tickRoot(), BT::NodeStatus::FAILURE);
+}
+
+int main(int argc, char ** argv)
+{
+ ::testing::InitGoogleTest(&argc, argv);
+
+ // initialize ROS
+ rclcpp::init(argc, argv);
+
+ bool all_successful = RUN_ALL_TESTS();
+
+ // shutdown ROS
+ rclcpp::shutdown();
+
+ return all_successful;
+}
diff --git a/nav2_behaviors/package.xml b/nav2_behaviors/package.xml
index b4b941258e..e3855fc231 100644
--- a/nav2_behaviors/package.xml
+++ b/nav2_behaviors/package.xml
@@ -2,7 +2,7 @@
nav2_behaviors
- 1.1.6
+ 1.1.7
TODO
Carlos Orduno
Steve Macenski
diff --git a/nav2_bringup/package.xml b/nav2_bringup/package.xml
index 3d0ee04f32..89e9d77d87 100644
--- a/nav2_bringup/package.xml
+++ b/nav2_bringup/package.xml
@@ -2,7 +2,7 @@
nav2_bringup
- 1.1.6
+ 1.1.7
Bringup scripts and configurations for the Nav2 stack
Michael Jeronimo
Steve Macenski
diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml
index a198b6ae70..b486f1982b 100644
--- a/nav2_bringup/params/nav2_multirobot_params_1.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml
@@ -103,6 +103,7 @@ bt_navigator:
- nav2_back_up_cancel_bt_node
- nav2_assisted_teleop_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
+ - nav2_is_battery_charging_condition_bt_node
error_code_names:
- compute_path_error_code
- follow_path_error_code
diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml
index f14c932170..0edd141539 100644
--- a/nav2_bringup/params/nav2_multirobot_params_2.yaml
+++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml
@@ -103,6 +103,7 @@ bt_navigator:
- nav2_back_up_cancel_bt_node
- nav2_assisted_teleop_cancel_bt_node
- nav2_drive_on_heading_cancel_bt_node
+ - nav2_is_battery_charging_condition_bt_node
error_code_names:
- compute_path_error_code
- follow_path_error_code
diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml
index 2e701cb421..7965e5d940 100644
--- a/nav2_bringup/params/nav2_params.yaml
+++ b/nav2_bringup/params/nav2_params.yaml
@@ -111,6 +111,7 @@ bt_navigator:
- compute_path_error_code
- follow_path_error_code
+ - nav2_is_battery_charging_condition_bt_node
bt_navigator_navigate_through_poses_rclcpp_node:
ros__parameters:
diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml
index 6218119ed6..dd8d8f9353 100644
--- a/nav2_bt_navigator/package.xml
+++ b/nav2_bt_navigator/package.xml
@@ -2,7 +2,7 @@
nav2_bt_navigator
- 1.1.6
+ 1.1.7
TODO
Michael Jeronimo
Apache-2.0
diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index ac40c54c70..7736106535 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -82,7 +82,8 @@ BtNavigator::BtNavigator(const rclcpp::NodeOptions & options)
"nav2_spin_cancel_bt_node",
"nav2_assisted_teleop_cancel_bt_node",
"nav2_back_up_cancel_bt_node",
- "nav2_drive_on_heading_cancel_bt_node"
+ "nav2_drive_on_heading_cancel_bt_node",
+ "nav2_is_battery_charging_condition_bt_node"
};
declare_parameter("plugin_lib_names", plugin_libs);
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
index 87f498e401..2dc2995856 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/collision_monitor_node.hpp
@@ -127,13 +127,16 @@ class CollisionMonitor : public nav2_util::LifecycleNode
* source->base time inerpolated transform.
* @param transform_tolerance Transform tolerance
* @param source_timeout Maximum time interval in which data is considered valid
+ * @param base_shift_correction Whether to correct source data towards to base frame movement,
+ * considering the difference between current time and latest source time
* @return True if all sources were configured successfully or false in failure case
*/
bool configureSources(
const std::string & base_frame_id,
const std::string & odom_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & source_timeout);
+ const rclcpp::Duration & source_timeout,
+ const bool base_shift_correction);
/**
* @brief Main processing routine
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
index c316c065f7..d5af32c277 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/pointcloud.hpp
@@ -41,6 +41,8 @@ class PointCloud : public Source
* @param global_frame_id Global frame ID for correct transform calculation
* @param transform_tolerance Transform tolerance
* @param source_timeout Maximum time interval in which data is considered valid
+ * @param base_shift_correction Whether to correct source data towards to base frame movement,
+ * considering the difference between current time and latest source time
*/
PointCloud(
const nav2_util::LifecycleNode::WeakPtr & node,
@@ -49,7 +51,8 @@ class PointCloud : public Source
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & source_timeout);
+ const rclcpp::Duration & source_timeout,
+ const bool base_shift_correction);
/**
* @brief PointCloud destructor
*/
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp
index 0fbe47501a..777b8e404c 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/range.hpp
@@ -41,6 +41,8 @@ class Range : public Source
* @param global_frame_id Global frame ID for correct transform calculation
* @param transform_tolerance Transform tolerance
* @param source_timeout Maximum time interval in which data is considered valid
+ * @param base_shift_correction Whether to correct source data towards to base frame movement,
+ * considering the difference between current time and latest source time
*/
Range(
const nav2_util::LifecycleNode::WeakPtr & node,
@@ -49,7 +51,8 @@ class Range : public Source
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & source_timeout);
+ const rclcpp::Duration & source_timeout,
+ const bool base_shift_correction);
/**
* @brief Range destructor
*/
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp
index 29747e8131..c3f7a11f3f 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/scan.hpp
@@ -41,6 +41,8 @@ class Scan : public Source
* @param global_frame_id Global frame ID for correct transform calculation
* @param transform_tolerance Transform tolerance
* @param source_timeout Maximum time interval in which data is considered valid
+ * @param base_shift_correction Whether to correct source data towards to base frame movement,
+ * considering the difference between current time and latest source time
*/
Scan(
const nav2_util::LifecycleNode::WeakPtr & node,
@@ -49,7 +51,8 @@ class Scan : public Source
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & source_timeout);
+ const rclcpp::Duration & source_timeout,
+ const bool base_shift_correction);
/**
* @brief Scan destructor
*/
diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp
index a24859bb4a..b7d58b6736 100644
--- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp
+++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp
@@ -46,6 +46,8 @@ class Source
* @param global_frame_id Global frame ID for correct transform calculation
* @param transform_tolerance Transform tolerance
* @param source_timeout Maximum time interval in which data is considered valid
+ * @param base_shift_correction Whether to correct source data towards to base frame movement,
+ * considering the difference between current time and latest source time
*/
Source(
const nav2_util::LifecycleNode::WeakPtr & node,
@@ -54,7 +56,8 @@ class Source
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & source_timeout);
+ const rclcpp::Duration & source_timeout,
+ const bool base_shift_correction);
/**
* @brief Source destructor
*/
@@ -125,6 +128,9 @@ class Source
tf2::Duration transform_tolerance_;
/// @brief Maximum time interval in which data is considered valid
rclcpp::Duration source_timeout_;
+ /// @brief Whether to correct source data towards to base frame movement,
+ /// considering the difference between current time and latest source time
+ bool base_shift_correction_;
}; // class Source
} // namespace nav2_collision_monitor
diff --git a/nav2_collision_monitor/package.xml b/nav2_collision_monitor/package.xml
index 7efdefac4e..67edd5606c 100644
--- a/nav2_collision_monitor/package.xml
+++ b/nav2_collision_monitor/package.xml
@@ -2,7 +2,7 @@
nav2_collision_monitor
- 1.1.6
+ 1.1.7
Collision Monitor
Alexey Merzlyakov
Steve Macenski
diff --git a/nav2_collision_monitor/params/collision_monitor_params.yaml b/nav2_collision_monitor/params/collision_monitor_params.yaml
index f0fb4ef5dd..1b0c36529e 100644
--- a/nav2_collision_monitor/params/collision_monitor_params.yaml
+++ b/nav2_collision_monitor/params/collision_monitor_params.yaml
@@ -7,6 +7,7 @@ collision_monitor:
cmd_vel_out_topic: "cmd_vel"
transform_tolerance: 0.5
source_timeout: 5.0
+ base_shift_correction: True
stop_pub_timeout: 2.0
# Polygons represent zone around the robot for "stop" and "slowdown" action types,
# and robot footprint for "approach" action type.
diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp
index 313d71fb0a..939555bf3d 100644
--- a/nav2_collision_monitor/src/collision_monitor_node.cpp
+++ b/nav2_collision_monitor/src/collision_monitor_node.cpp
@@ -205,6 +205,10 @@ bool CollisionMonitor::getParameters(
node, "source_timeout", rclcpp::ParameterValue(2.0));
source_timeout =
rclcpp::Duration::from_seconds(get_parameter("source_timeout").as_double());
+ nav2_util::declare_parameter_if_not_declared(
+ node, "base_shift_correction", rclcpp::ParameterValue(true));
+ const bool base_shift_correction =
+ get_parameter("base_shift_correction").as_bool();
nav2_util::declare_parameter_if_not_declared(
node, "stop_pub_timeout", rclcpp::ParameterValue(1.0));
@@ -215,7 +219,10 @@ bool CollisionMonitor::getParameters(
return false;
}
- if (!configureSources(base_frame_id, odom_frame_id, transform_tolerance, source_timeout)) {
+ if (
+ !configureSources(
+ base_frame_id, odom_frame_id, transform_tolerance, source_timeout, base_shift_correction))
+ {
return false;
}
@@ -271,7 +278,8 @@ bool CollisionMonitor::configureSources(
const std::string & base_frame_id,
const std::string & odom_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & source_timeout)
+ const rclcpp::Duration & source_timeout,
+ const bool base_shift_correction)
{
try {
auto node = shared_from_this();
@@ -289,7 +297,7 @@ bool CollisionMonitor::configureSources(
if (source_type == "scan") {
std::shared_ptr s = std::make_shared(
node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
- transform_tolerance, source_timeout);
+ transform_tolerance, source_timeout, base_shift_correction);
s->configure();
@@ -297,7 +305,7 @@ bool CollisionMonitor::configureSources(
} else if (source_type == "pointcloud") {
std::shared_ptr p = std::make_shared(
node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
- transform_tolerance, source_timeout);
+ transform_tolerance, source_timeout, base_shift_correction);
p->configure();
@@ -305,7 +313,7 @@ bool CollisionMonitor::configureSources(
} else if (source_type == "range") {
std::shared_ptr r = std::make_shared(
node, source_name, tf_buffer_, base_frame_id, odom_frame_id,
- transform_tolerance, source_timeout);
+ transform_tolerance, source_timeout, base_shift_correction);
r->configure();
diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp
index df4e86b63e..d4d2ea1adf 100644
--- a/nav2_collision_monitor/src/pointcloud.cpp
+++ b/nav2_collision_monitor/src/pointcloud.cpp
@@ -30,10 +30,11 @@ PointCloud::PointCloud(
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & source_timeout)
+ const rclcpp::Duration & source_timeout,
+ const bool base_shift_correction)
: Source(
node, source_name, tf_buffer, base_frame_id, global_frame_id,
- transform_tolerance, source_timeout),
+ transform_tolerance, source_timeout, base_shift_correction),
data_(nullptr)
{
RCLCPP_INFO(logger_, "[%s]: Creating PointCloud", source_name_.c_str());
@@ -75,11 +76,29 @@ void PointCloud::getData(
return;
}
- // Obtaining the transform to get data from source frame and time where it was received
- // to the base frame and current time
tf2::Transform tf_transform;
- if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) {
- return;
+ if (base_shift_correction_) {
+ // Obtaining the transform to get data from source frame and time where it was received
+ // to the base frame and current time
+ if (
+ !nav2_util::getTransform(
+ data_->header.frame_id, data_->header.stamp,
+ base_frame_id_, curr_time, global_frame_id_,
+ transform_tolerance_, tf_buffer_, tf_transform))
+ {
+ return;
+ }
+ } else {
+ // Obtaining the transform to get data from source frame to base frame without time shift
+ // considered. Less accurate but much more faster option not dependent on state estimation
+ // frames.
+ if (
+ !nav2_util::getTransform(
+ data_->header.frame_id, base_frame_id_,
+ transform_tolerance_, tf_buffer_, tf_transform))
+ {
+ return;
+ }
}
sensor_msgs::PointCloud2ConstIterator iter_x(*data_, "x");
diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp
index 3ef51e2b69..7c12a4bd52 100644
--- a/nav2_collision_monitor/src/range.cpp
+++ b/nav2_collision_monitor/src/range.cpp
@@ -30,10 +30,11 @@ Range::Range(
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & source_timeout)
+ const rclcpp::Duration & source_timeout,
+ const bool base_shift_correction)
: Source(
node, source_name, tf_buffer, base_frame_id, global_frame_id,
- transform_tolerance, source_timeout),
+ transform_tolerance, source_timeout, base_shift_correction),
data_(nullptr)
{
RCLCPP_INFO(logger_, "[%s]: Creating Range", source_name_.c_str());
@@ -84,11 +85,29 @@ void Range::getData(
return;
}
- // Obtaining the transform to get data from source frame and time where it was received
- // to the base frame and current time
tf2::Transform tf_transform;
- if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) {
- return;
+ if (base_shift_correction_) {
+ // Obtaining the transform to get data from source frame and time where it was received
+ // to the base frame and current time
+ if (
+ !nav2_util::getTransform(
+ data_->header.frame_id, data_->header.stamp,
+ base_frame_id_, curr_time, global_frame_id_,
+ transform_tolerance_, tf_buffer_, tf_transform))
+ {
+ return;
+ }
+ } else {
+ // Obtaining the transform to get data from source frame to base frame without time shift
+ // considered. Less accurate but much more faster option not dependent on state estimation
+ // frames.
+ if (
+ !nav2_util::getTransform(
+ data_->header.frame_id, base_frame_id_,
+ transform_tolerance_, tf_buffer_, tf_transform))
+ {
+ return;
+ }
}
// Calculate poses and refill data array
diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp
index d1f52b31f1..50f670cb13 100644
--- a/nav2_collision_monitor/src/scan.cpp
+++ b/nav2_collision_monitor/src/scan.cpp
@@ -27,10 +27,11 @@ Scan::Scan(
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & source_timeout)
+ const rclcpp::Duration & source_timeout,
+ const bool base_shift_correction)
: Source(
node, source_name, tf_buffer, base_frame_id, global_frame_id,
- transform_tolerance, source_timeout),
+ transform_tolerance, source_timeout, base_shift_correction),
data_(nullptr)
{
RCLCPP_INFO(logger_, "[%s]: Creating Scan", source_name_.c_str());
@@ -73,11 +74,29 @@ void Scan::getData(
return;
}
- // Obtaining the transform to get data from source frame and time where it was received
- // to the base frame and current time
tf2::Transform tf_transform;
- if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) {
- return;
+ if (base_shift_correction_) {
+ // Obtaining the transform to get data from source frame and time where it was received
+ // to the base frame and current time
+ if (
+ !nav2_util::getTransform(
+ data_->header.frame_id, data_->header.stamp,
+ base_frame_id_, curr_time, global_frame_id_,
+ transform_tolerance_, tf_buffer_, tf_transform))
+ {
+ return;
+ }
+ } else {
+ // Obtaining the transform to get data from source frame to base frame without time shift
+ // considered. Less accurate but much more faster option not dependent on state estimation
+ // frames.
+ if (
+ !nav2_util::getTransform(
+ data_->header.frame_id, base_frame_id_,
+ transform_tolerance_, tf_buffer_, tf_transform))
+ {
+ return;
+ }
}
// Calculate poses and refill data array
diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp
index bd1028518c..2b73672368 100644
--- a/nav2_collision_monitor/src/source.cpp
+++ b/nav2_collision_monitor/src/source.cpp
@@ -33,10 +33,12 @@ Source::Source(
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & source_timeout)
+ const rclcpp::Duration & source_timeout,
+ const bool base_shift_correction)
: node_(node), source_name_(source_name), tf_buffer_(tf_buffer),
base_frame_id_(base_frame_id), global_frame_id_(global_frame_id),
- transform_tolerance_(transform_tolerance), source_timeout_(source_timeout)
+ transform_tolerance_(transform_tolerance), source_timeout_(source_timeout),
+ base_shift_correction_(base_shift_correction)
{
}
diff --git a/nav2_collision_monitor/test/sources_test.cpp b/nav2_collision_monitor/test/sources_test.cpp
index 7101b61175..0bfcd1a062 100644
--- a/nav2_collision_monitor/test/sources_test.cpp
+++ b/nav2_collision_monitor/test/sources_test.cpp
@@ -172,10 +172,11 @@ class ScanWrapper : public nav2_collision_monitor::Scan
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & data_timeout)
+ const rclcpp::Duration & data_timeout,
+ const bool base_shift_correction)
: nav2_collision_monitor::Scan(
node, source_name, tf_buffer, base_frame_id, global_frame_id,
- transform_tolerance, data_timeout)
+ transform_tolerance, data_timeout, base_shift_correction)
{}
bool dataReceived() const
@@ -194,10 +195,11 @@ class PointCloudWrapper : public nav2_collision_monitor::PointCloud
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & data_timeout)
+ const rclcpp::Duration & data_timeout,
+ const bool base_shift_correction)
: nav2_collision_monitor::PointCloud(
node, source_name, tf_buffer, base_frame_id, global_frame_id,
- transform_tolerance, data_timeout)
+ transform_tolerance, data_timeout, base_shift_correction)
{}
bool dataReceived() const
@@ -216,10 +218,11 @@ class RangeWrapper : public nav2_collision_monitor::Range
const std::string & base_frame_id,
const std::string & global_frame_id,
const tf2::Duration & transform_tolerance,
- const rclcpp::Duration & data_timeout)
+ const rclcpp::Duration & data_timeout,
+ const bool base_shift_correction)
: nav2_collision_monitor::Range(
node, source_name, tf_buffer, base_frame_id, global_frame_id,
- transform_tolerance, data_timeout)
+ transform_tolerance, data_timeout, base_shift_correction)
{}
bool dataReceived() const
@@ -235,6 +238,9 @@ class Tester : public ::testing::Test
~Tester();
protected:
+ // Data sources creation routine
+ void createSources(const bool base_shift_correction = true);
+
// Setting TF chains
void sendTransforms(const rclcpp::Time & stamp);
@@ -263,7 +269,22 @@ Tester::Tester()
tf_buffer_ = std::make_shared(test_node_->get_clock());
tf_buffer_->setUsingDedicatedThread(true); // One-thread broadcasting-listening model
tf_listener_ = std::make_shared(*tf_buffer_);
+}
+
+Tester::~Tester()
+{
+ scan_.reset();
+ pointcloud_.reset();
+ range_.reset();
+ test_node_.reset();
+
+ tf_listener_.reset();
+ tf_buffer_.reset();
+}
+
+void Tester::createSources(const bool base_shift_correction)
+{
// Create Scan object
test_node_->declare_parameter(
std::string(SCAN_NAME) + ".topic", rclcpp::ParameterValue(SCAN_TOPIC));
@@ -273,7 +294,7 @@ Tester::Tester()
scan_ = std::make_shared(
test_node_, SCAN_NAME, tf_buffer_,
BASE_FRAME_ID, GLOBAL_FRAME_ID,
- TRANSFORM_TOLERANCE, DATA_TIMEOUT);
+ TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction);
scan_->configure();
// Create PointCloud object
@@ -293,7 +314,7 @@ Tester::Tester()
pointcloud_ = std::make_shared(
test_node_, POINTCLOUD_NAME, tf_buffer_,
BASE_FRAME_ID, GLOBAL_FRAME_ID,
- TRANSFORM_TOLERANCE, DATA_TIMEOUT);
+ TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction);
pointcloud_->configure();
// Create Range object
@@ -308,22 +329,10 @@ Tester::Tester()
range_ = std::make_shared(
test_node_, RANGE_NAME, tf_buffer_,
BASE_FRAME_ID, GLOBAL_FRAME_ID,
- TRANSFORM_TOLERANCE, DATA_TIMEOUT);
+ TRANSFORM_TOLERANCE, DATA_TIMEOUT, base_shift_correction);
range_->configure();
}
-Tester::~Tester()
-{
- scan_.reset();
- pointcloud_.reset();
- range_.reset();
-
- test_node_.reset();
-
- tf_listener_.reset();
- tf_buffer_.reset();
-}
-
void Tester::sendTransforms(const rclcpp::Time & stamp)
{
std::shared_ptr tf_broadcaster =
@@ -453,6 +462,8 @@ TEST_F(Tester, testGetData)
{
rclcpp::Time curr_time = test_node_->now();
+ createSources();
+
sendTransforms(curr_time);
// Publish data for sources
@@ -485,6 +496,8 @@ TEST_F(Tester, testGetOutdatedData)
{
rclcpp::Time curr_time = test_node_->now();
+ createSources();
+
sendTransforms(curr_time);
// Publish outdated data for sources
@@ -515,6 +528,8 @@ TEST_F(Tester, testIncorrectFrameData)
{
rclcpp::Time curr_time = test_node_->now();
+ createSources();
+
// Send incorrect transform
sendTransforms(curr_time - 1s);
@@ -546,6 +561,8 @@ TEST_F(Tester, testIncorrectData)
{
rclcpp::Time curr_time = test_node_->now();
+ createSources();
+
sendTransforms(curr_time);
// Publish data for sources
@@ -567,6 +584,41 @@ TEST_F(Tester, testIncorrectData)
ASSERT_EQ(data.size(), 0u);
}
+TEST_F(Tester, testIgnoreTimeShift)
+{
+ rclcpp::Time curr_time = test_node_->now();
+
+ createSources(false);
+
+ // Send incorrect transform
+ sendTransforms(curr_time - 1s);
+
+ // Publish data for sources
+ test_node_->publishScan(curr_time, 1.0);
+ test_node_->publishPointCloud(curr_time);
+ test_node_->publishRange(curr_time, 1.0);
+
+ // Wait until all sources will receive the data
+ ASSERT_TRUE(waitScan(500ms));
+ ASSERT_TRUE(waitPointCloud(500ms));
+ ASSERT_TRUE(waitRange(500ms));
+
+ // Scan data should be consistent
+ std::vector data;
+ scan_->getData(curr_time, data);
+ checkScan(data);
+
+ // Pointcloud data should be consistent
+ data.clear();
+ pointcloud_->getData(curr_time, data);
+ checkPointCloud(data);
+
+ // Range data should be consistent
+ data.clear();
+ range_->getData(curr_time, data);
+ checkRange(data);
+}
+
int main(int argc, char ** argv)
{
// Initialize the system
diff --git a/nav2_common/nav2_common/launch/rewritten_yaml.py b/nav2_common/nav2_common/launch/rewritten_yaml.py
index 63d3905d5e..3b29fdda59 100644
--- a/nav2_common/nav2_common/launch/rewritten_yaml.py
+++ b/nav2_common/nav2_common/launch/rewritten_yaml.py
@@ -161,7 +161,7 @@ def pathify(self, d, p=None, paths=None, joinchar='.'):
if isinstance(d, dict):
for k in d:
v = d[k]
- self.pathify(v, pn + k, paths, joinchar=joinchar)
+ self.pathify(v, str(pn) + str(k), paths, joinchar=joinchar)
elif isinstance(d, list):
for idx, e in enumerate(d):
self.pathify(e, pn + str(idx), paths, joinchar=joinchar)
diff --git a/nav2_common/package.xml b/nav2_common/package.xml
index 9056c712cf..8ef6b4cf6f 100644
--- a/nav2_common/package.xml
+++ b/nav2_common/package.xml
@@ -2,7 +2,7 @@
nav2_common
- 1.1.6
+ 1.1.7
Common support functionality used throughout the navigation 2 stack
Carl Delsey
Apache-2.0
diff --git a/nav2_constrained_smoother/CMakeLists.txt b/nav2_constrained_smoother/CMakeLists.txt
index 0d0cb497ae..6bc4e7dc6e 100644
--- a/nav2_constrained_smoother/CMakeLists.txt
+++ b/nav2_constrained_smoother/CMakeLists.txt
@@ -17,6 +17,10 @@ find_package(Ceres REQUIRED COMPONENTS SuiteSparse)
set(CMAKE_CXX_STANDARD 17)
+if(${CERES_VERSION} VERSION_LESS_EQUAL 2.0.0)
+ add_definitions(-DUSE_OLD_CERES_API)
+endif()
+
nav2_package()
set(library_name nav2_constrained_smoother)
diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp
index c0c3919ccc..7253119721 100644
--- a/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp
+++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/smoother_cost_function.hpp
@@ -159,7 +159,7 @@ class SmootherCostFunction
Eigen::Matrix center = arcCenter(
pt_prev, pt, pt_next,
next_to_last_length_ratio_ < 0);
- if (ceres::isinf(center[0])) {
+ if (CERES_ISINF(center[0])) {
return;
}
T turning_rad = (pt - center).norm();
diff --git a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp
index 682ed1c16b..fb4e2aa130 100644
--- a/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp
+++ b/nav2_constrained_smoother/include/nav2_constrained_smoother/utils.hpp
@@ -20,6 +20,16 @@
#define EPSILON 0.0001
+/**
+ * Compatibility with different ceres::isinf() and ceres::IsInfinite() API
+ * used in Ceres Solver 2.1.0+ and 2.0.0- versions respectively
+ */
+#if defined(USE_OLD_CERES_API)
+ #define CERES_ISINF(x) ceres::IsInfinite(x)
+#else
+ #define CERES_ISINF(x) ceres::isinf(x)
+#endif
+
namespace nav2_constrained_smoother
{
@@ -86,7 +96,7 @@ inline Eigen::Matrix tangentDir(
bool is_cusp)
{
Eigen::Matrix center = arcCenter(pt_prev, pt, pt_next, is_cusp);
- if (ceres::isinf(center[0])) { // straight line
+ if (CERES_ISINF(center[0])) { // straight line
Eigen::Matrix d1 = pt - pt_prev;
Eigen::Matrix d2 = pt_next - pt;
diff --git a/nav2_constrained_smoother/package.xml b/nav2_constrained_smoother/package.xml
index 837c67ec16..b20c946bf7 100644
--- a/nav2_constrained_smoother/package.xml
+++ b/nav2_constrained_smoother/package.xml
@@ -2,7 +2,7 @@
nav2_constrained_smoother
- 1.1.6
+ 1.1.7
Ceres constrained smoother
Matej Vargovcik
Steve Macenski
diff --git a/nav2_controller/CMakeLists.txt b/nav2_controller/CMakeLists.txt
index e5ea991c34..ded67fbf41 100644
--- a/nav2_controller/CMakeLists.txt
+++ b/nav2_controller/CMakeLists.txt
@@ -50,6 +50,10 @@ set(dependencies
add_library(simple_progress_checker SHARED plugins/simple_progress_checker.cpp)
ament_target_dependencies(simple_progress_checker ${dependencies})
+add_library(pose_progress_checker SHARED plugins/pose_progress_checker.cpp)
+target_link_libraries(pose_progress_checker simple_progress_checker)
+ament_target_dependencies(pose_progress_checker ${dependencies})
+
add_library(simple_goal_checker SHARED plugins/simple_goal_checker.cpp)
ament_target_dependencies(simple_goal_checker ${dependencies})
@@ -79,7 +83,7 @@ target_link_libraries(${executable_name} ${library_name})
rclcpp_components_register_nodes(${library_name} "nav2_controller::ControllerServer")
-install(TARGETS simple_progress_checker simple_goal_checker stopped_goal_checker ${library_name}
+install(TARGETS simple_progress_checker pose_progress_checker simple_goal_checker stopped_goal_checker ${library_name}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
@@ -102,6 +106,7 @@ endif()
ament_export_include_directories(include)
ament_export_libraries(simple_progress_checker
+ pose_progress_checker
simple_goal_checker
stopped_goal_checker
${library_name})
diff --git a/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp
new file mode 100644
index 0000000000..3db219eaae
--- /dev/null
+++ b/nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp
@@ -0,0 +1,67 @@
+// Copyright (c) 2023 Dexory
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_
+
+#include
+#include
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_controller/plugins/simple_progress_checker.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+
+namespace nav2_controller
+{
+/**
+* @class PoseProgressChecker
+* @brief This plugin is used to check the position and the angle of the robot to make sure
+* that it is actually progressing or rotating towards a goal.
+*/
+
+class PoseProgressChecker : public SimpleProgressChecker
+{
+public:
+ void initialize(
+ const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+ const std::string & plugin_name) override;
+ bool check(geometry_msgs::msg::PoseStamped & current_pose) override;
+
+protected:
+ /**
+ * @brief Calculates robots movement from baseline pose
+ * @param pose Current pose of the robot
+ * @return true, if movement is greater than radius_, or false
+ */
+ bool isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose);
+
+ static double poseAngleDistance(
+ const geometry_msgs::msg::Pose2D &,
+ const geometry_msgs::msg::Pose2D &);
+
+ double required_movement_angle_;
+
+ // Dynamic parameters handler
+ rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+ std::string plugin_name_;
+
+ /**
+ * @brief Callback executed when a paramter change is detected
+ * @param parameters list of changed parameters
+ */
+ rcl_interfaces::msg::SetParametersResult
+ dynamicParametersCallback(std::vector parameters);
+};
+} // namespace nav2_controller
+
+#endif // NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_
diff --git a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp
index 656d67fb30..b86a7018e5 100644
--- a/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp
+++ b/nav2_controller/include/nav2_controller/plugins/simple_progress_checker.hpp
@@ -46,12 +46,16 @@ class SimpleProgressChecker : public nav2_core::ProgressChecker
* @param pose Current pose of the robot
* @return true, if movement is greater than radius_, or false
*/
- bool is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose);
+ bool isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose);
/**
* @brief Resets baseline pose with the current pose of the robot
* @param pose Current pose of the robot
*/
- void reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose);
+ void resetBaselinePose(const geometry_msgs::msg::Pose2D & pose);
+
+ static double pose_distance(
+ const geometry_msgs::msg::Pose2D &,
+ const geometry_msgs::msg::Pose2D &);
rclcpp::Clock::SharedPtr clock_;
diff --git a/nav2_controller/package.xml b/nav2_controller/package.xml
index 3c6d8f8626..14bfa0e04e 100644
--- a/nav2_controller/package.xml
+++ b/nav2_controller/package.xml
@@ -2,7 +2,7 @@
nav2_controller
- 1.1.6
+ 1.1.7
Controller action interface
Carl Delsey
Apache-2.0
diff --git a/nav2_controller/plugins.xml b/nav2_controller/plugins.xml
index ad27127c03..ffbd0e5f1c 100644
--- a/nav2_controller/plugins.xml
+++ b/nav2_controller/plugins.xml
@@ -4,6 +4,11 @@
Checks if distance between current and previous pose is above a threshold
+
+
+ Checks if distance and angle between current and previous pose is above a threshold
+
+
Checks if current pose is within goal window for x,y and yaw
diff --git a/nav2_controller/plugins/pose_progress_checker.cpp b/nav2_controller/plugins/pose_progress_checker.cpp
new file mode 100644
index 0000000000..271e5635c0
--- /dev/null
+++ b/nav2_controller/plugins/pose_progress_checker.cpp
@@ -0,0 +1,97 @@
+// Copyright (c) 2023 Dexory
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "nav2_controller/plugins/pose_progress_checker.hpp"
+#include
+#include
+#include
+#include
+#include "angles/angles.h"
+#include "nav_2d_utils/conversions.hpp"
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+using rcl_interfaces::msg::ParameterType;
+using std::placeholders::_1;
+
+namespace nav2_controller
+{
+
+void PoseProgressChecker::initialize(
+ const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+ const std::string & plugin_name)
+{
+ plugin_name_ = plugin_name;
+ SimpleProgressChecker::initialize(parent, plugin_name);
+ auto node = parent.lock();
+
+ nav2_util::declare_parameter_if_not_declared(
+ node, plugin_name + ".required_movement_angle", rclcpp::ParameterValue(0.5));
+ node->get_parameter_or(plugin_name + ".required_movement_angle", required_movement_angle_, 0.5);
+
+ // Add callback for dynamic parameters
+ dyn_params_handler_ = node->add_on_set_parameters_callback(
+ std::bind(&PoseProgressChecker::dynamicParametersCallback, this, _1));
+}
+
+bool PoseProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose)
+{
+ // relies on short circuit evaluation to not call is_robot_moved_enough if
+ // baseline_pose is not set.
+ geometry_msgs::msg::Pose2D current_pose2d;
+ current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose);
+
+ if (!baseline_pose_set_ || PoseProgressChecker::isRobotMovedEnough(current_pose2d)) {
+ resetBaselinePose(current_pose2d);
+ return true;
+ }
+ return clock_->now() - baseline_time_ <= time_allowance_;
+}
+
+bool PoseProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose)
+{
+ return pose_distance(pose, baseline_pose_) > radius_ ||
+ poseAngleDistance(pose, baseline_pose_) > required_movement_angle_;
+}
+
+double PoseProgressChecker::poseAngleDistance(
+ const geometry_msgs::msg::Pose2D & pose1,
+ const geometry_msgs::msg::Pose2D & pose2)
+{
+ return abs(angles::shortest_angular_distance(pose1.theta, pose2.theta));
+}
+
+rcl_interfaces::msg::SetParametersResult
+PoseProgressChecker::dynamicParametersCallback(std::vector parameters)
+{
+ rcl_interfaces::msg::SetParametersResult result;
+ for (auto parameter : parameters) {
+ const auto & type = parameter.get_type();
+ const auto & name = parameter.get_name();
+
+ if (type == ParameterType::PARAMETER_DOUBLE) {
+ if (name == plugin_name_ + ".required_movement_angle") {
+ required_movement_angle_ = parameter.as_double();
+ }
+ }
+ }
+ result.successful = true;
+ return result;
+}
+
+} // namespace nav2_controller
+
+PLUGINLIB_EXPORT_CLASS(nav2_controller::PoseProgressChecker, nav2_core::ProgressChecker)
diff --git a/nav2_controller/plugins/simple_progress_checker.cpp b/nav2_controller/plugins/simple_progress_checker.cpp
index 0b2ec3e739..120b4320da 100644
--- a/nav2_controller/plugins/simple_progress_checker.cpp
+++ b/nav2_controller/plugins/simple_progress_checker.cpp
@@ -28,8 +28,6 @@ using std::placeholders::_1;
namespace nav2_controller
{
-static double pose_distance(const geometry_msgs::msg::Pose2D &, const geometry_msgs::msg::Pose2D &);
-
void SimpleProgressChecker::initialize(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
const std::string & plugin_name)
@@ -61,8 +59,8 @@ bool SimpleProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose
geometry_msgs::msg::Pose2D current_pose2d;
current_pose2d = nav_2d_utils::poseToPose2D(current_pose.pose);
- if ((!baseline_pose_set_) || (is_robot_moved_enough(current_pose2d))) {
- reset_baseline_pose(current_pose2d);
+ if ((!baseline_pose_set_) || (isRobotMovedEnough(current_pose2d))) {
+ resetBaselinePose(current_pose2d);
return true;
}
return !((clock_->now() - baseline_time_) > time_allowance_);
@@ -73,19 +71,19 @@ void SimpleProgressChecker::reset()
baseline_pose_set_ = false;
}
-void SimpleProgressChecker::reset_baseline_pose(const geometry_msgs::msg::Pose2D & pose)
+void SimpleProgressChecker::resetBaselinePose(const geometry_msgs::msg::Pose2D & pose)
{
baseline_pose_ = pose;
baseline_time_ = clock_->now();
baseline_pose_set_ = true;
}
-bool SimpleProgressChecker::is_robot_moved_enough(const geometry_msgs::msg::Pose2D & pose)
+bool SimpleProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose)
{
return pose_distance(pose, baseline_pose_) > radius_;
}
-static double pose_distance(
+double SimpleProgressChecker::pose_distance(
const geometry_msgs::msg::Pose2D & pose1,
const geometry_msgs::msg::Pose2D & pose2)
{
diff --git a/nav2_controller/plugins/test/CMakeLists.txt b/nav2_controller/plugins/test/CMakeLists.txt
index d4f34d3918..0226676ce3 100644
--- a/nav2_controller/plugins/test/CMakeLists.txt
+++ b/nav2_controller/plugins/test/CMakeLists.txt
@@ -1,4 +1,4 @@
ament_add_gtest(pctest progress_checker.cpp)
-target_link_libraries(pctest simple_progress_checker)
+target_link_libraries(pctest simple_progress_checker pose_progress_checker)
ament_add_gtest(gctest goal_checker.cpp)
target_link_libraries(gctest simple_goal_checker stopped_goal_checker)
diff --git a/nav2_controller/plugins/test/goal_checker.cpp b/nav2_controller/plugins/test/goal_checker.cpp
index 387bfcc735..acfffcb9cd 100644
--- a/nav2_controller/plugins/test/goal_checker.cpp
+++ b/nav2_controller/plugins/test/goal_checker.cpp
@@ -145,6 +145,16 @@ TEST(VelocityIterator, goal_checker_reset)
EXPECT_TRUE(true);
}
+TEST(VelocityIterator, stopped_goal_checker_reset)
+{
+ auto x = std::make_shared("stopped_goal_checker");
+
+ nav2_core::GoalChecker * sgc = new StoppedGoalChecker;
+ sgc->reset();
+ delete sgc;
+ EXPECT_TRUE(true);
+}
+
TEST(VelocityIterator, two_checks)
{
auto x = std::make_shared("goal_checker");
diff --git a/nav2_controller/plugins/test/progress_checker.cpp b/nav2_controller/plugins/test/progress_checker.cpp
index 418ff1032a..0f42561447 100644
--- a/nav2_controller/plugins/test/progress_checker.cpp
+++ b/nav2_controller/plugins/test/progress_checker.cpp
@@ -37,10 +37,13 @@
#include "gtest/gtest.h"
#include "nav2_controller/plugins/simple_progress_checker.hpp"
+#include "nav2_controller/plugins/pose_progress_checker.hpp"
#include "nav_2d_utils/conversions.hpp"
#include "nav2_util/lifecycle_node.hpp"
+#include "nav2_util/geometry_utils.hpp"
using nav2_controller::SimpleProgressChecker;
+using nav2_controller::PoseProgressChecker;
class TestLifecycleNode : public nav2_util::LifecycleNode
{
@@ -83,8 +86,8 @@ class TestLifecycleNode : public nav2_util::LifecycleNode
void checkMacro(
nav2_core::ProgressChecker & pc,
- double x0, double y0,
- double x1, double y1,
+ double x0, double y0, double theta0,
+ double x1, double y1, double theta1,
int delay,
bool expected_result)
{
@@ -92,10 +95,12 @@ void checkMacro(
geometry_msgs::msg::PoseStamped pose0, pose1;
pose0.pose.position.x = x0;
pose0.pose.position.y = y0;
+ pose0.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta0);
pose1.pose.position.x = x1;
pose1.pose.position.y = y1;
+ pose1.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta1);
EXPECT_TRUE(pc.check(pose0));
- rclcpp::sleep_for(std::chrono::seconds(delay));
+ rclcpp::sleep_for(std::chrono::milliseconds(delay));
if (expected_result) {
EXPECT_TRUE(pc.check(pose1));
} else {
@@ -119,12 +124,116 @@ TEST(SimpleProgressChecker, unit_tests)
SimpleProgressChecker pc;
pc.initialize(x, "nav2_controller");
- checkMacro(pc, 0, 0, 0, 0, 1, true);
- checkMacro(pc, 0, 0, 1, 0, 1, true);
- checkMacro(pc, 0, 0, 0, 1, 1, true);
- checkMacro(pc, 0, 0, 1, 0, 11, true);
- checkMacro(pc, 0, 0, 0, 1, 11, true);
- checkMacro(pc, 0, 0, 0, 0, 11, false);
+
+ double time_allowance = 0.5;
+ int half_time_allowance_ms = static_cast(time_allowance * 0.5 * 1000);
+ int twice_time_allowance_ms = static_cast(time_allowance * 2.0 * 1000);
+
+ auto rec_param = std::make_shared(
+ x->get_node_base_interface(), x->get_node_topics_interface(),
+ x->get_node_graph_interface(),
+ x->get_node_services_interface());
+
+ auto results = rec_param->set_parameters_atomically(
+ {rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)});
+
+ rclcpp::spin_until_future_complete(
+ x->get_node_base_interface(),
+ results);
+
+ EXPECT_EQ(
+ x->get_parameter("nav2_controller.movement_time_allowance").as_double(),
+ time_allowance);
+
+ // BELOW time allowance (set to time_allowance)
+ // no movement
+ checkMacro(pc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true);
+ // translation below required_movement_radius (default 0.5)
+ checkMacro(pc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true);
+ checkMacro(pc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true);
+ // translation above required_movement_radius (default 0.5)
+ checkMacro(pc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true);
+ checkMacro(pc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true);
+
+ // ABOVE time allowance (set to time_allowance)
+ // no movement
+ checkMacro(pc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false);
+ // translation below required_movement_radius (default 0.5)
+ checkMacro(pc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false);
+ checkMacro(pc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false);
+ // translation above required_movement_radius (default 0.5)
+ checkMacro(pc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true);
+ checkMacro(pc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true);
+}
+
+TEST(PoseProgressChecker, pose_progress_checker_reset)
+{
+ auto x = std::make_shared("pose_progress_checker");
+
+ PoseProgressChecker * rpc = new PoseProgressChecker;
+ rpc->reset();
+ delete rpc;
+ EXPECT_TRUE(true);
+}
+
+TEST(PoseProgressChecker, unit_tests)
+{
+ auto x = std::make_shared("pose_progress_checker");
+
+ PoseProgressChecker rpc;
+ rpc.initialize(x, "nav2_controller");
+
+ double time_allowance = 0.5;
+ int half_time_allowance_ms = static_cast(time_allowance * 0.5 * 1000);
+ int twice_time_allowance_ms = static_cast(time_allowance * 2.0 * 1000);
+
+ auto rec_param = std::make_shared(
+ x->get_node_base_interface(), x->get_node_topics_interface(),
+ x->get_node_graph_interface(),
+ x->get_node_services_interface());
+
+ auto results = rec_param->set_parameters_atomically(
+ {rclcpp::Parameter("nav2_controller.movement_time_allowance", time_allowance)});
+
+ rclcpp::spin_until_future_complete(
+ x->get_node_base_interface(),
+ results);
+
+ EXPECT_EQ(
+ x->get_parameter("nav2_controller.movement_time_allowance").as_double(),
+ time_allowance);
+
+ // BELOW time allowance (set to time_allowance)
+ // no movement
+ checkMacro(rpc, 0, 0, 0, 0, 0, 0, half_time_allowance_ms, true);
+ // translation below required_movement_radius (default 0.5)
+ checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, half_time_allowance_ms, true);
+ checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, half_time_allowance_ms, true);
+ // rotation below required_movement_angle (default 0.5)
+ checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, half_time_allowance_ms, true);
+ checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, half_time_allowance_ms, true);
+ // translation above required_movement_radius (default 0.5)
+ checkMacro(rpc, 0, 0, 0, 1, 0, 0, half_time_allowance_ms, true);
+ checkMacro(rpc, 0, 0, 0, 0, 1, 0, half_time_allowance_ms, true);
+ // rotation above required_movement_angle (default 0.5)
+ checkMacro(rpc, 0, 0, 0, 0, 0, 1, half_time_allowance_ms, true);
+ checkMacro(rpc, 0, 0, 0, 0, 0, -1, half_time_allowance_ms, true);
+
+ // ABOVE time allowance (set to time_allowance)
+ // no movement
+ checkMacro(rpc, 0, 0, 0, 0, 0, 0, twice_time_allowance_ms, false);
+ // translation below required_movement_radius (default 0.5)
+ checkMacro(rpc, 0, 0, 0, 0.25, 0, 0, twice_time_allowance_ms, false);
+ checkMacro(rpc, 0, 0, 0, 0, 0.25, 0, twice_time_allowance_ms, false);
+ // rotation below required_movement_angle (default 0.5)
+ checkMacro(rpc, 0, 0, 0, 0, 0, 0.25, twice_time_allowance_ms, false);
+ checkMacro(rpc, 0, 0, 0, 0, 0, -0.25, twice_time_allowance_ms, false);
+ // translation above required_movement_radius (default 0.5)
+ checkMacro(rpc, 0, 0, 0, 1, 0, 0, twice_time_allowance_ms, true);
+ checkMacro(rpc, 0, 0, 0, 0, 1, 0, twice_time_allowance_ms, true);
+ // rotation above required_movement_angle (default 0.5)
+ checkMacro(rpc, 0, 0, 0, 0, 0, 1, twice_time_allowance_ms, true);
+ checkMacro(rpc, 0, 0, 0, 0, 0, -1, twice_time_allowance_ms, true);
}
int main(int argc, char ** argv)
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
index f7682bf13c..4747035983 100644
--- a/nav2_controller/src/controller_server.cpp
+++ b/nav2_controller/src/controller_server.cpp
@@ -123,6 +123,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
get_parameter("failure_tolerance", failure_tolerance_);
costmap_ros_->configure();
+ // Launch a thread to run the costmap node
+ costmap_thread_ = std::make_unique(costmap_ros_);
try {
progress_checker_type_ = nav2_util::get_plugin_type_param(node, progress_checker_id_);
diff --git a/nav2_core/package.xml b/nav2_core/package.xml
index 39883326bf..f26b2b6a59 100644
--- a/nav2_core/package.xml
+++ b/nav2_core/package.xml
@@ -2,7 +2,7 @@
nav2_core
- 1.1.6
+ 1.1.7
A set of headers for plugins core to the Nav2 stack
Steve Macenski
Carl Delsey
diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt
index 683cb04fdd..f039cf5d42 100644
--- a/nav2_costmap_2d/CMakeLists.txt
+++ b/nav2_costmap_2d/CMakeLists.txt
@@ -87,6 +87,7 @@ add_library(layers SHARED
src/observation_buffer.cpp
plugins/voxel_layer.cpp
plugins/range_sensor_layer.cpp
+ plugins/denoise_layer.cpp
)
ament_target_dependencies(layers
${dependencies}
diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml
index e5867d5108..6748cd5fca 100644
--- a/nav2_costmap_2d/costmap_plugins.xml
+++ b/nav2_costmap_2d/costmap_plugins.xml
@@ -15,6 +15,9 @@
A range-sensor (sonar, IR) based obstacle layer for costmap_2d
+
+ Filters noise-induced freestanding obstacles or small obstacles groups
+
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
index eb1630be26..c336d3c091 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp
@@ -2,6 +2,7 @@
*
* Software License Agreement (BSD License)
*
+ * Copyright (c) 2008, 2013, Willow Garage, Inc.
* Copyright (c) 2020 Samsung Research Russia
* All rights reserved.
*
@@ -32,7 +33,9 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
- * Author: Alexey Merzlyakov
+ * Author: Eitan Marder-Eppstein
+ * David V. Lu!!
+ * Alexey Merzlyakov
*********************************************************************/
#ifndef NAV2_COSTMAP_2D__COSTMAP_FILTERS__COSTMAP_FILTER_HPP_
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp
new file mode 100644
index 0000000000..db7ae8fce8
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp
@@ -0,0 +1,207 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_COSTMAP_2D__DENOISE__IMAGE_HPP_
+#define NAV2_COSTMAP_2D__DENOISE__IMAGE_HPP_
+
+#include
+#include
+
+namespace nav2_costmap_2d
+{
+
+/**
+ * @brief Image with pixels of type T
+ * Сan own data, be a wrapper over some memory buffer, or refer to a fragment of another image
+ * Pixels of one row are stored continuity. But rows continuity is not guaranteed.
+ * The distance (number of elements of type T) from row(i) to row(i + 1) is equal to step()
+ * @tparam T type of pixel
+ */
+template
+class Image
+{
+public:
+ /// @brief Create empty image
+ Image() = default;
+
+ /**
+ * @brief Create image referencing to a third-party buffer
+ * @param rows number of image rows
+ * @param columns number of image columns
+ * @param data existing memory buffer with size at least rows * columns
+ * @param step offset from row(i) to row(i + 1) in memory buffer (number of elements of type T).
+ * offset = columns if rows are stored continuity
+ */
+ Image(size_t rows, size_t columns, T * data, size_t step);
+
+ /**
+ * @brief Create image referencing to the other
+ * Share image data between new and old object.
+ * Changing data in a new object will affect the given one and vice versa
+ */
+ Image(Image & other);
+
+ /**
+ * @brief Create image from the other (move constructor)
+ */
+ Image(Image && other) noexcept;
+
+ /// @return number of image rows
+ size_t rows() const {return rows_;}
+
+ /// @return number of image columns
+ size_t columns() const {return columns_;}
+
+ /// @return true if image empty
+ bool empty() const {return rows_ == 0 || columns_ == 0;}
+
+ /// @return offset (number of elements of type T) from row(i) to row(i + 1)
+ size_t step() const {return step_;}
+
+ /**
+ * @return pointer to first pixel of row
+ * @warning If row >= rows(), the behavior is undefined
+ */
+ T * row(size_t row);
+
+ /// @overload
+ const T * row(size_t row) const;
+
+ /**
+ * @brief Read (and modify, if need) each pixel sequentially
+ * @tparam Functor function object.
+ * Signature should be equivalent to the following:
+ * void fn(T& pixel) or void fn(const T& pixel)
+ * @param fn a function that will be applied to each pixel in the image. Can modify image data
+ */
+ template
+ void forEach(Functor && fn);
+
+ /**
+ * @brief Read each pixel sequentially
+ * @tparam Functor function object.
+ * Signature should be equivalent to the following:
+ * void fn(const T& pixel)
+ * @param fn a function that will be applied to each pixel in the image
+ */
+ template
+ void forEach(Functor && fn) const;
+ /**
+ * @brief Convert each pixel to corresponding pixel of target using a custom function
+ *
+ * The source and target must be the same size.
+ * For calculation of new target value the operation can use source value and
+ * an optionally current target value.
+ * This function call operation(this[i, j], target[i, j]) for each pixel
+ * where target[i, j] is mutable
+ * @tparam TargetElement type of target pixel
+ * @tparam Converter function object.
+ * Signature should be equivalent to the following:
+ * void fn(const T& src, TargetElement& trg)
+ * @param target output image with TargetElement-type pixels
+ * @param operation the binary operation op is applied to pairs of pixels:
+ * first (const) from source and second (mutable) from target
+ * @throw std::logic_error if the source and target of different sizes
+ */
+ template
+ void convert(Image & target, Converter && converter) const;
+
+private:
+ T * data_start_{};
+ size_t rows_{};
+ size_t columns_{};
+ size_t step_{};
+};
+
+template
+Image::Image(size_t rows, size_t columns, T * data, size_t step)
+: rows_{rows}, columns_{columns}, step_{step}
+{
+ data_start_ = data;
+}
+
+template
+Image::Image(Image & other)
+: data_start_{other.data_start_},
+ rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {}
+
+template
+Image::Image(Image && other) noexcept
+: data_start_{other.data_start_},
+ rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {}
+
+template
+T * Image::row(size_t row)
+{
+ return const_cast( static_cast &>(*this).row(row) );
+}
+
+template
+const T * Image::row(size_t row) const
+{
+ return data_start_ + row * step_;
+}
+
+template
+template
+void Image::forEach(Functor && fn)
+{
+ static_cast &>(*this).forEach(
+ [&](const T & pixel) {
+ fn(const_cast(pixel));
+ });
+}
+
+template
+template
+void Image::forEach(Functor && fn) const
+{
+ const T * rowPtr = row(0);
+
+ for (size_t row = 0; row < rows(); ++row) {
+ const T * rowEnd = rowPtr + columns();
+
+ for (const T * pixel = rowPtr; pixel != rowEnd; ++pixel) {
+ fn(*pixel);
+ }
+ rowPtr += step();
+ }
+}
+
+template
+template
+void Image::convert(Image & target, Converter && converter) const
+{
+ if (rows() != target.rows() || columns() != target.columns()) {
+ throw std::logic_error("Image::convert. The source and target images size are different");
+ }
+ const T * source_row = row(0);
+ TargetElement * target_row = target.row(0);
+
+ for (size_t row = 0; row < rows(); ++row) {
+ const T * rowInEnd = source_row + columns();
+ const T * src = source_row;
+ TargetElement * trg = target_row;
+
+ for (; src != rowInEnd; ++src, ++trg) {
+ converter(*src, *trg);
+ }
+ source_row += step();
+ target_row += target.step();
+ }
+}
+
+} // namespace nav2_costmap_2d
+
+#endif // NAV2_COSTMAP_2D__DENOISE__IMAGE_HPP_
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp
new file mode 100644
index 0000000000..ee79894847
--- /dev/null
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp
@@ -0,0 +1,1051 @@
+// Copyright (c) 2023 Andrey Ryzhikov
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_COSTMAP_2D__DENOISE__IMAGE_PROCESSING_HPP_
+#define NAV2_COSTMAP_2D__DENOISE__IMAGE_PROCESSING_HPP_
+
+#include "image.hpp"
+#include
+#include
+#include
+#include
+#include
+
+namespace nav2_costmap_2d
+{
+
+/**
+ * @enum nav2_costmap_2d::ConnectivityType
+ * @brief Describes the type of pixel connectivity (is the way in which
+ * pixels in image relate to their neighbors)
+ */
+enum class ConnectivityType : int
+{
+ /// neighbors pixels are connected horizontally and vertically
+ Way4 = 4,
+ /// neighbors pixels are connected horizontally, vertically and diagonally
+ Way8 = 8
+};
+
+/**
+ * @brief A memory buffer that can grow to an upper-bounded capacity
+ */
+class MemoryBuffer
+{
+public:
+ /// @brief Free memory allocated for the buffer
+ inline ~MemoryBuffer() {reset();}
+ /**
+ * @brief Return a pointer to an uninitialized array of count elements
+ * Delete the old block of memory and allocates a new one if the size of the old is too small.
+ * The returned pointer is valid until the next call to get() or destructor.
+ * @tparam T type of element
+ * @param count number of elements
+ * @throw std::bad_alloc or any other exception thrown by allocator
+ */
+ template
+ T * get(std::size_t count);
+
+private:
+ inline void reset();
+ inline void allocate(size_t bytes);
+
+private:
+ void * data_{};
+ size_t size_{};
+};
+
+// forward declarations
+namespace imgproc_impl
+{
+template
+class EquivalenceLabelTrees;
+
+template
+void morphologyOperation(
+ const Image & input, Image & output,
+ const Image & shape, AggregateFn aggregate);
+
+using ShapeBuffer3x3 = std::array;
+inline Image createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity);
+} // namespace imgproc_impl
+
+/**
+ * @brief Perform morphological dilation
+ * @tparam Max function object
+ * @param input input image
+ * @param output output image
+ * @param connectivity selector for selecting structuring element (Way4-> cross, Way8-> rect)
+ * @param max_function takes as input std::initializer_list with three elements.
+ * Returns the greatest value in list
+ */
+template
+inline void dilate(
+ const Image & input, Image & output,
+ ConnectivityType connectivity, Max && max_function)
+{
+ using namespace imgproc_impl;
+ ShapeBuffer3x3 shape_buffer;
+ Image shape = createShape(shape_buffer, connectivity);
+ morphologyOperation(input, output, shape, max_function);
+}
+
+/**
+* @brief Compute the connected components labeled image of binary image
+* Implements the SAUF algorithm
+* (Two Strategies to Speed up Connected Component Labeling Algorithms
+* Kesheng Wu, Ekow Otoo, Kenji Suzuki).
+* @tparam connectivity pixels connectivity type
+* @tparam Label integer type of label
+* @tparam IsBg functor with signature bool (uint8_t)
+* @param image input image
+* @param buffer memory block that will be used to store the result (labeled image)
+* and the internal buffer for labels trees
+* @param label_trees union-find data structure
+* @param is_background returns true if the passed pixel value is background
+* @throw LabelOverflow if all possible values of the Label type are used and
+* it is impossible to create a new unique
+* @return pair(labeled image, total number of labels)
+* Labeled image has the same size as image. Label 0 represents the background label,
+* labels [1, - 1] - separate components.
+* Total number of labels == 0 for empty image.
+* In other cases, label 0 is always counted,
+* even if there is no background in the image.
+* For example, for an image of one background pixel, the total number of labels == 2.
+* Two labels (0, 1) have been counted, although label 0 is not used)
+*/
+template
+std::pair, Label> connectedComponents(
+ const Image & image, MemoryBuffer & buffer,
+ imgproc_impl::EquivalenceLabelTrees
+
](/foxglove/autoconnect)
](/gzweb/)
](/glances/)
](/nav2/)