From 45f75c4c883d0d174951342f9713a489d1ebe5cc Mon Sep 17 00:00:00 2001 From: HovorunB Date: Tue, 30 Jan 2024 11:41:48 +0100 Subject: [PATCH 1/4] AMRNAV-6258 --- nav2_behaviors/src/behavior_server.cpp | 8 ++++++++ .../src/collision_monitor_node.cpp | 1 + .../include/nav2_costmap_2d/costmap_subscriber.hpp | 5 +++-- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 ++ nav2_costmap_2d/src/costmap_2d_ros.cpp | 14 ++++++++++++-- nav2_costmap_2d/src/costmap_subscriber.cpp | 4 +++- nav2_planner/src/planner_server.cpp | 2 ++ nav2_velocity_smoother/src/velocity_smoother.cpp | 1 + 8 files changed, 32 insertions(+), 5 deletions(-) diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index b5ec19ba626..5a2bf057319 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -202,6 +202,14 @@ BehaviorServer::on_activate(const rclcpp_lifecycle::State & /*state*/) // create bond connection createBond(); + if (get_node_options().use_intra_process_comms()) + { + RCLCPP_INFO(get_logger(), "use_intra_process_comms is true for %s", get_name()); + } else { + RCLCPP_INFO(get_logger(), "use_intra_process_comms is false for %s", get_name()); + } + + return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index f04bc8b5db3..180d982407e 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -183,6 +183,7 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/) void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg) { // If message contains NaN or Inf, ignore + RCLCPP_INFO(get_logger(), "Recived velocity: x: %f on address: %lu", msg->linear.x, reinterpret_cast(msg.get())); if (!nav2_util::validateTwist(*msg)) { RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!"); return; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index e4ea745015d..48bd43f8e81 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -63,14 +63,15 @@ class CostmapSubscriber /** * @brief Callback for the costmap topic */ - void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg); + void costmapCallback(const nav2_msgs::msg::Costmap::ConstSharedPtr msg); protected: std::shared_ptr costmap_; - nav2_msgs::msg::Costmap::SharedPtr costmap_msg_; + nav2_msgs::msg::Costmap::ConstSharedPtr costmap_msg_; std::string topic_name_; bool costmap_received_{false}; rclcpp::Subscription::SharedPtr costmap_sub_; + rclcpp::Logger logger_{rclcpp::get_logger("costmap_sub")}; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 87881bc7caa..f9a6daf91aa 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -185,6 +185,8 @@ void Costmap2DPublisher::publishCostmap() { if (costmap_raw_pub_->get_subscription_count() > 0) { prepareCostmap(); + RCLCPP_INFO(logger_, "Publishing costmap %i on address: %lu", costmap_raw_->header.stamp.nanosec, reinterpret_cast(costmap_raw_.get())); + costmap_raw_pub_->publish(std::move(costmap_raw_)); } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 0d7b442a8ae..5df40275f12 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -89,7 +89,7 @@ Costmap2DROS::Costmap2DROS( nav2_util::add_namespaces(parent_namespace, local_namespace), "--ros-args", "-r", name + ":" + std::string("__node:=") + name, "--ros-args", "-p", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"), - })), + }).use_intra_process_comms(true)), name_(name), parent_namespace_(parent_namespace), default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, @@ -208,6 +208,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Initialized costmap filter \"%s\"", filter_names_[i].c_str()); } + auto qos = rclcpp::SystemDefaultsQoS(); + RCLCPP_INFO(get_logger(), "Created SystemDefaultsQoS with depth %li", qos.depth()); // Create the publishers and subscribers footprint_sub_ = create_subscription( "footprint", @@ -215,7 +217,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) std::bind(&Costmap2DROS::setRobotFootprintPolygon, this, std::placeholders::_1)); footprint_pub_ = create_publisher( - "published_footprint", rclcpp::SystemDefaultsQoS()); + "published_footprint", rclcpp::SystemDefaultsQoS().keep_last(1)); costmap_publisher_ = std::make_unique( shared_from_this(), @@ -543,6 +545,14 @@ Costmap2DROS::updateMap() footprint->header = pose.header; transformFootprint(x, y, yaw, padded_footprint_, *footprint); + RCLCPP_DEBUG(get_logger(), "Publishing footprint"); + if (get_node_options().use_intra_process_comms()) + { + RCLCPP_INFO(get_logger(), "use_intra_process_comms is true for %s", get_name()); + } else { + RCLCPP_INFO(get_logger(), "use_intra_process_comms is false for %s", get_name()); + } + RCLCPP_DEBUG(get_logger(), "Publishing footprint"); footprint_pub_->publish(std::move(footprint)); initialized_ = true; diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index d56ac942e98..874ba631095 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -26,6 +26,7 @@ CostmapSubscriber::CostmapSubscriber( : topic_name_(topic_name) { auto node = parent.lock(); + logger_ = node->get_logger(); costmap_sub_ = node->create_subscription( topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), @@ -86,8 +87,9 @@ void CostmapSubscriber::toCostmap2D() } } -void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) +void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::ConstSharedPtr msg) { + RCLCPP_INFO(logger_, "Received costmap %i on topic %s, on address: %lu ", msg->header.stamp.nanosec, topic_name_.c_str(), reinterpret_cast(msg.get())); std::atomic_store(&costmap_msg_, msg); if (!costmap_received_) { costmap_received_ = true; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 4b914e92d0d..8498d24d561 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -63,6 +63,8 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) } // Setup the global costmap + auto qos = rclcpp::SystemDefaultsQoS(); + RCLCPP_INFO(get_logger(), "Created SystemDefaultsQoS with depth %li", qos.depth()); costmap_ros_ = std::make_shared( "global_costmap", std::string{get_namespace()}, "global_costmap", get_parameter("use_sim_time").as_bool()); diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 3742cbb1eee..41df5b350b5 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -332,6 +332,7 @@ void VelocitySmoother::smootherTimer() cmd_vel->angular.z = fabs(cmd_vel->angular.z) < deadband_velocities_[2] ? 0.0 : cmd_vel->angular.z; + RCLCPP_INFO(get_logger(), "Publishing velocity: x: %f on address: %lu", cmd_vel->linear.x, reinterpret_cast(cmd_vel.get())); smoothed_cmd_pub_->publish(std::move(cmd_vel)); } From bf28c4ce51166359039bb9bccd3e954b761fc88b Mon Sep 17 00:00:00 2001 From: HovorunB Date: Tue, 30 Jan 2024 17:40:35 +0100 Subject: [PATCH 2/4] . --- nav2_collision_monitor/src/collision_detector_node.cpp | 2 +- nav2_collision_monitor/src/polygon.cpp | 4 ++-- nav2_controller/src/controller_server.cpp | 2 +- .../include/nav2_costmap_2d/costmap_2d_ros.hpp | 5 +++-- nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp | 4 ++-- nav2_costmap_2d/plugins/static_layer.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 9 +++++---- nav2_planner/src/planner_server.cpp | 4 +--- 8 files changed, 16 insertions(+), 16 deletions(-) diff --git a/nav2_collision_monitor/src/collision_detector_node.cpp b/nav2_collision_monitor/src/collision_detector_node.cpp index d7252c83e15..b3c60cd7ef1 100644 --- a/nav2_collision_monitor/src/collision_detector_node.cpp +++ b/nav2_collision_monitor/src/collision_detector_node.cpp @@ -53,7 +53,7 @@ CollisionDetector::on_configure(const rclcpp_lifecycle::State & /*state*/) tf_listener_ = std::make_shared(*tf_buffer_); state_pub_ = this->create_publisher( - "collision_detector_state", rclcpp::SystemDefaultsQoS()); + "collision_detector_state", rclcpp::SystemDefaultsQoS().keep_last(1)); collision_points_marker_pub_ = this->create_publisher( "~/collision_points_marker", 1); diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index 0d104623da8..89f5b688028 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -77,7 +77,7 @@ bool Polygon::configure() logger_, "[%s]: Subscribing on %s topic for polygon", polygon_name_.c_str(), polygon_sub_topic.c_str()); - rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS().keep_last(1); // set to default polygon_sub_ = node->create_subscription( polygon_sub_topic, polygon_qos, std::bind(&Polygon::polygonCallback, this, std::placeholders::_1)); @@ -106,7 +106,7 @@ bool Polygon::configure() polygon_.polygon.points.push_back(p_s); } - rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS().keep_last(1); // set to default polygon_pub_ = node->create_publisher( polygon_pub_topic, polygon_qos); } diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 7c503fa2e27..08a349d4abb 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -67,7 +67,7 @@ ControllerServer::ControllerServer(const rclcpp::NodeOptions & options) // The costmap node is used in the implementation of the controller costmap_ros_ = std::make_shared( "local_costmap", std::string{get_namespace()}, "local_costmap", - get_parameter("use_sim_time").as_bool()); + get_parameter("use_sim_time").as_bool(), options.use_intra_process_comms()); } ControllerServer::~ControllerServer() diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 46a71c8e76d..95d2ef26732 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -84,7 +84,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode * @param name Name of the costmap ROS node * @param use_sim_time Whether to use simulation or real time */ - explicit Costmap2DROS(const std::string & name, const bool & use_sim_time = false); + explicit Costmap2DROS(const std::string & name, const bool & use_sim_time = false, const bool & use_intra_process_comms = false); /** * @brief Constructor for the wrapper @@ -97,7 +97,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode const std::string & name, const std::string & parent_namespace, const std::string & local_namespace, - const bool & use_sim_time); + const bool & use_sim_time, + const bool & use_intra_process_comms = false); /** * @brief Common initialization for constructors diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index dd1a65cef0c..2678eadad09 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -140,7 +140,7 @@ class StaticLayer : public CostmapLayer * map along with its size will determine what parts of the costmap's * static map are overwritten. */ - void incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map); + void incomingMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr new_map); /** * @brief Callback to update the costmap's map from the map_server (or SLAM) * with an update in a particular area of the map @@ -185,7 +185,7 @@ class StaticLayer : public CostmapLayer bool map_received_{false}; bool map_received_in_update_bounds_{false}; tf2::Duration transform_tolerance_; - nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; + nav_msgs::msg::OccupancyGrid::ConstSharedPtr map_buffer_; // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; }; diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 079932b1b73..3dd62f4eeeb 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -275,7 +275,7 @@ StaticLayer::interpretValue(unsigned char value) } void -StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map) +StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::ConstSharedPtr new_map) { if (!map_received_) { processMap(*new_map); diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 5df40275f12..04d019230df 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -58,8 +58,8 @@ using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { -Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time) -: Costmap2DROS(name, "/", name, use_sim_time) {} +Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time, const bool & use_intra_process_comms) +: Costmap2DROS(name, "/", name, use_sim_time, use_intra_process_comms) {} Costmap2DROS::Costmap2DROS() : nav2_util::LifecycleNode("costmap", ""), @@ -78,7 +78,8 @@ Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, const std::string & local_namespace, - const bool & use_sim_time) + const bool & use_sim_time, + const bool & use_intra_process_comms) : nav2_util::LifecycleNode(name, "", // NodeOption arguments take precedence over the ones provided on the command line // use this to make sure the node is placed on the provided namespace @@ -89,7 +90,7 @@ Costmap2DROS::Costmap2DROS( nav2_util::add_namespaces(parent_namespace, local_namespace), "--ros-args", "-r", name + ":" + std::string("__node:=") + name, "--ros-args", "-p", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"), - }).use_intra_process_comms(true)), + }).use_intra_process_comms(use_intra_process_comms)), name_(name), parent_namespace_(parent_namespace), default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 8498d24d561..071d05de35f 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -63,11 +63,9 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options) } // Setup the global costmap - auto qos = rclcpp::SystemDefaultsQoS(); - RCLCPP_INFO(get_logger(), "Created SystemDefaultsQoS with depth %li", qos.depth()); costmap_ros_ = std::make_shared( "global_costmap", std::string{get_namespace()}, "global_costmap", - get_parameter("use_sim_time").as_bool()); + get_parameter("use_sim_time").as_bool(), options.use_intra_process_comms()); } PlannerServer::~PlannerServer() From 188cca0fd2dc92bc01e33421bee4c14f002f34d0 Mon Sep 17 00:00:00 2001 From: HovorunB Date: Wed, 31 Jan 2024 11:15:30 +0100 Subject: [PATCH 3/4] remove logs --- nav2_behaviors/src/behavior_server.cpp | 10 +--------- nav2_collision_monitor/src/collision_monitor_node.cpp | 1 - nav2_costmap_2d/src/costmap_2d_publisher.cpp | 1 - nav2_costmap_2d/src/costmap_2d_ros.cpp | 10 ---------- nav2_costmap_2d/src/costmap_subscriber.cpp | 2 -- nav2_velocity_smoother/src/velocity_smoother.cpp | 1 - 6 files changed, 1 insertion(+), 24 deletions(-) diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 5a2bf057319..718e0561103 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -200,15 +200,7 @@ BehaviorServer::on_activate(const rclcpp_lifecycle::State & /*state*/) } // create bond connection - createBond(); - - if (get_node_options().use_intra_process_comms()) - { - RCLCPP_INFO(get_logger(), "use_intra_process_comms is true for %s", get_name()); - } else { - RCLCPP_INFO(get_logger(), "use_intra_process_comms is false for %s", get_name()); - } - + createBond(); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 180d982407e..f04bc8b5db3 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -183,7 +183,6 @@ CollisionMonitor::on_shutdown(const rclcpp_lifecycle::State & /*state*/) void CollisionMonitor::cmdVelInCallback(geometry_msgs::msg::Twist::ConstSharedPtr msg) { // If message contains NaN or Inf, ignore - RCLCPP_INFO(get_logger(), "Recived velocity: x: %f on address: %lu", msg->linear.x, reinterpret_cast(msg.get())); if (!nav2_util::validateTwist(*msg)) { RCLCPP_ERROR(get_logger(), "Velocity message contains NaNs or Infs! Ignoring as invalid!"); return; diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index f9a6daf91aa..45bea71e0e5 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -185,7 +185,6 @@ void Costmap2DPublisher::publishCostmap() { if (costmap_raw_pub_->get_subscription_count() > 0) { prepareCostmap(); - RCLCPP_INFO(logger_, "Publishing costmap %i on address: %lu", costmap_raw_->header.stamp.nanosec, reinterpret_cast(costmap_raw_.get())); costmap_raw_pub_->publish(std::move(costmap_raw_)); } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 04d019230df..d953cd6a325 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -209,8 +209,6 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Initialized costmap filter \"%s\"", filter_names_[i].c_str()); } - auto qos = rclcpp::SystemDefaultsQoS(); - RCLCPP_INFO(get_logger(), "Created SystemDefaultsQoS with depth %li", qos.depth()); // Create the publishers and subscribers footprint_sub_ = create_subscription( "footprint", @@ -545,14 +543,6 @@ Costmap2DROS::updateMap() auto footprint = std::make_unique(); footprint->header = pose.header; transformFootprint(x, y, yaw, padded_footprint_, *footprint); - - RCLCPP_DEBUG(get_logger(), "Publishing footprint"); - if (get_node_options().use_intra_process_comms()) - { - RCLCPP_INFO(get_logger(), "use_intra_process_comms is true for %s", get_name()); - } else { - RCLCPP_INFO(get_logger(), "use_intra_process_comms is false for %s", get_name()); - } RCLCPP_DEBUG(get_logger(), "Publishing footprint"); footprint_pub_->publish(std::move(footprint)); diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index 874ba631095..d26320a0be4 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -26,7 +26,6 @@ CostmapSubscriber::CostmapSubscriber( : topic_name_(topic_name) { auto node = parent.lock(); - logger_ = node->get_logger(); costmap_sub_ = node->create_subscription( topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), @@ -89,7 +88,6 @@ void CostmapSubscriber::toCostmap2D() void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::ConstSharedPtr msg) { - RCLCPP_INFO(logger_, "Received costmap %i on topic %s, on address: %lu ", msg->header.stamp.nanosec, topic_name_.c_str(), reinterpret_cast(msg.get())); std::atomic_store(&costmap_msg_, msg); if (!costmap_received_) { costmap_received_ = true; diff --git a/nav2_velocity_smoother/src/velocity_smoother.cpp b/nav2_velocity_smoother/src/velocity_smoother.cpp index 41df5b350b5..3742cbb1eee 100644 --- a/nav2_velocity_smoother/src/velocity_smoother.cpp +++ b/nav2_velocity_smoother/src/velocity_smoother.cpp @@ -332,7 +332,6 @@ void VelocitySmoother::smootherTimer() cmd_vel->angular.z = fabs(cmd_vel->angular.z) < deadband_velocities_[2] ? 0.0 : cmd_vel->angular.z; - RCLCPP_INFO(get_logger(), "Publishing velocity: x: %f on address: %lu", cmd_vel->linear.x, reinterpret_cast(cmd_vel.get())); smoothed_cmd_pub_->publish(std::move(cmd_vel)); } From f1e9fcbe1361a89fd3ca542039bf0dfc8c8d9641 Mon Sep 17 00:00:00 2001 From: HovorunB Date: Wed, 31 Jan 2024 11:16:59 +0100 Subject: [PATCH 4/4] format --- nav2_behaviors/src/behavior_server.cpp | 2 +- nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp | 1 - nav2_costmap_2d/src/costmap_2d_publisher.cpp | 1 - nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- 4 files changed, 2 insertions(+), 4 deletions(-) diff --git a/nav2_behaviors/src/behavior_server.cpp b/nav2_behaviors/src/behavior_server.cpp index 718e0561103..b5ec19ba626 100644 --- a/nav2_behaviors/src/behavior_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -200,7 +200,7 @@ BehaviorServer::on_activate(const rclcpp_lifecycle::State & /*state*/) } // create bond connection - createBond(); + createBond(); return nav2_util::CallbackReturn::SUCCESS; } diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index 48bd43f8e81..491ea5dc382 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -71,7 +71,6 @@ class CostmapSubscriber std::string topic_name_; bool costmap_received_{false}; rclcpp::Subscription::SharedPtr costmap_sub_; - rclcpp::Logger logger_{rclcpp::get_logger("costmap_sub")}; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 45bea71e0e5..87881bc7caa 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -185,7 +185,6 @@ void Costmap2DPublisher::publishCostmap() { if (costmap_raw_pub_->get_subscription_count() > 0) { prepareCostmap(); - costmap_raw_pub_->publish(std::move(costmap_raw_)); } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index d953cd6a325..cf264429c0d 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -543,7 +543,7 @@ Costmap2DROS::updateMap() auto footprint = std::make_unique(); footprint->header = pose.header; transformFootprint(x, y, yaw, padded_footprint_, *footprint); - + RCLCPP_DEBUG(get_logger(), "Publishing footprint"); footprint_pub_->publish(std::move(footprint)); initialized_ = true;