Skip to content

Commit d9095d8

Browse files
GoesMGoesMSteveMacenski
authored and
jwallace42
committed
* bug fixed * add space * Update planner_server.cpp * add space for code style * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add ChildLifecycleNode mode in costmap_2d_ros * NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * fit to NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * fit to NodeOption: is_lifecycle_follower * fit to NodeOption: is_lifecycle_follower * fit reorder Werror * fix wrong use of is_lifecycle_follower * remove blank line * NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * Add files via upload * NodeOption: is_lifecycle_follower_ * NodeOption:is_lifecycle_follower_ * NodeOption:is_lifecycle_follower * NodeOption:is_lifecycle_follower * NodeOption:is_lifecycle_follower * change default * add NodeOption for costmap_2d_ros * add node options for costmap2dros as an independent node * code style reformat * fit to NodeOption of Costmap2DROS * fit to NodeOption of Costmap2DROS * fit to NodeOption of Costmap2DROS * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * changes * comment changes * change get_parameter into =false * comment modification * missing line * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * delete last line * change lifecycle_test fit to NodeOption --------- Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com>
1 parent 7f80b0d commit d9095d8

File tree

5 files changed

+36
-28
lines changed

5 files changed

+36
-28
lines changed

‎nav2_controller/src/controller_server.cpp‎

Lines changed: 4 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -290,11 +290,7 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
290290
* unordered_set iteration. Once this issue is resolved, we can maybe make a stronger
291291
* ordering assumption: https://github.com/ros2/rclcpp/issues/2096
292292
*/
293-
if (costmap_ros_->get_current_state().id() ==
294-
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
295-
{
296-
costmap_ros_->deactivate();
297-
}
293+
costmap_ros_->deactivate();
298294

299295
publishZeroVelocity();
300296
vel_publisher_->on_deactivate();
@@ -320,11 +316,9 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
320316

321317
goal_checkers_.clear();
322318
progress_checkers_.clear();
323-
if (costmap_ros_->get_current_state().id() ==
324-
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
325-
{
326-
costmap_ros_->cleanup();
327-
}
319+
320+
costmap_ros_->cleanup();
321+
328322

329323
// Release any allocated resources
330324
action_server_.reset();

‎nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp‎

Lines changed: 26 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -75,8 +75,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode
7575
public:
7676
/**
7777
* @brief Constructor for the wrapper
78+
* @param options Additional options to control creation of the node.
7879
*/
79-
Costmap2DROS();
80+
Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
8081

8182
/**
8283
* @brief Constructor for the wrapper, the node will
@@ -134,6 +135,28 @@ class Costmap2DROS : public nav2_util::LifecycleNode
134135
*/
135136
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
136137

138+
/**
139+
* @brief as a child-LifecycleNode :
140+
* Costmap2DROS may be launched by another Lifecycle Node as a composed module
141+
* If composed, its parents will handle the shutdown, which includes this module
142+
*/
143+
void on_rcl_preshutdown() override
144+
{
145+
if (is_lifecycle_follower_) {
146+
// Transitioning handled by parent node
147+
return;
148+
}
149+
150+
// Else, if this is an independent node, this node needs to handle itself.
151+
RCLCPP_INFO(
152+
get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)",
153+
this->get_name());
154+
155+
runCleanups();
156+
157+
destroyBond();
158+
}
159+
137160
/**
138161
* @brief Subscribes to sensor topics if necessary and starts costmap
139162
* updates, can be called to restart the costmap after calls to either
@@ -381,6 +404,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode
381404
double transform_tolerance_{0}; ///< The timeout before transform errors
382405
double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors
383406

407+
bool is_lifecycle_follower_{true}; ///< whether is a child-LifecycleNode or an independent node
408+
384409
// Derived parameters
385410
bool use_radius_{false};
386411
std::vector<geometry_msgs::msg::Point> unpadded_footprint_;

‎nav2_costmap_2d/src/costmap_2d_ros.cpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -61,8 +61,8 @@ namespace nav2_costmap_2d
6161
Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time)
6262
: Costmap2DROS(name, "/", name, use_sim_time) {}
6363

64-
Costmap2DROS::Costmap2DROS()
65-
: nav2_util::LifecycleNode("costmap", ""),
64+
Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options)
65+
: nav2_util::LifecycleNode("costmap", "", options),
6666
name_("costmap"),
6767
default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"},
6868
default_types_{
@@ -71,6 +71,7 @@ Costmap2DROS::Costmap2DROS()
7171
"nav2_costmap_2d::InflationLayer"}
7272
{
7373
declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map")));
74+
is_lifecycle_follower_ = false;
7475
init();
7576
}
7677

‎nav2_costmap_2d/test/unit/lifecycle_test.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@
2222
TEST(LifecylceTest, CheckInitialTfTimeout) {
2323
rclcpp::init(0, nullptr);
2424

25-
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
25+
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(rclcpp::NodeOptions());
2626
costmap->set_parameter({"initial_transform_timeout", 0.0});
2727

2828
std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}};

‎nav2_planner/src/planner_server.cpp‎

Lines changed: 2 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -224,11 +224,7 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
224224
* unordered_set iteration. Once this issue is resolved, we can maybe make a stronger
225225
* ordering assumption: https://github.com/ros2/rclcpp/issues/2096
226226
*/
227-
if (costmap_ros_->get_current_state().id() ==
228-
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
229-
{
230-
costmap_ros_->deactivate();
231-
}
227+
costmap_ros_->deactivate();
232228

233229
PlannerMap::iterator it;
234230
for (it = planners_.begin(); it != planners_.end(); ++it) {
@@ -254,15 +250,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
254250
failed_footprint_.reset();
255251
tf_.reset();
256252

257-
/*
258-
* Double check whether something else transitioned it to INACTIVE
259-
* already, e.g. the rcl preshutdown callback.
260-
*/
261-
if (costmap_ros_->get_current_state().id() ==
262-
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
263-
{
264-
costmap_ros_->cleanup();
265-
}
253+
costmap_ros_->cleanup();
266254

267255
PlannerMap::iterator it;
268256
for (it = planners_.begin(); it != planners_.end(); ++it) {

0 commit comments

Comments
 (0)