diff --git a/CMakeLists.txt b/CMakeLists.txt index 53e3c91..34ff722 100644 --- a/CMakeLists.txt +++ b/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}) diff --git a/include/realsense_gazebo_plugin/RealSensePlugin.h b/include/realsense_gazebo_plugin/RealSensePlugin.h index 82c6315..d5e3e77 100644 --- a/include/realsense_gazebo_plugin/RealSensePlugin.h +++ b/include/realsense_gazebo_plugin/RealSensePlugin.h @@ -126,6 +126,8 @@ class RealSensePlugin : public ModelPlugin { bool pointCloud_ = false; std::string pointCloudTopic_; + std::basic_string pointCloudQos; + std::basic_string colorQos; double pointCloudCutOff_, pointCloudCutOffMax_; double colorUpdateRate_; diff --git a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h index a441eb6..aa9b285 100644 --- a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h +++ b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h @@ -9,7 +9,7 @@ #include #include #include - +#include "realsense_gazebo_plugin/ros_qos_specifier.hpp" #include #include @@ -63,7 +63,7 @@ class GazeboRosRealsense : public RealSensePlugin rclcpp::Node::SharedPtr node_; private: - std::unique_ptr itnode_; + std::unique_ptr itnode_; rclcpp::Publisher::SharedPtr pointcloud_pub_; diff --git a/include/realsense_gazebo_plugin/ros_qos_specifier.hpp b/include/realsense_gazebo_plugin/ros_qos_specifier.hpp new file mode 100644 index 0000000..564ebad --- /dev/null +++ b/include/realsense_gazebo_plugin/ros_qos_specifier.hpp @@ -0,0 +1,39 @@ +#include "realsense_gazebo_plugin/RealSensePlugin.h" +#include +#include +#include +#include +#include +#include + +#include +#include + +#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 topic_name, + std::string colorQos); + +private: + rclcpp::Node::SharedPtr ad_node_p; + uint32_t queue_size; +}; +} + +#endif //REALSENSE_GAZEBO_PLUGIN_ROS_QOS_SPECIFIER_HPP + + diff --git a/src/RealSensePlugin.cpp b/src/RealSensePlugin.cpp index 61e84a5..cb9721d 100644 --- a/src/RealSensePlugin.cpp +++ b/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() {} + diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index 7e667bf..f265792 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -49,22 +49,32 @@ 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( - 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(pointCloudTopic_, temp_qos); } - RCLCPP_INFO(node_->get_logger(), "Loaded Realsense Gazebo ROS plugin."); } @@ -72,7 +82,7 @@ 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 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; diff --git a/src/ros_qos_specifier.cpp b/src/ros_qos_specifier.cpp new file mode 100644 index 0000000..d785a67 --- /dev/null +++ b/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 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); + + +}