[loopback_sim] Publish clock, [nav2_costmap_2d] Fix Qos (#4726)

* Publish /clock from loopback sim

Signed-off-by: Adi Vardi <[email protected]>

* [nav2_costmap_2d] Fix obstacle_layer trying to use RELIABLE QoS

Use QoS profile from rclcpp::SensorDataQoS() instead of rmw_qos_profile_t.
This solves an issue where the subscriber uses RELIABLE setting even when initialized from rmw_qos_profile_sensor_data.
In addition the Subscriber(..., rmw_qos_profile_t) constructor is deprecated in favor of Subscriber(..., rclcpp::QoS)

Signed-off-by: Adi Vardi <[email protected]>

* [nav2_smac_planner] fix typos

Signed-off-by: Adi Vardi <[email protected]>

* Use single quotes

Signed-off-by: Adi Vardi <[email protected]>

---------

Signed-off-by: Adi Vardi <[email protected]>

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -225,8 +225,7 @@ void ObstacleLayer::onInitialize()
       source.c_str(), topic.c_str(),
       global_frame_.c_str(), expected_update_rate, observation_keep_time);
 
-    rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data;
-    custom_qos_profile.depth = 50;
+    const auto custom_qos_profile = rclcpp::SensorDataQoS().keep_last(50);
 
     // create a callback for the topic
     if (data_type == "LaserScan") {

nav2_loopback_sim/README.md

@@ -8,7 +8,7 @@ This was created by Steve Macenski of [Open Navigation LLC](https://opennav.org)
 
 It is drop-in replacable with AMR simulators and global localization by providing:
 - Map -> Odom transform
-- Odom -> Base Link transform, `nav_msgs/Odometry` odometry 
+- Odom -> Base Link transform, `nav_msgs/Odometry` odometry
 - Accepts the standard `/initialpose` topic for transporting the robot to another location
 
 Note: This does not provide sensor data, so it is required that the global (and probably local) costmap contain the `StaticLayer` to avoid obstacles.
@@ -33,12 +33,15 @@ ros2 launch nav2_bringup tb4_loopback_simulation.launch.py  # Nav2 integrated na
 
 ### Parameters
 
-- `update_duration`: the duration between updates (default 0.01 -- 100hz)
+- `update_duration`: The duration between updates (default 0.01 -- 100hz)
 - `base_frame_id`: The base frame to use (default `base_link`)
 - `odom_frame_id`: The odom frame to use (default `odom`)
 - `map_frame_id`: The map frame to use (default `map`)
 - `scan_frame_id`: The can frame to use to publish a scan to keep the collision monitor fed and happy (default `base_scan` for TB3, `rplidar_link` for TB4)
 - `enable_stamped_cmd_vel`: Whether cmd_vel is stamped or unstamped (i.e. Twist or TwistStamped). Default `false` for `Twist`.
+- `scan_publish_dur`: : The duration between publishing scan (default 0.1s -- 10hz)
+- `publish_map_odom_tf`: Whether or not to publish tf from `map_frame_id` to `odom_frame_id` (default `true`)
+- `publish_clock`: Whether or not to publish simulated clock to `/clock` (default `true`)
 
 ### Topics
 
@@ -47,6 +50,7 @@ This node subscribes to:
 - `cmd_vel`: Nav2's output twist to get the commanded velocity
 
 This node publishes:
+- `clock`: To publish a simulation clock for all other nodes with `use_sim_time=True`
 - `odom`: To publish odometry from twist
 - `tf`: To publish map->odom and odom->base_link transforms
-- `scan`: To publish a bogus max range laser scan sensor to make the collision monitor happy
+- `scan`: To publish a range laser scan sensor based on the static map

nav2_loopback_sim/nav2_loopback_sim/loopback_simulator.py

@@ -24,6 +24,7 @@
 from rclpy.duration import Duration
 from rclpy.node import Node
 from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
+from rosgraph_msgs.msg import Clock
 from sensor_msgs.msg import LaserScan
 from tf2_ros import Buffer, TransformBroadcaster, TransformListener
 import tf_transformations
@@ -84,6 +85,9 @@ def __init__(self):
         self.publish_map_odom_tf = self.get_parameter(
             'publish_map_odom_tf').get_parameter_value().bool_value
 
+        self.declare_parameter('publish_clock', True)
+        self.publish_clock = self.get_parameter('publish_clock').get_parameter_value().bool_value
+
         self.t_map_to_odom = TransformStamped()
         self.t_map_to_odom.header.frame_id = self.map_frame_id
         self.t_map_to_odom.child_frame_id = self.odom_frame_id
@@ -112,6 +116,10 @@ def __init__(self):
             depth=10)
         self.scan_pub = self.create_publisher(LaserScan, 'scan', sensor_qos)
 
+        if self.publish_clock:
+            self.clock_timer = self.create_timer(0.1, self.clockTimerCallback)
+            self.clock_pub = self.create_publisher(Clock, '/clock', 10)
+
         self.setupTimer = self.create_timer(0.1, self.setupTimerCallback)
 
         self.map_client = self.create_client(GetMap, '/map_server/map')
@@ -139,6 +147,11 @@ def setupTimerCallback(self):
         if self.mat_base_to_laser is None:
             self.getBaseToLaserTf()
 
+    def clockTimerCallback(self):
+        msg = Clock()
+        msg.clock = self.get_clock().now().to_msg()
+        self.clock_pub.publish(msg)
+
     def cmdVelCallback(self, msg):
         self.debug('Received cmd_vel')
         if self.initial_pose is None: