use tf2's .hpp includes

Signed-off-by: rayferric <[email protected]>

Author: rayferric

nav2_amcl/src/amcl_node.cpp

@@ -15,40 +15,36 @@
 #ifndef NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
 #define NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
 
-#include <vector>
 #include <memory>
 #include <utility>
+#include <vector>
 
 #include "behaviortree_cpp/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"
-#include "tf2/time.h"
+#include "rclcpp/rclcpp.hpp"
+#include "tf2/time.hpp"
 #include "tf2_ros/buffer_interface.h"
 
-namespace nav2_behavior_tree
-{
+namespace nav2_behavior_tree {
 
 /**
  * @brief A class to publish BT logs on BT status change
  */
-class RosTopicLogger : public BT::StatusChangeLogger
-{
+class RosTopicLogger : public BT::StatusChangeLogger {
 public:
   /**
    * @brief A constructor for nav2_behavior_tree::RosTopicLogger
    * @param ros_node Weak pointer to parent rclcpp::Node
    * @param tree BT to monitor
    */
-  RosTopicLogger(const rclcpp::Node::WeakPtr & ros_node, const BT::Tree & tree)
-  : StatusChangeLogger(tree.rootNode())
-  {
+  RosTopicLogger(const rclcpp::Node::WeakPtr &ros_node, const BT::Tree &tree)
+      : StatusChangeLogger(tree.rootNode()) {
     auto node = ros_node.lock();
     clock_ = node->get_clock();
     logger_ = node->get_logger();
     log_pub_ = node->create_publisher<nav2_msgs::msg::BehaviorTreeLog>(
-      "behavior_tree_log",
-      rclcpp::QoS(10));
+        "behavior_tree_log", rclcpp::QoS(10));
   }
 
   /**
@@ -58,36 +54,29 @@ class RosTopicLogger : public BT::StatusChangeLogger
    * @param prev_status Previous status of the node
    * @param status Current status of the node
    */
-  void callback(
-    BT::Duration timestamp,
-    const BT::TreeNode & node,
-    BT::NodeStatus prev_status,
-    BT::NodeStatus status) override
-  {
+  void callback(BT::Duration timestamp, const BT::TreeNode &node,
+                BT::NodeStatus prev_status, BT::NodeStatus status) override {
     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.
+    // 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(tf2::TimePoint(timestamp));
     event.node_name = node.name();
     event.uid = node.UID();
     event.previous_status = toStr(prev_status, false);
     event.current_status = toStr(status, false);
     event_log_.push_back(std::move(event));
 
-    RCLCPP_DEBUG(
-      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() );
+    RCLCPP_DEBUG(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());
   }
 
   /**
    * @brief Clear log buffer if any
    */
-  void flush() override
-  {
+  void flush() override {
     if (!event_log_.empty()) {
       auto log_msg = std::make_unique<nav2_msgs::msg::BehaviorTreeLog>();
       log_msg->timestamp = clock_->now();
@@ -104,6 +93,6 @@ class RosTopicLogger : public BT::StatusChangeLogger
   std::vector<nav2_msgs::msg::BehaviorTreeStatusChange> event_log_;
 };
 
-}   // namespace nav2_behavior_tree
+} // namespace nav2_behavior_tree
 
-#endif   // NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_
+#endif // NAV2_BEHAVIOR_TREE__ROS_TOPIC_LOGGER_HPP_

nav2_behavior_tree/include/nav2_behavior_tree/ros_topic_logger.hpp

@@ -12,28 +12,23 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <string>
-#include <memory>
 #include <limits>
+#include <memory>
+#include <string>
 
-#include "nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp"
 #include "nav2_behavior_tree/bt_utils.hpp"
-#include "tf2/utils.h"
+#include "nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp"
+#include "tf2/utils.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
-namespace nav2_behavior_tree
-{
+namespace nav2_behavior_tree {
 
 RemoveInCollisionGoals::RemoveInCollisionGoals(
-  const std::string & service_node_name,
-  const BT::NodeConfiguration & conf)
-: BtServiceNode<nav2_msgs::srv::GetCosts>(service_node_name, conf,
-    "/global_costmap/get_cost_global_costmap")
-{}
-
+    const std::string &service_node_name, const BT::NodeConfiguration &conf)
+    : BtServiceNode<nav2_msgs::srv::GetCosts>(
+          service_node_name, conf, "/global_costmap/get_cost_global_costmap") {}
 
-void RemoveInCollisionGoals::on_tick()
-{
+void RemoveInCollisionGoals::on_tick() {
   getInput("use_footprint", use_footprint_);
   getInput("cost_threshold", cost_threshold_);
   getInput("input_goals", input_goals_);
@@ -47,44 +42,40 @@ void RemoveInCollisionGoals::on_tick()
   request_ = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
   request_->use_footprint = use_footprint_;
 
-  for (const auto & goal : input_goals_) {
+  for (const auto &goal : input_goals_) {
     request_->poses.push_back(goal);
   }
 }
 
 BT::NodeStatus RemoveInCollisionGoals::on_completion(
-  std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
-{
+    std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response) {
   if (!response->success) {
-    RCLCPP_ERROR(
-      node_->get_logger(),
-      "GetCosts service call failed");
+    RCLCPP_ERROR(node_->get_logger(), "GetCosts service call failed");
     setOutput("output_goals", input_goals_);
     return BT::NodeStatus::FAILURE;
   }
 
   Goals valid_goal_poses;
   for (size_t i = 0; i < response->costs.size(); ++i) {
     if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||
-      response->costs[i] < cost_threshold_)
-    {
+        response->costs[i] < cost_threshold_) {
       valid_goal_poses.push_back(input_goals_[i]);
     }
   }
   // Inform if all goals have been removed
   if (valid_goal_poses.empty()) {
     RCLCPP_INFO(
-      node_->get_logger(),
-      "All goals are in collision and have been removed from the list");
+        node_->get_logger(),
+        "All goals are in collision and have been removed from the list");
   }
   setOutput("output_goals", valid_goal_poses);
   return BT::NodeStatus::SUCCESS;
 }
 
-}   // namespace nav2_behavior_tree
+} // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp/bt_factory.h"
-BT_REGISTER_NODES(factory)
-{
-  factory.registerNodeType<nav2_behavior_tree::RemoveInCollisionGoals>("RemoveInCollisionGoals");
+BT_REGISTER_NODES(factory) {
+  factory.registerNodeType<nav2_behavior_tree::RemoveInCollisionGoals>(
+      "RemoveInCollisionGoals");
 }

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -24,27 +24,23 @@
 #include "nav2_util/robot_utils.hpp"
 #include "nav_msgs/msg/path.hpp"
 #include "rclcpp/rclcpp.hpp"
-#include "tf2/LinearMath/Quaternion.h"
+#include "tf2/LinearMath/Quaternion.hpp"
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/create_timer_ros.h"
 
 #include "nav2_behavior_tree/plugins/action/truncate_path_local_action.hpp"
 
-namespace nav2_behavior_tree
-{
+namespace nav2_behavior_tree {
 
-TruncatePathLocal::TruncatePathLocal(
-  const std::string & name,
-  const BT::NodeConfiguration & conf)
-: BT::ActionNodeBase(name, conf)
-{
+TruncatePathLocal::TruncatePathLocal(const std::string &name,
+                                     const BT::NodeConfiguration &conf)
+    : BT::ActionNodeBase(name, conf) {
   tf_buffer_ =
-    config().blackboard->template get<std::shared_ptr<tf2_ros::Buffer>>(
-    "tf_buffer");
+      config().blackboard->template get<std::shared_ptr<tf2_ros::Buffer>>(
+          "tf_buffer");
 }
 
-inline BT::NodeStatus TruncatePathLocal::tick()
-{
+inline BT::NodeStatus TruncatePathLocal::tick() {
   setStatus(BT::NodeStatus::RUNNING);
 
   double distance_forward, distance_backward;
@@ -76,70 +72,77 @@ inline BT::NodeStatus TruncatePathLocal::tick()
 
   auto closest_pose_detection_end = path_.poses.end();
   if (path_pruning) {
-    closest_pose_detection_end = nav2_util::geometry_utils::first_after_integrated_distance(
-      closest_pose_detection_begin_, path_.poses.end(), max_robot_pose_search_dist);
+    closest_pose_detection_end =
+        nav2_util::geometry_utils::first_after_integrated_distance(
+            closest_pose_detection_begin_, path_.poses.end(),
+            max_robot_pose_search_dist);
   }
 
   // find the closest pose on the path
   auto current_pose = nav2_util::geometry_utils::min_by(
-    closest_pose_detection_begin_, closest_pose_detection_end,
-    [&pose, angular_distance_weight](const geometry_msgs::msg::PoseStamped & ps) {
-      return poseDistance(pose, ps, angular_distance_weight);
-    });
+      closest_pose_detection_begin_, closest_pose_detection_end,
+      [&pose,
+       angular_distance_weight](const geometry_msgs::msg::PoseStamped &ps) {
+        return poseDistance(pose, ps, angular_distance_weight);
+      });
 
   if (path_pruning) {
     closest_pose_detection_begin_ = current_pose;
   }
 
   // expand forwards to extract desired length
-  auto forward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance(
-    current_pose, path_.poses.end(), distance_forward);
+  auto forward_pose_it =
+      nav2_util::geometry_utils::first_after_integrated_distance(
+          current_pose, path_.poses.end(), distance_forward);
 
   // expand backwards to extract desired length
-  // Note: current_pose + 1 is used because reverse iterator points to a cell before it
-  auto backward_pose_it = nav2_util::geometry_utils::first_after_integrated_distance(
-    std::reverse_iterator(current_pose + 1), path_.poses.rend(), distance_backward);
+  // Note: current_pose + 1 is used because reverse iterator points to a cell
+  // before it
+  auto backward_pose_it =
+      nav2_util::geometry_utils::first_after_integrated_distance(
+          std::reverse_iterator(current_pose + 1), path_.poses.rend(),
+          distance_backward);
 
   nav_msgs::msg::Path output_path;
   output_path.header = path_.header;
   output_path.poses = std::vector<geometry_msgs::msg::PoseStamped>(
-    backward_pose_it.base(), forward_pose_it);
+      backward_pose_it.base(), forward_pose_it);
   setOutput("output_path", output_path);
 
   return BT::NodeStatus::SUCCESS;
 }
 
-inline bool TruncatePathLocal::getRobotPose(
-  std::string path_frame_id, geometry_msgs::msg::PoseStamped & pose)
-{
+inline bool
+TruncatePathLocal::getRobotPose(std::string path_frame_id,
+                                geometry_msgs::msg::PoseStamped &pose) {
   if (!getInput("pose", pose)) {
     std::string robot_frame;
     if (!getInput("robot_frame", robot_frame)) {
-      RCLCPP_ERROR(
-        config().blackboard->get<rclcpp::Node::SharedPtr>("node")->get_logger(),
-        "Neither pose nor robot_frame specified for %s", name().c_str());
+      RCLCPP_ERROR(config()
+                       .blackboard->get<rclcpp::Node::SharedPtr>("node")
+                       ->get_logger(),
+                   "Neither pose nor robot_frame specified for %s",
+                   name().c_str());
       return false;
     }
     double transform_tolerance;
     getInput("transform_tolerance", transform_tolerance);
-    if (!nav2_util::getCurrentPose(
-        pose, *tf_buffer_, path_frame_id, robot_frame, transform_tolerance))
-    {
-      RCLCPP_WARN(
-        config().blackboard->get<rclcpp::Node::SharedPtr>("node")->get_logger(),
-        "Failed to lookup current robot pose for %s", name().c_str());
+    if (!nav2_util::getCurrentPose(pose, *tf_buffer_, path_frame_id,
+                                   robot_frame, transform_tolerance)) {
+      RCLCPP_WARN(config()
+                      .blackboard->get<rclcpp::Node::SharedPtr>("node")
+                      ->get_logger(),
+                  "Failed to lookup current robot pose for %s", name().c_str());
       return false;
     }
   }
   return true;
 }
 
 double
-TruncatePathLocal::poseDistance(
-  const geometry_msgs::msg::PoseStamped & pose1,
-  const geometry_msgs::msg::PoseStamped & pose2,
-  const double angular_distance_weight)
-{
+TruncatePathLocal::poseDistance(const geometry_msgs::msg::PoseStamped &pose1,
+                                const geometry_msgs::msg::PoseStamped &pose2,
+                                const double angular_distance_weight) {
   double dx = pose1.pose.position.x - pose2.pose.position.x;
   double dy = pose1.pose.position.y - pose2.pose.position.y;
   // taking angular distance into account in addition to spatial distance
@@ -152,10 +155,10 @@ TruncatePathLocal::poseDistance(
   return std::sqrt(dx * dx + dy * dy + da * da);
 }
 
-}  // namespace nav2_behavior_tree
+} // namespace nav2_behavior_tree
 
 #include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory) {
   factory.registerNodeType<nav2_behavior_tree::TruncatePathLocal>(
-    "TruncatePathLocal");
+      "TruncatePathLocal");
 }