Skip to content
Snippets Groups Projects
Unverified Commit 19402664 authored by Steven Macenski's avatar Steven Macenski Committed by GitHub
Browse files

[Continued from 1329] Provide behavior tree logging output on the console and a topic (#1486)


* Add console logging

* Fix node names so every node is uniquely identified in log

* Adding ros topic bt logger

* update BT nav + recovery for merge issues

* rename compute path -> compute path to pose

* renaming to follow literature

Co-authored-by: default avatarCarl Delsey <carl.r.delsey@intel.com>
parent a3e03443
No related branches found
No related tags found
No related merge requests found
......@@ -51,10 +51,10 @@ BehaviorTreeEngine::run(
return BtStatus::CANCELED;
}
onLoop();
result = tree->root_node->executeTick();
onLoop();
loopRate.sleep();
}
......
......@@ -32,6 +32,7 @@ set(library_name ${executable_name}_core)
add_library(${library_name} SHARED
src/bt_navigator.cpp
src/ros_topic_logger.cpp
)
set(dependencies
......
<!--
This Behavior Tree replans the global path periodically at 1 Hz and it also has
recovery actions.
......@@ -9,17 +10,17 @@
<RateController hz="1.0">
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
<ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</RateController>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="FollowPath"/>
<ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
</RecoveryNode>
</PipelineSequence>
<SequenceStar name="RecoveryActions">
<ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
<Spin spin_dist="1.57"/>
<Wait wait_duration="5"/>
</SequenceStar>
......
......@@ -11,7 +11,7 @@
<RecoveryNode number_of_retries="4" name="ComputePathToPose">
<ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
<RoundRobin name="GlobalPlannerRecoveryActions">
<ClearEntireCostmap service_name="global_costmap/clear_entirely_global_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
<Wait wait_duration="2"/>
</RoundRobin>
</RecoveryNode>
......@@ -19,7 +19,7 @@
<RecoveryNode number_of_retries="4" name="FollowPath">
<FollowPath path="{path}" controller_id="FollowPath"/>
<RoundRobin name="PlannerRecoveryActions">
<ClearEntireCostmap service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
<Spin spin_dist="1.57"/>
</RoundRobin>
</RecoveryNode>
......
// Copyright (c) 2019 Intel Corporation
//
// 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_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_
#define NAV2_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_
#include <vector>
#include "behaviortree_cpp_v3/loggers/abstract_logger.h"
#include "rclcpp/rclcpp.hpp"
#include "nav2_msgs/msg/behavior_tree_log.hpp"
#include "nav2_msgs/msg/behavior_tree_status_change.h"
namespace nav2_bt_navigator
{
class RosTopicLogger : public BT::StatusChangeLogger
{
public:
RosTopicLogger(const rclcpp::Node::SharedPtr & ros_node, const BT::Tree & tree);
void callback(
BT::Duration timestamp,
const BT::TreeNode & node,
BT::NodeStatus prev_status,
BT::NodeStatus status) override;
void flush() override;
protected:
rclcpp::Node::SharedPtr ros_node_;
rclcpp::Publisher<nav2_msgs::msg::BehaviorTreeLog>::SharedPtr log_pub_;
std::vector<nav2_msgs::msg::BehaviorTreeStatusChange> event_log_;
};
} // namespace nav2_bt_navigator
#endif // NAV2_BT_NAVIGATOR__ROS_TOPIC_LOGGER_HPP_
......@@ -22,6 +22,7 @@
#include <vector>
#include "nav2_behavior_tree/bt_conversions.hpp"
#include "nav2_bt_navigator/ros_topic_logger.hpp"
namespace nav2_bt_navigator
{
......@@ -206,12 +207,15 @@ BtNavigator::navigateToPose()
return action_server_->is_cancel_requested();
};
auto on_loop = [this]() {
RosTopicLogger topic_logger(client_node_, *tree_);
auto on_loop = [&]() {
if (action_server_->is_preempt_requested()) {
RCLCPP_INFO(get_logger(), "Received goal preemption request");
action_server_->accept_pending_goal();
initializeGoalPose();
}
topic_logger.flush();
};
// Execute the BT that was previously created in the configure step
......
// Copyright (c) 2019 Intel Corporation
//
// 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 <utility>
#include "nav2_bt_navigator/ros_topic_logger.hpp"
#include "tf2_ros/buffer_interface.h"
namespace nav2_bt_navigator
{
RosTopicLogger::RosTopicLogger(
const rclcpp::Node::SharedPtr & ros_node, const BT::Tree & tree)
: StatusChangeLogger(tree.root_node), ros_node_(ros_node)
{
log_pub_ = ros_node_->create_publisher<nav2_msgs::msg::BehaviorTreeLog>(
"behavior_tree_log",
rclcpp::QoS(10));
}
void RosTopicLogger::callback(
BT::Duration timestamp,
const BT::TreeNode & node,
BT::NodeStatus prev_status,
BT::NodeStatus status)
{
nav2_msgs::msg::BehaviorTreeStatusChange event;
// BT timestamps are a duration since the epoch. Need to convert to a time_point
// before converting to a msg.
event.timestamp =
tf2_ros::toMsg(std::chrono::time_point<std::chrono::high_resolution_clock>(timestamp));
event.node_name = node.name();
event.previous_status = toStr(prev_status, false);
event.current_status = toStr(status, false);
event_log_.push_back(std::move(event));
RCLCPP_DEBUG(ros_node_->get_logger(), "[%.3f]: %25s %s -> %s",
std::chrono::duration<double>(timestamp).count(),
node.name().c_str(),
toStr(prev_status, true).c_str(),
toStr(status, true).c_str() );
}
void RosTopicLogger::flush()
{
if (event_log_.size() > 0) {
nav2_msgs::msg::BehaviorTreeLog log_msg;
log_msg.timestamp = ros_node_->now();
log_msg.event_log = event_log_;
log_pub_->publish(log_msg);
event_log_.clear();
}
}
} // namespace nav2_bt_navigator
......@@ -16,6 +16,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/Costmap.msg"
"msg/CostmapMetaData.msg"
"msg/VoxelGrid.msg"
"msg/BehaviorTreeStatusChange.msg"
"msg/BehaviorTreeLog.msg"
"srv/GetCostmap.srv"
"srv/ClearCostmapExceptRegion.srv"
"srv/ClearCostmapAroundRobot.srv"
......
builtin_interfaces/Time timestamp # ROS time that this log message was sent.
BehaviorTreeStatusChange[] event_log
builtin_interfaces/Time timestamp # internal behavior tree event timestamp. Typically this is wall clock time
string node_name
string previous_status # IDLE, RUNNING, SUCCESS or FAILURE
string current_status # IDLE, RUNNING, SUCCESS or FAILURE
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment