Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add cout bt logs #4449

Closed
wants to merge 1 commit into from
Closed

Add cout bt logs #4449

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <string>
#include <vector>

#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "nav2_behavior_tree/behavior_tree_engine.hpp"
#include "nav2_behavior_tree/ros_topic_logger.hpp"
Expand Down
2 changes: 2 additions & 0 deletions nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include <string>
#include <vector>

#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
#include "nav2_util/lifecycle_node.hpp"
#include "nav2_util/odometry_utils.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
Expand Down Expand Up @@ -92,6 +93,7 @@ class BtNavigator : public nav2_util::LifecycleNode

// Odometry smoother object
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
std::shared_ptr<BT::StdCoutLogger> logger_cout_;

// Metrics for feedback
std::string robot_frame_;
Expand Down
2 changes: 1 addition & 1 deletion nav2_bt_navigator/src/bt_navigator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,7 +125,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
navigators_.push_back(class_loader_.createUniqueInstance(navigator_type));
if (!navigators_.back()->on_configure(
node, plugin_lib_names, feedback_utils,
&plugin_muxer_, odom_smoother_))
&plugin_muxer_, odom_smoother_, logger_cout_))
Copy link
Member

@SteveMacenski SteveMacenski Jun 18, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Where is logger_cout_ made? Shouldn't this be created in the BT Navigator to be passed onto the navigator base to share? I'm not sure why we're passing in a nullptr and its unused in the BT Navigator level unless (1) we're creating it here for all N instances to share or (2) we're keeping the logger object here to use somewhere.

(1) would mean we should be instantiating it above and then passing it in to share. Move the parameter logic here too.
(2) would mean we can remove modifying the on_configure API / storing the logger object at this high level, because its unused and breaks ABI (since its unused here anyway). That would make this fully a feature of the plugins and not of the BT Navigator instance itself.

The mixing is weird unless it would be used at a higher level

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yeah I know its weird to pass a nullptr but I got to this point to statisfy:

  • There can only be one instance of StdCoutLogger so it has to be shared across all navigator plugins
  • the logger can only be created with a tree_ which is only available BehaviorTreeNavigator and not in BTNavigator

It's possible that I missed another obvious way of implementing though

Copy link
Member

@SteveMacenski SteveMacenski Jun 18, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@facontidavide this is the second time that the singleton nature of the loggers has caused us problems in Nav2. We actually removed live Groot monitoring because of it.

Would it be possible to allow multiple loggers in BT.CPP to allow for this? If we’re running into this every couple of years, I bet others are too.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

First, it is ok to remove the singleton constraint. Let me work on it.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done: BehaviorTree/BehaviorTree.CPP@ced041b

Copy link
Contributor

@facontidavide facontidavide Jun 18, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note that creating your own logger is trivial and boring. StdCout is only 30 lines of code. Are you sure that you don't want to create your own, maybe one that use RCLCPP_INFO instead of std::cout ?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I leave that to @tonynajjar , but I agree that doing an RCLCPP log would be better so it enters the .ros/logs entries. That would probably need to live here I guess since ROS shouldn't bleed into BT.CPP.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That would probably need to live here I guess since ROS shouldn't bleed into BT.CPP.

exactly!

{
return nav2_util::CallbackReturn::FAILURE;
}
Expand Down
19 changes: 16 additions & 3 deletions nav2_core/include/nav2_core/behavior_tree_navigator.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -128,7 +128,8 @@ class NavigatorBase
const std::vector<std::string> & plugin_lib_names,
const FeedbackUtils & feedback_utils,
nav2_core::NavigatorMuxer * plugin_muxer,
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother) = 0;
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother,
std::shared_ptr<BT::StdCoutLogger> logger_cout_) = 0;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why not the abstract logger so folks could pass in any logger they want (or have option in the higher level node to select the logger type they prefer)?


/**
* @brief Activation of the navigator's backend BT and actions
Expand Down Expand Up @@ -190,13 +191,19 @@ class BehaviorTreeNavigator : public NavigatorBase
const std::vector<std::string> & plugin_lib_names,
const FeedbackUtils & feedback_utils,
nav2_core::NavigatorMuxer * plugin_muxer,
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother) final
std::shared_ptr<nav2_util::OdomSmoother> odom_smoother,
std::shared_ptr<BT::StdCoutLogger> logger_cout) final
{
auto node = parent_node.lock();
logger_ = node->get_logger();
clock_ = node->get_clock();
feedback_utils_ = feedback_utils;
plugin_muxer_ = plugin_muxer;
logger_cout_ = logger_cout;
if (!node->has_parameter("enable_cout_logger")) {
node->declare_parameter("enable_cout_logger", false);
}
enable_cout_logger_ = node->get_parameter("enable_cout_logger").as_bool();

// get the default behavior tree for this navigator
std::string default_bt_xml_filename = getDefaultBTFilepath(parent_node);
Expand Down Expand Up @@ -296,7 +303,10 @@ class BehaviorTreeNavigator : public NavigatorBase
}

bool goal_accepted = goalReceived(goal);

logger_cout_.reset();
logger_cout_ = std::make_shared<BT::StdCoutLogger>(bt_action_server_->getTree());
logger_cout_->enableTransitionToIdle(false);
logger_cout_->setEnabled(enable_cout_logger_);
if (goal_accepted) {
plugin_muxer_->startNavigating(getName());
}
Expand All @@ -313,6 +323,7 @@ class BehaviorTreeNavigator : public NavigatorBase
{
plugin_muxer_->stopNavigating(getName());
goalCompleted(result, final_bt_status);
logger_cout_.reset();
}

/**
Expand Down Expand Up @@ -371,6 +382,8 @@ class BehaviorTreeNavigator : public NavigatorBase
rclcpp::Clock::SharedPtr clock_;
FeedbackUtils feedback_utils_;
NavigatorMuxer * plugin_muxer_;
std::shared_ptr<BT::StdCoutLogger> logger_cout_;
bool enable_cout_logger_;
};

} // namespace nav2_core
Expand Down
Loading