diff --git a/slam_gmapping/launch/slam_gmapping.launch.py b/slam_gmapping/launch/slam_gmapping.launch.py index 0f99610..4306cf0 100644 --- a/slam_gmapping/launch/slam_gmapping.launch.py +++ b/slam_gmapping/launch/slam_gmapping.launch.py @@ -1,12 +1,41 @@ from launch import LaunchDescription -from launch.substitutions import EnvironmentVariable -import launch.actions -import launch_ros.actions +from launch.actions import DeclareLaunchArgument, GroupAction +from launch_ros.actions import Node, PushRosNamespace +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration def generate_launch_description(): - use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time', default='true') - return LaunchDescription([ - launch_ros.actions.Node( - package='slam_gmapping', node_executable='slam_gmapping', output='screen', parameters=[{'use_sim_time':use_sim_time}]), - ]) + namespace = LaunchConfiguration("namespace") + use_sim_time = LaunchConfiguration("use_sim_time") + declare_namespace_cmd = DeclareLaunchArgument( + "namespace", default_value="", description="Top-level namespace" + ) + declare_use_sim_time_cmd = DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="Use simulation (Gazebo) clock if true", + ) + + # 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 + remappings = [("/tf", "tf"), ("/tf_static", "tf_static")] + + # Create the launch description and populate + ld = LaunchDescription( + [ + declare_namespace_cmd, + declare_use_sim_time_cmd, + Node( + package="slam_gmapping", + namespace=namespace, + executable="slam_gmapping", + output="screen", + parameters=[{"use_sim_time": use_sim_time}], + remappings=remappings, + ), + ] + ) + return ld diff --git a/slam_gmapping/src/slam_gmapping.cpp b/slam_gmapping/src/slam_gmapping.cpp index 8500f8e..a5d2eb5 100644 --- a/slam_gmapping/src/slam_gmapping.cpp +++ b/slam_gmapping/src/slam_gmapping.cpp @@ -99,8 +99,8 @@ void SlamGmapping::init() { void SlamGmapping::startLiveSlam() { entropy_publisher_ = this->create_publisher("entropy", rclcpp::SystemDefaultsQoS()); - sst_ = this->create_publisher("map", rclcpp::SystemDefaultsQoS()); - sstm_ = this->create_publisher("map_metadata", rclcpp::SystemDefaultsQoS()); + sst_ = this->create_publisher("map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + sstm_ = this->create_publisher("map_metadata", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); scan_filter_sub_ = std::make_shared> (node_, "scan", rclcpp::SensorDataQoS().get_rmw_qos_profile()); // sub_ = this->create_subscription(