From 6acfbb341c14b8696e5c26ffd130c83532979a63 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Fri, 3 Jul 2020 16:06:55 -0400 Subject: [PATCH] reload behavior tree before creating RosTopicLogger to prevent nullptr error or no /behavior_tree_log problem Signed-off-by: Daisuke Sato --- nav2_bt_navigator/src/bt_navigator.cpp | 26 +++++++++++++------------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index f0c063c79a6..beee45ac91d 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -247,6 +247,19 @@ BtNavigator::navigateToPose() return action_server_->is_cancel_requested(); }; + auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree; + + // Empty id in request is default for backward compatibility + bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename; + + if (!loadBehaviorTree(bt_xml_filename)) { + RCLCPP_ERROR( + get_logger(), "BT file not found: %s. Navigation canceled", + bt_xml_filename.c_str(), current_bt_xml_filename_.c_str()); + action_server_->terminate_current(); + return; + } + RosTopicLogger topic_logger(client_node_, tree_); std::shared_ptr feedback_msg = std::make_shared(); @@ -276,19 +289,6 @@ BtNavigator::navigateToPose() action_server_->publish_feedback(feedback_msg); }; - auto bt_xml_filename = action_server_->get_current_goal()->behavior_tree; - - // Empty id in request is default for backward compatibility - bt_xml_filename = bt_xml_filename == "" ? default_bt_xml_filename_ : bt_xml_filename; - - if (!loadBehaviorTree(bt_xml_filename)) { - RCLCPP_ERROR( - get_logger(), "BT file not found: %s. Navigation canceled", - bt_xml_filename.c_str(), current_bt_xml_filename_.c_str()); - action_server_->terminate_current(); - return; - } - // Execute the BT that was previously created in the configure step nav2_behavior_tree::BtStatus rc = bt_->run(&tree_, on_loop, is_canceling); // Make sure that the Bt is not in a running state from a previous execution