Skip to content

Commit

Permalink
Add namespace (#10)
Browse files Browse the repository at this point in the history
* Add namespace

* fix map saving

* fix healthcheck namespace

* healthcheck
  • Loading branch information
rafal-gorecki authored Jan 23, 2024
1 parent 202fde9 commit 136441d
Show file tree
Hide file tree
Showing 7 changed files with 850 additions and 17 deletions.
15 changes: 5 additions & 10 deletions Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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 && \
Expand All @@ -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"]
177 changes: 177 additions & 0 deletions husarion_utils/bringup_launch.py
Original file line number Diff line number Diff line change
@@ -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),
]
)
24 changes: 17 additions & 7 deletions healthcheck.cpp → husarion_utils/healthcheck.cpp
100755 → 100644
Original file line number Diff line number Diff line change
Expand Up @@ -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_ =
Expand All @@ -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 {
Expand Down Expand Up @@ -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";
Expand All @@ -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");
}
}
}
Expand All @@ -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
Expand Down
File renamed without changes.
Loading

0 comments on commit 136441d

Please sign in to comment.