diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp
index 7db27064694831d14d438f82f06c86d9aa1afc27..8c468614973deff785fbd4ebca5808ff6f47627d 100644
--- a/nav2_bt_navigator/src/bt_navigator.cpp
+++ b/nav2_bt_navigator/src/bt_navigator.cpp
@@ -31,8 +31,7 @@ BtNavigator::BtNavigator()
RCLCPP_INFO(get_logger(), "Creating");
// Declare this node's parameters
- declare_parameter("bt_xml_filename",
- rclcpp::ParameterValue(std::string("navigate_w_replanning_and_recovery.xml")));
+ declare_parameter("bt_xml_filename");
}
BtNavigator::~BtNavigator()
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp
index a8cf8a9ebcdbe981ac3f17741eb6cded9116e691..acc3e7c23470ba6d3453fdc70c74126dc50f1753 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/footprint_subscriber.hpp
@@ -30,32 +30,48 @@ class FootprintSubscriber
public:
FootprintSubscriber(
nav2_util::LifecycleNode::SharedPtr node,
- const std::string & topic_name);
+ const std::string & topic_name,
+ const double & footprint_timeout);
FootprintSubscriber(
rclcpp::Node::SharedPtr node,
- const std::string & topic_name);
+ const std::string & topic_name,
+ const double & footprint_timeout);
FootprintSubscriber(
const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
- const std::string & topic_name);
+ const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+ const std::string & topic_name,
+ const double & footprint_timeout);
~FootprintSubscriber() {}
+ // Returns an oriented robot footprint at current time.
+ bool getFootprint(
+ std::vector<geometry_msgs::msg::Point> & footprint,
+ rclcpp::Duration & valid_footprint_timeout);
bool getFootprint(std::vector<geometry_msgs::msg::Point> & footprint);
+ // Returns an oriented robot footprint.
+ // The second parameter is the time the fooptrint was at this orientation.
+ bool getFootprint(
+ std::vector<geometry_msgs::msg::Point> & footprint,
+ rclcpp::Time & stamp, rclcpp::Duration valid_footprint_timeout);
+
protected:
// Interfaces used for logging and creating publishers and subscribers
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
+ rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
void footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg);
std::string topic_name_;
bool footprint_received_{false};
+ rclcpp::Duration footprint_timeout_;
geometry_msgs::msg::PolygonStamped::SharedPtr footprint_;
rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr footprint_sub_;
};
diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp
index e097ac751f6d22bcf0ad31c712593d0caeffcc91..69b40f446610048f006302159589247cb24486e2 100644
--- a/nav2_costmap_2d/src/collision_checker.cpp
+++ b/nav2_costmap_2d/src/collision_checker.cpp
@@ -97,7 +97,7 @@ Footprint CollisionChecker::getFootprint(const geometry_msgs::msg::Pose2D & pose
{
Footprint footprint;
if (!footprint_sub_.getFootprint(footprint)) {
- throw CollisionCheckerException("Footprint not available.");
+ throw CollisionCheckerException("Current footprint not available.");
}
Footprint footprint_spec;
diff --git a/nav2_costmap_2d/src/footprint_subscriber.cpp b/nav2_costmap_2d/src/footprint_subscriber.cpp
index 401595cd90b6ca4f4a55941ab6d4502d836e4a9a..21c20376d423a5a6e889340f1ff6d02a238d263e 100644
--- a/nav2_costmap_2d/src/footprint_subscriber.cpp
+++ b/nav2_costmap_2d/src/footprint_subscriber.cpp
@@ -23,31 +23,39 @@ namespace nav2_costmap_2d
FootprintSubscriber::FootprintSubscriber(
nav2_util::LifecycleNode::SharedPtr node,
- const std::string & topic_name)
+ const std::string & topic_name,
+ const double & footprint_timeout)
: FootprintSubscriber(node->get_node_base_interface(),
node->get_node_topics_interface(),
node->get_node_logging_interface(),
- topic_name)
+ node->get_node_clock_interface(),
+ topic_name, footprint_timeout)
{}
FootprintSubscriber::FootprintSubscriber(
rclcpp::Node::SharedPtr node,
- const std::string & topic_name)
+ const std::string & topic_name,
+ const double & footprint_timeout)
: FootprintSubscriber(node->get_node_base_interface(),
node->get_node_topics_interface(),
node->get_node_logging_interface(),
- topic_name)
+ node->get_node_clock_interface(),
+ topic_name, footprint_timeout)
{}
FootprintSubscriber::FootprintSubscriber(
const rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
const rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
- const std::string & topic_name)
+ const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock,
+ const std::string & topic_name,
+ const double & footprint_timeout)
: node_base_(node_base),
node_topics_(node_topics),
node_logging_(node_logging),
- topic_name_(topic_name)
+ node_clock_(node_clock),
+ topic_name_(topic_name),
+ footprint_timeout_(rclcpp::Duration(footprint_timeout, 0.))
{
footprint_sub_ = rclcpp::create_subscription<geometry_msgs::msg::PolygonStamped>(node_topics_,
topic_name, rclcpp::SystemDefaultsQoS(),
@@ -55,7 +63,10 @@ FootprintSubscriber::FootprintSubscriber(
}
bool
-FootprintSubscriber::getFootprint(std::vector<geometry_msgs::msg::Point> & footprint)
+FootprintSubscriber::getFootprint(
+ std::vector<geometry_msgs::msg::Point> & footprint,
+ rclcpp::Time & stamp,
+ rclcpp::Duration valid_footprint_timeout)
{
if (!footprint_received_) {
return false;
@@ -63,9 +74,31 @@ FootprintSubscriber::getFootprint(std::vector<geometry_msgs::msg::Point> & footp
footprint = toPointVector(
std::make_shared<geometry_msgs::msg::Polygon>(footprint_->polygon));
+ auto & footprint_stamp = footprint_->header.stamp;
+
+ if (stamp - footprint_stamp > valid_footprint_timeout) {
+ return false;
+ }
+
return true;
}
+bool
+FootprintSubscriber::getFootprint(
+ std::vector<geometry_msgs::msg::Point> & footprint,
+ rclcpp::Duration & valid_footprint_timeout)
+{
+ rclcpp::Time t = node_clock_->get_clock()->now();
+ return getFootprint(footprint, t, valid_footprint_timeout);
+}
+
+bool
+FootprintSubscriber::getFootprint(
+ std::vector<geometry_msgs::msg::Point> & footprint)
+{
+ return getFootprint(footprint, footprint_timeout_);
+}
+
void
FootprintSubscriber::footprint_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg)
{
diff --git a/nav2_costmap_2d/test/integration/test_collision_checker.cpp b/nav2_costmap_2d/test/integration/test_collision_checker.cpp
index db988ecf26f71717e1fda56d53b5014b75bc0391..a7a79c07d81f30353562568f6309de3ca0a5ac1d 100644
--- a/nav2_costmap_2d/test/integration/test_collision_checker.cpp
+++ b/nav2_costmap_2d/test/integration/test_collision_checker.cpp
@@ -72,7 +72,7 @@ public:
DummyFootprintSubscriber(
nav2_util::LifecycleNode::SharedPtr node,
std::string & topic_name)
- : FootprintSubscriber(node, topic_name)
+ : FootprintSubscriber(node, topic_name, 10.0)
{}
void setFootprint(geometry_msgs::msg::PolygonStamped::SharedPtr msg)
diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp
index 0ac1ac8756dd8ed2e8e3fea7fb11f7aa29e083b3..27e3945ab90f6bf7200e2e1c53b1afa3502ffd35 100644
--- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp
+++ b/nav2_recoveries/include/nav2_recoveries/recovery.hpp
@@ -109,7 +109,7 @@ public:
node_, costmap_topic);
footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
- node_, footprint_topic);
+ node_, footprint_topic, 1.0);
collision_checker_ = std::make_unique<nav2_costmap_2d::CollisionChecker>(
*costmap_sub_, *footprint_sub_, *tf_, node_->get_name(), "odom");