Use node time instead of sim time

CMakeLists.txt

@@ -45,6 +45,7 @@ add_library(
     ${PROJECT_NAME} SHARED
     src/RealSensePlugin.cpp
     src/gazebo_ros_realsense.cpp
+    src/ros_qos_specifier.cpp
     )
 
 ament_target_dependencies(${PROJECT_NAME} ${dependencies})

include/realsense_gazebo_plugin/RealSensePlugin.h

@@ -126,6 +126,8 @@ class RealSensePlugin : public ModelPlugin {
 
   bool pointCloud_ = false;
   std::string pointCloudTopic_;
+  std::basic_string<char> pointCloudQos;
+  std::basic_string<char> colorQos;
   double pointCloudCutOff_, pointCloudCutOffMax_;
 
   double colorUpdateRate_;

include/realsense_gazebo_plugin/gazebo_ros_realsense.h

@@ -9,7 +9,7 @@
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <camera_info_manager/camera_info_manager.hpp>
 #include <image_transport/image_transport.hpp>
-
+#include "realsense_gazebo_plugin/ros_qos_specifier.hpp"
 #include <memory>
 #include <string>
 
@@ -63,7 +63,7 @@ class GazeboRosRealsense : public RealSensePlugin
   rclcpp::Node::SharedPtr node_;
 
 private:
-  std::unique_ptr<image_transport::ImageTransport> itnode_;
+  std::unique_ptr<ImageTransportWithSpecifiedQos> itnode_;
 
   rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointcloud_pub_;
 

include/realsense_gazebo_plugin/ros_qos_specifier.hpp

@@ -0,0 +1,39 @@
+#include "realsense_gazebo_plugin/RealSensePlugin.h"
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/camera_info.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include <camera_info_manager/camera_info_manager.hpp>
+#include <image_transport/image_transport.hpp>
+
+#include <memory>
+#include <string>
+
+#ifndef REALSENSE_GAZEBO_PLUGIN_ROS_QOS_SPECIFIER_HPP
+#define REALSENSE_GAZEBO_PLUGIN_ROS_QOS_SPECIFIER_HPP
+
+namespace gazebo {
+/// \brief A Class that gives Real Sense plugin option to change Qos.
+class ImageTransportWithSpecifiedQos : public image_transport::ImageTransport {
+
+public:
+  explicit ImageTransportWithSpecifiedQos(rclcpp::Node::SharedPtr node) : ImageTransport(node),
+                                          ad_node_p(node), queue_size(2)  {};
+
+  image_transport::CameraPublisher advertise_camera(const std::string &base_topic,
+                                                    uint32_t queue_size,
+                                                    rmw_qos_profile_t qos_profile);
+
+  void specify_color_qos(image_transport::CameraPublisher &cam_color_pub,
+                         std::basic_string<char> topic_name,
+                         std::string colorQos);
+
+private:
+  rclcpp::Node::SharedPtr ad_node_p;
+  uint32_t queue_size;
+};
+}
+
+#endif //REALSENSE_GAZEBO_PLUGIN_ROS_QOS_SPECIFIER_HPP
+
+

src/RealSensePlugin.cpp

@@ -58,6 +58,7 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
   cameraParamsMap_.insert(std::make_pair(IRED1_CAMERA_NAME, CameraParams()));
   cameraParamsMap_.insert(std::make_pair(IRED2_CAMERA_NAME, CameraParams()));
 
+
   do {
     std::string name = _sdf->GetName();
     if (name == "depthUpdateRate") {
@@ -116,6 +117,10 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
       _sdf->GetValue()->Get(pointCloudCutOffMax_);
     } else if (name == "prefix") {
       this->prefix = _sdf->GetValue()->GetAsString();
+    } else if (name == "color_qos") {
+      this->colorQos = _sdf->GetValue()->GetAsString();
+    } else if (name == "pointcloud_qos") {
+      this->pointCloudQos = _sdf->GetValue()->GetAsString(); 
     } else if (name == "robotNamespace") {
       break;
     } else {
@@ -291,3 +296,4 @@ void RealSensePlugin::OnNewDepthFrame()
 
 /////////////////////////////////////////////////
 void RealSensePlugin::OnUpdate() {}
+

src/gazebo_ros_realsense.cpp

@@ -49,30 +49,40 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
   this->camera_info_manager_.reset(
     new camera_info_manager::CameraInfoManager(this->node_.get(), this->GetHandle()));
 
-  this->itnode_.reset(new image_transport::ImageTransport(this->node_));
+  itnode_.reset(new ImageTransportWithSpecifiedQos(node_));
 
-  this->color_pub_ = this->itnode_->advertiseCamera(
-    cameraParamsMap_[COLOR_CAMERA_NAME].topic_name, 2);
-  this->ir1_pub_ = this->itnode_->advertiseCamera(
-    cameraParamsMap_[IRED1_CAMERA_NAME].topic_name, 2);
-  this->ir2_pub_ = this->itnode_->advertiseCamera(
-    cameraParamsMap_[IRED2_CAMERA_NAME].topic_name, 2);
-  this->depth_pub_ = this->itnode_->advertiseCamera(
-    cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name, 2);
+  itnode_->specify_color_qos(color_pub_,cameraParamsMap_[COLOR_CAMERA_NAME].topic_name,colorQos);
+  itnode_->specify_color_qos(ir1_pub_,cameraParamsMap_[IRED1_CAMERA_NAME].topic_name,colorQos);
+  itnode_->specify_color_qos(ir2_pub_,cameraParamsMap_[IRED2_CAMERA_NAME].topic_name,colorQos);
+  itnode_->specify_color_qos(depth_pub_,cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name,colorQos);
 
   if (pointCloud_) {
-    this->pointcloud_pub_ = this->node_->create_publisher<sensor_msgs::msg::PointCloud2>(
-      pointCloudTopic_, rclcpp::SystemDefaultsQoS());
+        rclcpp::QoS temp_qos =rclcpp::SystemDefaultsQoS();
+        if(pointCloudQos=="SensorDataQoS") {
+            temp_qos = rclcpp::SensorDataQoS();
+            RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SensorDataQoS.");
+        } else if(pointCloudQos=="ParametersQoS") {
+            temp_qos = rclcpp::ParametersQoS();
+            RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParametersQoS.");
+        } else if(pointCloudQos=="ServicesQoS") {
+            temp_qos = rclcpp::ServicesQoS();
+            RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ServicesQoS.");
+        } else if(pointCloudQos=="ParameterEventsQoS") {
+            temp_qos = rclcpp::ParameterEventsQoS();
+            RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParameterEventsQoS.");
+        } else {
+            RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SystemDefaultsQoS.");
+        }
+        pointcloud_pub_ = node_->create_publisher<sensor_msgs::msg::PointCloud2>(pointCloudTopic_, temp_qos);
   }
-
   RCLCPP_INFO(node_->get_logger(), "Loaded Realsense Gazebo ROS plugin.");
 }
 
 void GazeboRosRealsense::OnNewFrame(
   const rendering::CameraPtr cam,
   const transport::PublisherPtr pub)
 {
-  common::Time current_time = this->world->SimTime();
+  rclcpp::Time current_time = this->node_->now();
 
   // identify camera
   std::string camera_id = extractCameraName(cam->Name());
@@ -87,8 +97,7 @@ void GazeboRosRealsense::OnNewFrame(
   // copy data into image
   this->image_msg_.header.frame_id =
     this->cameraParamsMap_[camera_id].optical_frame;
-  this->image_msg_.header.stamp.sec = current_time.sec;
-  this->image_msg_.header.stamp.nanosec = current_time.nsec;
+  this->image_msg_.header.stamp = current_time;
 
   // set image encoding
   const std::map<std::string, std::string> supported_image_encodings = {
@@ -216,15 +225,14 @@ bool GazeboRosRealsense::FillPointCloudHelper(
 void GazeboRosRealsense::OnNewDepthFrame()
 {
   // get current time
-  common::Time current_time = this->world->SimTime();
+  rclcpp::Time current_time = this->node_->now();
 
   RealSensePlugin::OnNewDepthFrame();
 
   // copy data into image
   this->depth_msg_.header.frame_id =
     this->cameraParamsMap_[DEPTH_CAMERA_NAME].optical_frame;
-  this->depth_msg_.header.stamp.sec = current_time.sec;
-  this->depth_msg_.header.stamp.nanosec = current_time.nsec;
+  this->depth_msg_.header.stamp = current_time;
 
   // set image encoding
   std::string pixel_format = sensor_msgs::image_encodings::TYPE_16UC1;

src/ros_qos_specifier.cpp

@@ -0,0 +1,34 @@
+#include "realsense_gazebo_plugin/ros_qos_specifier.hpp"
+
+image_transport::CameraPublisher gazebo::ImageTransportWithSpecifiedQos::advertise_camera(
+        const std::string & base_topic, uint32_t queue_size, rmw_qos_profile_t qos_profile)
+{
+    qos_profile.depth = queue_size;
+    return image_transport::CameraPublisher(ad_node_p.get(), base_topic, qos_profile);
+}
+
+void gazebo::ImageTransportWithSpecifiedQos::specify_color_qos(
+        image_transport::CameraPublisher &cam_color_pub,
+        std::basic_string<char> topic_name, std::string colorQos)
+{
+  rmw_qos_profile_t tmp_qoss = rclcpp::SensorDataQoS().get_rmw_qos_profile();
+  if(colorQos=="SensorDataQoS") {
+    tmp_qoss = rclcpp::SensorDataQoS().get_rmw_qos_profile();
+    RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using SensorDataQoS.");
+  } else if(colorQos=="ParametersQoS") {
+    tmp_qoss = rclcpp::ParametersQoS().get_rmw_qos_profile();
+    RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using ParametersQoS.");
+  } else if(colorQos=="ServicesQoS") {
+    tmp_qoss = rclcpp::ServicesQoS().get_rmw_qos_profile();
+    RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using ServicesQoS.");
+  } else if(colorQos=="ParameterEventsQoS") {
+    tmp_qoss = rclcpp::ParameterEventsQoS().get_rmw_qos_profile();
+    RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using ParameterEventsQoS.");
+  } else  {
+      RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using SystemDefaultsQoS.");
+  }
+  cam_color_pub = this->advertise_camera(
+            topic_name, queue_size, tmp_qoss);
+
+
+}