Add namespace

Dockerfile

@@ -7,7 +7,7 @@ ARG PREFIX
 
 WORKDIR /ros2_ws
 
-COPY ./healthcheck.cpp /
+COPY ./husarion_utils/healthcheck.cpp /husarion_utils/healthcheck.cpp
 
 # In version 1.0.12 MPPI doesn't work on RPi4 (build from source)
 # Install everything needed
@@ -32,7 +32,7 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \
                 healthcheck_node \
                 DESTINATION lib/${PROJECT_NAME})' \
             /ros2_ws/src/healthcheck_pkg/CMakeLists.txt && \
-    mv /healthcheck.cpp /ros2_ws/src/healthcheck_pkg/src/ && \
+    mv /husarion_utils/healthcheck.cpp /ros2_ws/src/healthcheck_pkg/src/ && \
     cd .. && \
     # Install dependencies
     rm -rf /etc/ros/rosdep/sources.list.d/20-default.list && \
@@ -55,11 +55,6 @@ RUN MYDISTRO=${PREFIX:-ros}; MYDISTRO=${MYDISTRO//-/} && \
     apt-get clean && \
     rm -rf /var/lib/apt/lists/*
 
-RUN entrypoint_file=$(if [ -f "/ros_entrypoint.sh" ]; then echo "/ros_entrypoint.sh"; else echo "/vulcanexus_entrypoint.sh"; fi) && \
-    sed -i '/test -f "\/ros2_ws\/install\/setup.bash" && source "\/ros2_ws\/install\/setup.bash"/a \
-            ros2 run healthcheck_pkg healthcheck_node &' \
-            $entrypoint_file
-
-COPY ./healthcheck.sh /
-HEALTHCHECK --interval=5s --timeout=2s  --start-period=5s --retries=4 \
-    CMD ["/healthcheck.sh"]
+COPY ./husarion_utils /husarion_utils
+HEALTHCHECK --interval=2s --timeout=1s --start-period=45s --retries=2 \
+    CMD ["/husarion_utils/healthcheck.sh"]

husarion_utils/bringup_launch.py

@@ -0,0 +1,177 @@
+# Copyright (c) 2018 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.
+
+import os
+
+from launch import LaunchDescription
+from launch.actions import (
+    DeclareLaunchArgument,
+    GroupAction,
+    IncludeLaunchDescription,
+    SetEnvironmentVariable,
+    OpaqueFunction,
+)
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PythonExpression
+from launch_ros.actions import Node
+from launch_ros.actions import PushRosNamespace
+from launch_ros.descriptions import ParameterFile
+from nav2_common.launch import RewrittenYaml, ReplaceString
+
+
+def launch_setup(context, *args, **kwargs):
+    # Get the launch directory
+    launch_dir = "/husarion_utils"
+
+    # Create the launch configuration variables
+    namespace = LaunchConfiguration("namespace").perform(context)
+    map_yaml_file = LaunchConfiguration("map").perform(context)
+    use_sim_time = LaunchConfiguration("use_sim_time").perform(context)
+    params_file = LaunchConfiguration("params_file").perform(context)
+    slam = LaunchConfiguration("slam").perform(context)
+    autostart = LaunchConfiguration("autostart").perform(context)
+    use_composition = LaunchConfiguration("use_composition").perform(context)
+    use_respawn = LaunchConfiguration("use_respawn").perform(context)
+    log_level = LaunchConfiguration("log_level").perform(context)
+
+    remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
+
+    # Create our own temporary YAML files that include substitutions
+    param_substitutions = {"use_sim_time": use_sim_time, "yaml_filename": map_yaml_file}
+
+    if namespace:
+        params_file = ReplaceString(
+            source_file=params_file, replacements={"<robot_namespace>": namespace}
+        )
+    else:
+        params_file = ReplaceString(
+            source_file=params_file, replacements={"<robot_namespace>/": ""}
+        )
+
+    configured_params = ParameterFile(
+        RewrittenYaml(
+            source_file=params_file,
+            root_key=namespace,
+            param_rewrites=param_substitutions,
+            convert_types=True,
+        ),
+        allow_substs=True,
+    )
+
+    # Specify the actions
+    bringup_cmd_group = GroupAction(
+        [
+            PushRosNamespace(namespace),
+            Node(
+                condition=IfCondition(use_composition),
+                name="nav2_container",
+                package="rclcpp_components",
+                executable="component_container_isolated",
+                parameters=[configured_params, {"autostart": autostart}],
+                arguments=["--ros-args", "--log-level", log_level],
+                remappings=remappings,
+                output="screen",
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(os.path.join(launch_dir, "slam_launch.py")),
+                condition=IfCondition(slam),
+                launch_arguments={
+                    "namespace": namespace,
+                    "use_sim_time": use_sim_time,
+                    "autostart": autostart,
+                    "use_respawn": use_respawn,
+                    "params_file": params_file,
+                }.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(os.path.join(launch_dir, "localization_launch.py")),
+                condition=IfCondition(PythonExpression(["not ", slam])),
+                launch_arguments={
+                    "namespace": namespace,
+                    "map": map_yaml_file,
+                    "use_sim_time": use_sim_time,
+                    "autostart": autostart,
+                    "params_file": params_file,
+                    "use_composition": use_composition,
+                    "use_respawn": use_respawn,
+                    "container_name": "nav2_container",
+                }.items(),
+            ),
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(os.path.join(launch_dir, "navigation_launch.py")),
+                launch_arguments={
+                    "namespace": namespace,
+                    "use_sim_time": use_sim_time,
+                    "autostart": autostart,
+                    "params_file": params_file,
+                    "use_composition": use_composition,
+                    "use_respawn": use_respawn,
+                    "container_name": "nav2_container",
+                }.items(),
+            ),
+        ]
+    )
+
+    healthcheck_node = Node(
+        package="healthcheck_pkg",
+        executable="healthcheck_node",
+        name="healthcheck_navigation",
+        namespace=namespace,
+        output="screen",
+    )
+
+    return [bringup_cmd_group, healthcheck_node]
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"),
+            DeclareLaunchArgument(
+                "namespace",
+                default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""),
+                description="Top-level namespace",
+            ),
+            DeclareLaunchArgument("slam", default_value="False", description="Whether run a SLAM"),
+            DeclareLaunchArgument("map", description="Full path to map yaml file to load"),
+            DeclareLaunchArgument(
+                "use_sim_time",
+                default_value="false",
+                description="Use simulation (Gazebo) clock if true",
+            ),
+            DeclareLaunchArgument(
+                "params_file",
+                default_value=os.path.join("/params.yaml"),
+                description="Full path to the ROS2 parameters file to use for all launched nodes",
+            ),
+            DeclareLaunchArgument(
+                "autostart",
+                default_value="true",
+                description="Automatically startup the nav2 stack",
+            ),
+            DeclareLaunchArgument(
+                "use_composition",
+                default_value="True",
+                description="Whether to use composed bringup",
+            ),
+            DeclareLaunchArgument(
+                "use_respawn",
+                default_value="False",
+                description="Whether to respawn if a node crashes. Applied when composition is disabled.",
+            ),
+            DeclareLaunchArgument("log_level", default_value="info", description="log level"),
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

healthcheck.cpp → husarion_utils/healthcheck.cpp

@@ -22,7 +22,7 @@ class HealthCheckNode : public rclcpp::Node {
     qos.transient_local();
     qos.reliable();
     map_subscriber_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
-        "/map", qos,
+        "map", qos,
         std::bind(&HealthCheckNode::mapCallback, this, std::placeholders::_1));
 
     controller_subscriber_ =
@@ -32,9 +32,18 @@ class HealthCheckNode : public rclcpp::Node {
                       std::placeholders::_1));
 
     save_map_client_ =
-        create_client<nav2_msgs::srv::SaveMap>("/map_saver/save_map");
+        create_client<nav2_msgs::srv::SaveMap>("map_saver/save_map");
+
+    // Read the ROBOT_NAMESPACE environment variable
+    const char *robotNamespaceEnv = std::getenv("ROBOT_NAMESPACE");
+    ns_prefix = (robotNamespaceEnv != nullptr)
+                    ? "/" + std::string(robotNamespaceEnv)
+                    : "";
+    if (!ns_prefix.empty()) {
+      RCLCPP_INFO(get_logger(), "ROBOT_NAMESPACE: %s", ns_prefix.c_str());
+    }
 
-    // Read the environment variable
+    // Read the SAVE_MAP_PERIOD environment variable
     const char *saveMapPeriodEnv = std::getenv("SAVE_MAP_PERIOD");
     if (saveMapPeriodEnv != nullptr) {
       try {
@@ -69,11 +78,11 @@ class HealthCheckNode : public rclcpp::Node {
     if (saveMapPeriod != 0s) {
       if (elapsed_time > saveMapPeriod) {
         if (save_map_client_->wait_for_service(SAVE_MAP_CONNECTION_TIMEOUT)) {
-          RCLCPP_DEBUG(get_logger(), "/map_saver/save_map service available");
+          RCLCPP_DEBUG(get_logger(), "map_saver/save_map service available");
           auto request = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
           request->free_thresh = 0.25;
           request->occupied_thresh = 0.65;
-          request->map_topic = "/map";
+          request->map_topic = ns_prefix + "/map";
           request->map_url = "/maps/map";
           request->map_mode = "trinary";
           request->image_format = "png";
@@ -87,10 +96,10 @@ class HealthCheckNode : public rclcpp::Node {
             last_saved_map_time = steady_clock::now();
           } else {
             RCLCPP_WARN(get_logger(),
-                        "/map_saver/save_map service response didn't arrived");
+                        "save_map service response didn't arrived");
           }
         } else {
-          RCLCPP_DEBUG(get_logger(), "/map_saver/save_map service unavailable");
+          RCLCPP_DEBUG(get_logger(), "save_map service unavailable");
         }
       }
     }
@@ -100,6 +109,7 @@ class HealthCheckNode : public rclcpp::Node {
   bool map_exist;
   bool is_controller_active;
   duration<double> saveMapPeriod;
+  std::string ns_prefix;
 
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr map_subscriber_;
   rclcpp::Subscription<lifecycle_msgs::msg::TransitionEvent>::SharedPtr