diff --git a/Dockerfile b/Dockerfile index d7e2ff3..c6769ee 100644 --- a/Dockerfile +++ b/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"] diff --git a/husarion_utils/bringup_launch.py b/husarion_utils/bringup_launch.py new file mode 100644 index 0000000..90f70ff --- /dev/null +++ b/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={"": namespace} + ) + else: + params_file = ReplaceString( + source_file=params_file, replacements={"/": ""} + ) + + 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), + ] + ) diff --git a/healthcheck.cpp b/husarion_utils/healthcheck.cpp old mode 100755 new mode 100644 similarity index 84% rename from healthcheck.cpp rename to husarion_utils/healthcheck.cpp index f1d8327..d83324b --- a/healthcheck.cpp +++ b/husarion_utils/healthcheck.cpp @@ -22,7 +22,7 @@ class HealthCheckNode : public rclcpp::Node { qos.transient_local(); qos.reliable(); map_subscriber_ = create_subscription( - "/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("/map_saver/save_map"); + create_client("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(); 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 saveMapPeriod; + std::string ns_prefix; rclcpp::Subscription::SharedPtr map_subscriber_; rclcpp::Subscription::SharedPtr diff --git a/healthcheck.sh b/husarion_utils/healthcheck.sh similarity index 100% rename from healthcheck.sh rename to husarion_utils/healthcheck.sh diff --git a/husarion_utils/localization_launch.py b/husarion_utils/localization_launch.py new file mode 100644 index 0000000..92a8ac0 --- /dev/null +++ b/husarion_utils/localization_launch.py @@ -0,0 +1,205 @@ +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory("nav2_bringup") + + namespace = LaunchConfiguration("namespace") + map_yaml_file = LaunchConfiguration("map") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + params_file = LaunchConfiguration("params_file") + use_composition = LaunchConfiguration("use_composition") + container_name = LaunchConfiguration("container_name") + container_name_full = (namespace, "/", container_name) + use_respawn = LaunchConfiguration("use_respawn") + log_level = LaunchConfiguration("log_level") + + lifecycle_nodes = ["map_server", "amcl"] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + 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} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ), + allow_substs=True, + ) + + stdout_linebuf_envvar = SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1") + + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + + declare_map_yaml_cmd = DeclareLaunchArgument( + "map", description="Full path to map yaml file to load" + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="false", description="Use simulation (Gazebo) clock if true" + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + "use_composition", default_value="False", description="Use composed bringup if True" + ) + + declare_container_name_cmd = DeclareLaunchArgument( + "container_name", + default_value="nav2_container", + description="the name of container that nodes will load in if use composition", + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + "use_respawn", + default_value="False", + description="Whether to respawn if a node crashes. Applied when composition is disabled.", + ) + + declare_log_level_cmd = DeclareLaunchArgument( + "log_level", default_value="info", description="log level" + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(["not ", use_composition])), + actions=[ + Node( + package="nav2_map_server", + executable="map_server", + name="map_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + remappings=remappings, + ), + Node( + package="nav2_amcl", + executable="amcl", + name="amcl", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + remappings=remappings, + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_localization", + output="screen", + arguments=["--ros-args", "--log-level", log_level], + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": autostart}, + {"node_names": lifecycle_nodes}, + ], + ), + ], + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package="nav2_map_server", + plugin="nav2_map_server::MapServer", + name="map_server", + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package="nav2_amcl", + plugin="nav2_amcl::AmclNode", + name="amcl", + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package="nav2_lifecycle_manager", + plugin="nav2_lifecycle_manager::LifecycleManager", + name="lifecycle_manager_localization", + parameters=[ + { + "use_sim_time": use_sim_time, + "autostart": autostart, + "node_names": lifecycle_nodes, + } + ], + ), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + + # Add the actions to launch all of the localiztion nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/husarion_utils/navigation_launch.py b/husarion_utils/navigation_launch.py new file mode 100644 index 0000000..dec2feb --- /dev/null +++ b/husarion_utils/navigation_launch.py @@ -0,0 +1,299 @@ +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.conditions import IfCondition +from launch.substitutions import EnvironmentVariable, LaunchConfiguration, PythonExpression +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node +from launch_ros.descriptions import ComposableNode, ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory("nav2_bringup") + + namespace = LaunchConfiguration("namespace") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + params_file = LaunchConfiguration("params_file") + use_composition = LaunchConfiguration("use_composition") + container_name = LaunchConfiguration("container_name") + container_name_full = (namespace, "/", container_name) + use_respawn = LaunchConfiguration("use_respawn") + log_level = LaunchConfiguration("log_level") + + lifecycle_nodes = [ + "controller_server", + "smoother_server", + "planner_server", + "behavior_server", + "bt_navigator", + "waypoint_follower", + "velocity_smoother", + ] + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Create our own temporary YAML files that include substitutions + param_substitutions = {"use_sim_time": use_sim_time, "autostart": autostart} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ), + allow_substs=True, + ) + + stdout_linebuf_envvar = SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1") + + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", + default_value=EnvironmentVariable("ROBOT_NAMESPACE", default_value=""), + description="Top-level namespace", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="false", description="Use simulation (Gazebo) clock if true" + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value=os.path.join(bringup_dir, "params", "nav2_params.yaml"), + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="true", description="Automatically startup the nav2 stack" + ) + + declare_use_composition_cmd = DeclareLaunchArgument( + "use_composition", default_value="False", description="Use composed bringup if True" + ) + + declare_container_name_cmd = DeclareLaunchArgument( + "container_name", + default_value="nav2_container", + description="the name of container that nodes will load in if use composition", + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + "use_respawn", + default_value="False", + description="Whether to respawn if a node crashes. Applied when composition is disabled.", + ) + + declare_log_level_cmd = DeclareLaunchArgument( + "log_level", default_value="info", description="log level" + ) + + load_nodes = GroupAction( + condition=IfCondition(PythonExpression(["not ", use_composition])), + actions=[ + Node( + package="nav2_controller", + executable="controller_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + remappings=remappings + [("cmd_vel", "cmd_vel_nav")], + ), + Node( + package="nav2_smoother", + executable="smoother_server", + name="smoother_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + remappings=remappings, + ), + Node( + package="nav2_planner", + executable="planner_server", + name="planner_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + remappings=remappings, + ), + Node( + package="nav2_behaviors", + executable="behavior_server", + name="behavior_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + remappings=remappings, + ), + Node( + package="nav2_bt_navigator", + executable="bt_navigator", + name="bt_navigator", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + remappings=remappings, + ), + Node( + package="nav2_waypoint_follower", + executable="waypoint_follower", + name="waypoint_follower", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + remappings=remappings, + ), + Node( + package="nav2_velocity_smoother", + executable="velocity_smoother", + name="velocity_smoother", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params], + arguments=["--ros-args", "--log-level", log_level], + remappings=remappings + + [("cmd_vel", "cmd_vel_nav"), ("cmd_vel_smoothed", "cmd_vel")], + ), + Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_navigation", + output="screen", + arguments=["--ros-args", "--log-level", log_level], + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": autostart}, + {"node_names": lifecycle_nodes}, + ], + ), + ], + ) + + load_composable_nodes = LoadComposableNodes( + condition=IfCondition(use_composition), + target_container=container_name_full, + composable_node_descriptions=[ + ComposableNode( + package="nav2_controller", + plugin="nav2_controller::ControllerServer", + name="controller_server", + parameters=[configured_params], + remappings=remappings + [("cmd_vel", "cmd_vel_nav")], + ), + ComposableNode( + package="nav2_smoother", + plugin="nav2_smoother::SmootherServer", + name="smoother_server", + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package="nav2_planner", + plugin="nav2_planner::PlannerServer", + name="planner_server", + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package="nav2_behaviors", + plugin="behavior_server::BehaviorServer", + name="behavior_server", + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package="nav2_bt_navigator", + plugin="nav2_bt_navigator::BtNavigator", + name="bt_navigator", + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package="nav2_waypoint_follower", + plugin="nav2_waypoint_follower::WaypointFollower", + name="waypoint_follower", + parameters=[configured_params], + remappings=remappings, + ), + ComposableNode( + package="nav2_velocity_smoother", + plugin="nav2_velocity_smoother::VelocitySmoother", + name="velocity_smoother", + parameters=[configured_params], + remappings=remappings + + [("cmd_vel", "cmd_vel_nav"), ("cmd_vel_smoothed", "cmd_vel")], + ), + ComposableNode( + package="nav2_lifecycle_manager", + plugin="nav2_lifecycle_manager::LifecycleManager", + name="lifecycle_manager_navigation", + parameters=[ + { + "use_sim_time": use_sim_time, + "autostart": autostart, + "node_names": lifecycle_nodes, + } + ], + ), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/husarion_utils/slam_launch.py b/husarion_utils/slam_launch.py new file mode 100644 index 0000000..3630b32 --- /dev/null +++ b/husarion_utils/slam_launch.py @@ -0,0 +1,147 @@ +# Copyright (c) 2020 Samsung Research Russia +# +# 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 ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, SetRemap +from launch_ros.descriptions import ParameterFile +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Input parameters declaration + namespace = LaunchConfiguration("namespace") + params_file = LaunchConfiguration("params_file") + use_sim_time = LaunchConfiguration("use_sim_time") + autostart = LaunchConfiguration("autostart") + use_respawn = LaunchConfiguration("use_respawn") + log_level = LaunchConfiguration("log_level") + + # Variables + lifecycle_nodes = ["map_saver"] + + # Getting directories and launch-files + slam_toolbox_dir = get_package_share_directory("slam_toolbox") + slam_launch_file = os.path.join(slam_toolbox_dir, "launch", "online_sync_launch.py") + + # Create our own temporary YAML files that include substitutions + param_substitutions = {"use_sim_time": use_sim_time} + + configured_params = ParameterFile( + RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True, + ), + allow_substs=True, + ) + + # Declare the launch arguments + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="robot", description="Top-level namespace" + ) + + declare_params_file_cmd = DeclareLaunchArgument( + "params_file", + default_value="/params.yaml", + description="Full path to the ROS2 parameters file to use for all launched nodes", + ) + + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", default_value="True", description="Use simulation (Gazebo) clock if true" + ) + + declare_autostart_cmd = DeclareLaunchArgument( + "autostart", default_value="True", description="Automatically startup the nav2 stack" + ) + + declare_use_respawn_cmd = DeclareLaunchArgument( + "use_respawn", + default_value="False", + description="Whether to respawn if a node crashes. Applied when composition is disabled.", + ) + + declare_log_level_cmd = DeclareLaunchArgument( + "log_level", default_value="info", description="log level" + ) + + # Nodes launching commands + + start_map_saver_server_cmd = Node( + package="nav2_map_server", + executable="map_saver_server", + output="screen", + respawn=use_respawn, + respawn_delay=2.0, + arguments=["--ros-args", "--log-level", log_level], + parameters=[configured_params], + ) + + start_lifecycle_manager_cmd = Node( + package="nav2_lifecycle_manager", + executable="lifecycle_manager", + name="lifecycle_manager_slam", + output="screen", + arguments=["--ros-args", "--log-level", log_level], + parameters=[ + {"use_sim_time": use_sim_time}, + {"autostart": autostart}, + {"node_names": lifecycle_nodes}, + ], + ) + + start_slam_toolbox_cmd_with_params = IncludeLaunchDescription( + PythonLaunchDescriptionSource(slam_launch_file), + launch_arguments={"use_sim_time": use_sim_time, "slam_params_file": params_file}.items(), + ) + + group_actions = [] + + # Remapping + group_actions.append(SetRemap("/map", "map")) + group_actions.append(SetRemap("/map_metadata", "map_metadata")) + group_actions.append(SetRemap("/tf", "tf")) + group_actions.append(SetRemap("/tf_static", "tf_static")) + group_actions.append(SetRemap("/scan", "scan")) + + # Running Map Saver Server + group_actions.append(start_map_saver_server_cmd) + group_actions.append(start_lifecycle_manager_cmd) + + # Running SLAM Toolbox (Only one of them will be run) + group_actions.append(start_slam_toolbox_cmd_with_params) + + slam_ns = GroupAction(actions=group_actions) + + ld = LaunchDescription() + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_respawn_cmd) + ld.add_action(declare_log_level_cmd) + + # Running SLAM + ld.add_action(slam_ns) + + return ld