AMRNAV-6258 Use intra process communication

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<tf2_ros::TransformListener>(*tf_buffer_);
 
   state_pub_ = this->create_publisher<nav2_msgs::msg::CollisionDetectorState>(
-    "collision_detector_state", rclcpp::SystemDefaultsQoS());
+    "collision_detector_state", rclcpp::SystemDefaultsQoS().keep_last(1));
 
   collision_points_marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>(
     "~/collision_points_marker", 1);

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<geometry_msgs::msg::PolygonStamped>(
       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<geometry_msgs::msg::PolygonStamped>(
       polygon_pub_topic, polygon_qos);
   }

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<nav2_costmap_2d::Costmap2DROS>(
     "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()

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

nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp

@@ -63,11 +63,11 @@ 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<Costmap2D> 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<nav2_msgs::msg::Costmap>::SharedPtr costmap_sub_;

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_;
 };

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);

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(use_intra_process_comms)),
   name_(name),
   parent_namespace_(parent_namespace),
   default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"},
@@ -215,7 +216,7 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
     std::bind(&Costmap2DROS::setRobotFootprintPolygon, this, std::placeholders::_1));
 
   footprint_pub_ = create_publisher<geometry_msgs::msg::PolygonStamped>(
-    "published_footprint", rclcpp::SystemDefaultsQoS());
+    "published_footprint", rclcpp::SystemDefaultsQoS().keep_last(1));
 
   costmap_publisher_ = std::make_unique<Costmap2DPublisher>(
     shared_from_this(),

nav2_costmap_2d/src/costmap_subscriber.cpp

@@ -86,7 +86,7 @@ void CostmapSubscriber::toCostmap2D()
   }
 }
 
-void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg)
+void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::ConstSharedPtr msg)
 {
   std::atomic_store(&costmap_msg_, msg);
   if (!costmap_received_) {

nav2_planner/src/planner_server.cpp

@@ -65,7 +65,7 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
   // Setup the global costmap
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "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()