From 5b6653a096c8bfb58ae4cd8bfd1c6383bbaa9e40 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Tue, 30 Nov 2021 14:13:43 +0100 Subject: [PATCH 01/20] use node time instead of sim time --- src/gazebo_ros_realsense.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index 7e667bf..0652761 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -72,7 +72,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 +87,8 @@ 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.sec = current_time.seconds(); + this->image_msg_.header.stamp.nanosec = current_time.nanoseconds(); // set image encoding const std::map supported_image_encodings = { @@ -216,15 +216,15 @@ 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.sec = current_time.seconds(); + this->depth_msg_.header.stamp.nanosec = current_time.nanoseconds(); // set image encoding std::string pixel_format = sensor_msgs::image_encodings::TYPE_16UC1; From 192e06d8ef1a56df55d23d5c1fc3187684c31e50 Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 7 Dec 2021 11:27:18 +0200 Subject: [PATCH 02/20] -> test --- src/RealSensePlugin.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/RealSensePlugin.cpp b/src/RealSensePlugin.cpp index 61e84a5..a650bf0 100644 --- a/src/RealSensePlugin.cpp +++ b/src/RealSensePlugin.cpp @@ -291,3 +291,4 @@ void RealSensePlugin::OnNewDepthFrame() ///////////////////////////////////////////////// void RealSensePlugin::OnUpdate() {} + From 93ba71689a21694c2fcddcc1b1381323dfb73161 Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 7 Dec 2021 13:24:41 +0200 Subject: [PATCH 03/20] -> qos specifier --- CMakeLists.txt | 1 + .../ros_qos_specifier.hpp | 32 +++++++++++++++++ src/ros_qos_specifier.cpp | 34 +++++++++++++++++++ 3 files changed, 67 insertions(+) create mode 100644 include/realsense_gazebo_plugin/ros_qos_specifier.hpp create mode 100644 src/ros_qos_specifier.cpp 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/ros_qos_specifier.hpp b/include/realsense_gazebo_plugin/ros_qos_specifier.hpp new file mode 100644 index 0000000..423c715 --- /dev/null +++ b/include/realsense_gazebo_plugin/ros_qos_specifier.hpp @@ -0,0 +1,32 @@ +#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 + +class ImageTransportWithSpecifiedQos: public image_transport::ImageTransport { +public: + explicit ImageTransportWithSpecifiedQos(rclcpp::Node::SharedPtr node) : ImageTransport(node), + queue_size(2),ad_node_p(node){}; + 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/ros_qos_specifier.cpp b/src/ros_qos_specifier.cpp new file mode 100644 index 0000000..6369a7e --- /dev/null +++ b/src/ros_qos_specifier.cpp @@ -0,0 +1,34 @@ +#include "realsense_gazebo_plugin/ros_qos_specifier.hpp" + +image_transport::CameraPublisher ImageTransportWithSpecifiedQos::advertise_camera( + const std::string & base_topic, uint32_t queue_size, rmw_qos_profile_t qos_profile) { + rmw_qos_profile_t custom_qos = qos_profile; + custom_qos.depth = queue_size; + return image_transport::CameraPublisher(ad_node_p.get(), base_topic, custom_qos); +} + +void ImageTransportWithSpecifiedQos::specify_color_qos( + image_transport::CameraPublisher &cam_color_pub, + std::basic_string topic_name, std::string colorQos) { + if(colorQos=="SensorDataQoS") { + cam_color_pub = this->advertise_camera( + topic_name, 2,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") { + cam_color_pub = this->advertise_camera( + topic_name, 2,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") { + cam_color_pub = this->advertise_camera( + topic_name, 2,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") { + cam_color_pub = this->advertise_camera( + topic_name, 2,rclcpp::ParameterEventsQoS().get_rmw_qos_profile()); + RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using ParameterEventsQoS."); + } else { + cam_color_pub = this->advertise_camera( + topic_name, 2,rclcpp::SystemDefaultsQoS().get_rmw_qos_profile()); + RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using SystemDefaultsQoS."); + } + } \ No newline at end of file From c6c7b48de5113a5e8487cac00cb54c6b5d860fa1 Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 7 Dec 2021 13:36:21 +0200 Subject: [PATCH 04/20] -> logic for changing qos --- .../realsense_gazebo_plugin/RealSensePlugin.h | 2 + .../gazebo_ros_realsense.h | 4 +- src/gazebo_ros_realsense.cpp | 45 ++++++++++++------- 3 files changed, 34 insertions(+), 17 deletions(-) diff --git a/include/realsense_gazebo_plugin/RealSensePlugin.h b/include/realsense_gazebo_plugin/RealSensePlugin.h index 82c6315..a96b7bf 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::string pointCloudQos; + std::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/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index 0652761..4c9672a 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -49,21 +49,36 @@ 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_)); - - 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); - - if (pointCloud_) { - this->pointcloud_pub_ = this->node_->create_publisher( - pointCloudTopic_, rclcpp::SystemDefaultsQoS()); - } + this->itnode_.reset(new ImageTransportWithSpecifiedQos(this->node_)); + + itnode_->specify_color_qos(this->color_pub_,cameraParamsMap_[COLOR_CAMERA_NAME].topic_name,colorQos); + itnode_->specify_color_qos(this->ir1_pub_,cameraParamsMap_[IRED1_CAMERA_NAME].topic_name,colorQos); + itnode_->specify_color_qos(this->ir2_pub_,cameraParamsMap_[IRED2_CAMERA_NAME].topic_name,colorQos); + itnode_->specify_color_qos(this->depth_pub_,cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name,colorQos); + + if (pointCloud_) { + if(pointCloudQos=="SensorDataQoS") { + this->pointcloud_pub_ = this->node_->create_publisher( + pointCloudTopic_, rclcpp::SensorDataQoS()); + RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SensorDataQoS."); + } else if(pointCloudQos=="ParametersQoS") { + this->pointcloud_pub_ = this->node_->create_publisher( + pointCloudTopic_, rclcpp::ParametersQoS()); + RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParametersQoS."); + } else if(pointCloudQos=="ServicesQoS") { + this->pointcloud_pub_ = this->node_->create_publisher( + pointCloudTopic_, rclcpp::ServicesQoS()); + RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ServicesQoS."); + } else if(pointCloudQos=="ParameterEventsQoS") { + this->pointcloud_pub_ = this->node_->create_publisher( + pointCloudTopic_, rclcpp::ParameterEventsQoS()); + RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParameterEventsQoS."); + } else { + this->pointcloud_pub_ = this->node_->create_publisher( + pointCloudTopic_, rclcpp::SystemDefaultsQoS()); + RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SystemDefaultsQoS."); + } + } RCLCPP_INFO(node_->get_logger(), "Loaded Realsense Gazebo ROS plugin."); } From 8aad482310ed994c341afa0bf43dce60abfc45f1 Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 7 Dec 2021 13:48:46 +0200 Subject: [PATCH 05/20] -> get data from gazebo.xarco file --- src/RealSensePlugin.cpp | 135 ++++++++++++++++++++-------------------- 1 file changed, 69 insertions(+), 66 deletions(-) diff --git a/src/RealSensePlugin.cpp b/src/RealSensePlugin.cpp index a650bf0..53b2596 100644 --- a/src/RealSensePlugin.cpp +++ b/src/RealSensePlugin.cpp @@ -58,72 +58,75 @@ 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") { - _sdf->GetValue()->Get(depthUpdateRate_); - } else if (name == "colorUpdateRate") { - _sdf->GetValue()->Get(colorUpdateRate_); - } else if (name == "infraredUpdateRate") { - _sdf->GetValue()->Get(infraredUpdateRate_); - } else if (name == "depthTopicName") { - cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "depthCameraInfoTopicName") { - cameraParamsMap_[DEPTH_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "colorTopicName") { - cameraParamsMap_[COLOR_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "colorCameraInfoTopicName") { - cameraParamsMap_[COLOR_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared1TopicName") { - cameraParamsMap_[IRED1_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared1CameraInfoTopicName") { - cameraParamsMap_[IRED1_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared2TopicName") { - cameraParamsMap_[IRED2_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared2CameraInfoTopicName") { - cameraParamsMap_[IRED2_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "colorOpticalframeName") { - cameraParamsMap_[COLOR_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "depthOpticalframeName") { - cameraParamsMap_[DEPTH_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared1OpticalframeName") { - cameraParamsMap_[IRED1_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared2OpticalframeName") { - cameraParamsMap_[IRED2_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "rangeMinDepth") { - _sdf->GetValue()->Get(rangeMinDepth_); - } else if (name == "rangeMaxDepth") { - _sdf->GetValue()->Get(rangeMaxDepth_); - } else if (name == "pointCloud") { - _sdf->GetValue()->Get(pointCloud_); - } else if (name == "pointCloudTopicName") { - pointCloudTopic_ = _sdf->GetValue()->GetAsString(); - } else if (name == "pointCloudCutoff") { - _sdf->GetValue()->Get(pointCloudCutOff_); - } else if (name == "pointCloudCutoffMax") { - _sdf->GetValue()->Get(pointCloudCutOffMax_); - } else if (name == "prefix") { - this->prefix = _sdf->GetValue()->GetAsString(); - } else if (name == "robotNamespace") { - break; - } else { - throw std::runtime_error("Ivalid parameter for ReakSensePlugin"); - } - - _sdf = _sdf->GetNextElement(); - } while (_sdf); + do { + std::string name = _sdf->GetName(); + if (name == "depthUpdateRate") { + _sdf->GetValue()->Get(depthUpdateRate_); + } else if (name == "colorUpdateRate") { + _sdf->GetValue()->Get(colorUpdateRate_); + } else if (name == "infraredUpdateRate") { + _sdf->GetValue()->Get(infraredUpdateRate_); + } else if (name == "depthTopicName") { + cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "depthCameraInfoTopicName") { + cameraParamsMap_[DEPTH_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "colorTopicName") { + cameraParamsMap_[COLOR_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "colorCameraInfoTopicName") { + cameraParamsMap_[COLOR_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared1TopicName") { + cameraParamsMap_[IRED1_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared1CameraInfoTopicName") { + cameraParamsMap_[IRED1_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared2TopicName") { + cameraParamsMap_[IRED2_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared2CameraInfoTopicName") { + cameraParamsMap_[IRED2_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "colorOpticalframeName") { + cameraParamsMap_[COLOR_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + } else if (name == "depthOpticalframeName") { + cameraParamsMap_[DEPTH_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared1OpticalframeName") { + cameraParamsMap_[IRED1_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared2OpticalframeName") { + cameraParamsMap_[IRED2_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + } else if (name == "rangeMinDepth") { + _sdf->GetValue()->Get(rangeMinDepth_); + } else if (name == "rangeMaxDepth") { + _sdf->GetValue()->Get(rangeMaxDepth_); + } else if (name == "pointCloud") { + _sdf->GetValue()->Get(pointCloud_); + } else if (name == "pointCloudTopicName") { + pointCloudTopic_ = _sdf->GetValue()->GetAsString(); + } else if (name == "pointCloudCutoff") { + _sdf->GetValue()->Get(pointCloudCutOff_); + } else if (name == "pointCloudCutoffMax") { + _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 { + throw std::runtime_error("Ivalid parameter for RealSensePlugin"); + } + _sdf = _sdf->GetNextElement(); + } while (_sdf); // Store a pointer to the this model this->rsModel = _model; From e9cef3aea98acda670e76a6edf5df73624682c45 Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 14 Dec 2021 12:58:27 +0200 Subject: [PATCH 06/20] -> RealSensePlugin.cpp manually formatted --- src/RealSensePlugin.cpp | 246 +++++++++++++++++++--------------------- 1 file changed, 119 insertions(+), 127 deletions(-) diff --git a/src/RealSensePlugin.cpp b/src/RealSensePlugin.cpp index 53b2596..3b8a7d4 100644 --- a/src/RealSensePlugin.cpp +++ b/src/RealSensePlugin.cpp @@ -43,13 +43,12 @@ RealSensePlugin::RealSensePlugin() RealSensePlugin::~RealSensePlugin() {} ///////////////////////////////////////////////// -void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ +void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { // Output the name of the model - std::cout << - std::endl << - "RealSensePlugin: The realsense_camera plugin is attach to model " << - _model->GetName() << std::endl; + std::cout + <GetName() << std::endl; _sdf = _sdf->GetFirstElement(); @@ -58,75 +57,74 @@ 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") { - _sdf->GetValue()->Get(depthUpdateRate_); - } else if (name == "colorUpdateRate") { - _sdf->GetValue()->Get(colorUpdateRate_); - } else if (name == "infraredUpdateRate") { - _sdf->GetValue()->Get(infraredUpdateRate_); - } else if (name == "depthTopicName") { - cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "depthCameraInfoTopicName") { - cameraParamsMap_[DEPTH_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "colorTopicName") { - cameraParamsMap_[COLOR_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "colorCameraInfoTopicName") { - cameraParamsMap_[COLOR_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared1TopicName") { - cameraParamsMap_[IRED1_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared1CameraInfoTopicName") { - cameraParamsMap_[IRED1_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared2TopicName") { - cameraParamsMap_[IRED2_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared2CameraInfoTopicName") { - cameraParamsMap_[IRED2_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "colorOpticalframeName") { - cameraParamsMap_[COLOR_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "depthOpticalframeName") { - cameraParamsMap_[DEPTH_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared1OpticalframeName") { - cameraParamsMap_[IRED1_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared2OpticalframeName") { - cameraParamsMap_[IRED2_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "rangeMinDepth") { - _sdf->GetValue()->Get(rangeMinDepth_); - } else if (name == "rangeMaxDepth") { - _sdf->GetValue()->Get(rangeMaxDepth_); - } else if (name == "pointCloud") { - _sdf->GetValue()->Get(pointCloud_); - } else if (name == "pointCloudTopicName") { - pointCloudTopic_ = _sdf->GetValue()->GetAsString(); - } else if (name == "pointCloudCutoff") { - _sdf->GetValue()->Get(pointCloudCutOff_); - } else if (name == "pointCloudCutoffMax") { - _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 { - throw std::runtime_error("Ivalid parameter for RealSensePlugin"); - } - _sdf = _sdf->GetNextElement(); - } while (_sdf); + do { + std::string name = _sdf->GetName(); + if (name == "depthUpdateRate") + _sdf->GetValue()->Get(depthUpdateRate_); + else if (name == "colorUpdateRate") + _sdf->GetValue()->Get(colorUpdateRate_); + else if (name == "infraredUpdateRate") + _sdf->GetValue()->Get(infraredUpdateRate_); + else if (name == "depthTopicName") + cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "depthCameraInfoTopicName") + cameraParamsMap_[DEPTH_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "colorTopicName") + cameraParamsMap_[COLOR_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "colorCameraInfoTopicName") + cameraParamsMap_[COLOR_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared1TopicName") + cameraParamsMap_[IRED1_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared1CameraInfoTopicName") + cameraParamsMap_[IRED1_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared2TopicName") + cameraParamsMap_[IRED2_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared2CameraInfoTopicName") + cameraParamsMap_[IRED2_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + else if (name == "colorOpticalframeName") + cameraParamsMap_[COLOR_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + else if (name == "depthOpticalframeName") + cameraParamsMap_[DEPTH_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared1OpticalframeName") + cameraParamsMap_[IRED1_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + else if (name == "infrared2OpticalframeName") + cameraParamsMap_[IRED2_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + else if (name == "rangeMinDepth") + _sdf->GetValue()->Get(rangeMinDepth_); + else if (name == "rangeMaxDepth") + _sdf->GetValue()->Get(rangeMaxDepth_); + else if (name == "pointCloud") + _sdf->GetValue()->Get(pointCloud_); + else if (name == "pointCloudTopicName") + pointCloudTopic_ = _sdf->GetValue()->GetAsString(); + else if (name == "pointCloudCutoff") + _sdf->GetValue()->Get(pointCloudCutOff_); + else if (name == "pointCloudCutoffMax") + _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 + throw std::runtime_error("Ivalid parameter for RealSensePlugin"); + _sdf = _sdf->GetNextElement(); + } while (_sdf); // Store a pointer to the this model this->rsModel = _model; @@ -139,49 +137,50 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) // Get Cameras Renderers this->depthCam = std::dynamic_pointer_cast( - smanager->GetSensor(prefix + DEPTH_CAMERA_NAME)) - ->DepthCamera(); + smanager->GetSensor(prefix + DEPTH_CAMERA_NAME)) + ->DepthCamera(); this->ired1Cam = std::dynamic_pointer_cast( - smanager->GetSensor(prefix + IRED1_CAMERA_NAME)) - ->Camera(); + smanager->GetSensor(prefix + IRED1_CAMERA_NAME)) + ->Camera(); + this->ired2Cam = std::dynamic_pointer_cast( - smanager->GetSensor(prefix + IRED2_CAMERA_NAME)) - ->Camera(); + smanager->GetSensor(prefix + IRED2_CAMERA_NAME)) + ->Camera(); + this->colorCam = std::dynamic_pointer_cast( - smanager->GetSensor(prefix + COLOR_CAMERA_NAME)) - ->Camera(); + smanager->GetSensor(prefix + COLOR_CAMERA_NAME)) + ->Camera(); // Check if camera renderers have been found successfuly if (!this->depthCam) { - std::cerr << "RealSensePlugin: Depth Camera has not been found" << - std::endl; + std::cerr << "RealSensePlugin: Depth Camera has not been found" + << std::endl; return; } if (!this->ired1Cam) { - std::cerr << "RealSensePlugin: InfraRed Camera 1 has not been found" << - std::endl; + std::cerr << "RealSensePlugin: InfraRed Camera 1 has not been found" + << std::endl; return; } if (!this->ired2Cam) { - std::cerr << "RealSensePlugin: InfraRed Camera 2 has not been found" << - std::endl; + std::cerr << "RealSensePlugin: InfraRed Camera 2 has not been found" + << std::endl; return; } if (!this->colorCam) { - std::cerr << "RealSensePlugin: Color Camera has not been found" << - std::endl; + std::cerr << "RealSensePlugin: Color Camera has not been found" + << std::endl; return; } // Resize Depth Map dimensions try { - this->depthMap.resize( - this->depthCam->ImageWidth() * - this->depthCam->ImageHeight()); + this->depthMap.resize(this->depthCam->ImageWidth() * + this->depthCam->ImageHeight()); } catch (std::bad_alloc & e) { - std::cerr << "RealSensePlugin: depthMap allocation failed: " << e.what() << - std::endl; + std::cerr << "RealSensePlugin: depthMap allocation failed: " << e.what() + << std::endl; return; } @@ -193,40 +192,38 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) std::string rsTopicRoot = "~/" + this->rsModel->GetName(); this->depthPub = this->transportNode->Advertise( - rsTopicRoot + DEPTH_CAMERA_TOPIC, 1, depthUpdateRate_); + rsTopicRoot + DEPTH_CAMERA_TOPIC, 1, depthUpdateRate_); this->ired1Pub = this->transportNode->Advertise( - rsTopicRoot + IRED1_CAMERA_TOPIC, 1, infraredUpdateRate_); + rsTopicRoot + IRED1_CAMERA_TOPIC, 1, infraredUpdateRate_); this->ired2Pub = this->transportNode->Advertise( - rsTopicRoot + IRED2_CAMERA_TOPIC, 1, infraredUpdateRate_); + rsTopicRoot + IRED2_CAMERA_TOPIC, 1, infraredUpdateRate_); this->colorPub = this->transportNode->Advertise( - rsTopicRoot + COLOR_CAMERA_TOPIC, 1, colorUpdateRate_); + rsTopicRoot + COLOR_CAMERA_TOPIC, 1, colorUpdateRate_); // Listen to depth camera new frame event this->newDepthFrameConn = this->depthCam->ConnectNewDepthFrame( - std::bind(&RealSensePlugin::OnNewDepthFrame, this)); + std::bind(&RealSensePlugin::OnNewDepthFrame, this)); this->newIred1FrameConn = this->ired1Cam->ConnectNewImageFrame( - std::bind( + std::bind( &RealSensePlugin::OnNewFrame, this, this->ired1Cam, this->ired1Pub)); this->newIred2FrameConn = this->ired2Cam->ConnectNewImageFrame( - std::bind( + std::bind( &RealSensePlugin::OnNewFrame, this, this->ired2Cam, this->ired2Pub)); this->newColorFrameConn = this->colorCam->ConnectNewImageFrame( - std::bind( + std::bind( &RealSensePlugin::OnNewFrame, this, this->colorCam, this->colorPub)); // Listen to the update event this->updateConnection = event::Events::ConnectWorldUpdateBegin( - boost::bind(&RealSensePlugin::OnUpdate, this)); + boost::bind(&RealSensePlugin::OnUpdate, this)); } ///////////////////////////////////////////////// -void RealSensePlugin::OnNewFrame( - const rendering::CameraPtr cam, - const transport::PublisherPtr pub) -{ +void RealSensePlugin::OnNewFrame(const rendering::CameraPtr cam, + const transport::PublisherPtr pub) { msgs::ImageStamped msg; // Set Simulation Time @@ -238,25 +235,23 @@ void RealSensePlugin::OnNewFrame( // Set Image Pixel Format msg.mutable_image()->set_pixel_format( - common::Image::ConvertPixelFormat(cam->ImageFormat())); + common::Image::ConvertPixelFormat(cam->ImageFormat())); // Set Image Data msg.mutable_image()->set_step(cam->ImageWidth() * cam->ImageDepth()); - msg.mutable_image()->set_data( - cam->ImageData(), - cam->ImageDepth() * cam->ImageWidth() * - cam->ImageHeight()); + msg.mutable_image()->set_data(cam->ImageData(), + cam->ImageDepth() * cam->ImageWidth() * + cam->ImageHeight()); // Publish realsense infrared stream pub->Publish(msg); } ///////////////////////////////////////////////// -void RealSensePlugin::OnNewDepthFrame() -{ +void RealSensePlugin::OnNewDepthFrame() { // Get Depth Map dimensions unsigned int imageSize = - this->depthCam->ImageWidth() * this->depthCam->ImageHeight(); + this->depthCam->ImageWidth() * this->depthCam->ImageHeight(); // Instantiate message msgs::ImageStamped msg; @@ -266,10 +261,9 @@ void RealSensePlugin::OnNewDepthFrame() for (unsigned int i = 0; i < imageSize; ++i) { // Check clipping and overflow if (depthDataFloat[i] < rangeMinDepth_ || - depthDataFloat[i] > rangeMaxDepth_ || - depthDataFloat[i] > DEPTH_SCALE_M * UINT16_MAX || - depthDataFloat[i] < 0) - { + depthDataFloat[i] > rangeMaxDepth_ || + depthDataFloat[i] > DEPTH_SCALE_M * UINT16_MAX || + depthDataFloat[i] < 0) { this->depthMap[i] = 0; } else { this->depthMap[i] = (uint16_t)(depthDataFloat[i] / DEPTH_SCALE_M); @@ -281,12 +275,10 @@ void RealSensePlugin::OnNewDepthFrame() msg.mutable_image()->set_width(this->depthCam->ImageWidth()); msg.mutable_image()->set_height(this->depthCam->ImageHeight()); msg.mutable_image()->set_pixel_format(common::Image::L_INT16); - msg.mutable_image()->set_step( - this->depthCam->ImageWidth() * - this->depthCam->ImageDepth()); - msg.mutable_image()->set_data( - this->depthMap.data(), - sizeof(*this->depthMap.data()) * imageSize); + msg.mutable_image()->set_step(this->depthCam->ImageWidth() * + this->depthCam->ImageDepth()); + msg.mutable_image()->set_data(this->depthMap.data(), + sizeof(*this->depthMap.data()) * imageSize); // Publish realsense scaled depth map this->depthPub->Publish(msg); From e01b791bad19e5c6b69fc37eb70c6a382490959d Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 14 Dec 2021 13:31:22 +0200 Subject: [PATCH 07/20] -> gazebo_ros_realsense.cpp manually formatted --- src/gazebo_ros_realsense.cpp | 186 ++++++++++++++++------------------- 1 file changed, 85 insertions(+), 101 deletions(-) diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index 4c9672a..d65c48b 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -3,42 +3,34 @@ #include #include -namespace -{ +namespace { std::string extractCameraName(const std::string & name); -sensor_msgs::msg::CameraInfo cameraInfo( - const sensor_msgs::msg::Image & image, - float horizontal_fov); +sensor_msgs::msg::CameraInfo cameraInfo(const sensor_msgs::msg::Image & image, + float horizontal_fov); } -namespace gazebo -{ +namespace gazebo { // Register the plugin GZ_REGISTER_MODEL_PLUGIN(GazeboRosRealsense) -GazeboRosRealsense::GazeboRosRealsense() -{ -} +GazeboRosRealsense::GazeboRosRealsense() {} -GazeboRosRealsense::~GazeboRosRealsense() -{ +GazeboRosRealsense::~GazeboRosRealsense() { RCLCPP_DEBUG_STREAM(this->node_->get_logger(), "realsense_camera Unloaded"); } -void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ +void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { this->node_ = rclcpp::Node::make_shared("GazeboRealsenseNode"); // Make sure the ROS node for Gazebo has already been initialized if (!rclcpp::ok()) { RCLCPP_ERROR( - node_->get_logger(), - "A ROS node for Gazebo has not been initialized, unable " - "to load plugin. " - "Load the Gazebo system plugin " - "'libgazebo_ros_api_plugin.so' in the gazebo_ros " - "package"); + node_->get_logger(),"A ROS node for Gazebo has not been initialized, unable " + "to load plugin. " + "Load the Gazebo system plugin " + "'libgazebo_ros_api_plugin.so' in the gazebo_ros " + "package"); return; } RCLCPP_INFO(node_->get_logger(), "Realsense Gazebo ROS plugin loading."); @@ -47,96 +39,91 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) // initialize camera_info_manager this->camera_info_manager_.reset( - new camera_info_manager::CameraInfoManager(this->node_.get(), this->GetHandle())); - - this->itnode_.reset(new ImageTransportWithSpecifiedQos(this->node_)); - - itnode_->specify_color_qos(this->color_pub_,cameraParamsMap_[COLOR_CAMERA_NAME].topic_name,colorQos); - itnode_->specify_color_qos(this->ir1_pub_,cameraParamsMap_[IRED1_CAMERA_NAME].topic_name,colorQos); - itnode_->specify_color_qos(this->ir2_pub_,cameraParamsMap_[IRED2_CAMERA_NAME].topic_name,colorQos); - itnode_->specify_color_qos(this->depth_pub_,cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name,colorQos); - - if (pointCloud_) { - if(pointCloudQos=="SensorDataQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( - pointCloudTopic_, rclcpp::SensorDataQoS()); - RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SensorDataQoS."); - } else if(pointCloudQos=="ParametersQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( - pointCloudTopic_, rclcpp::ParametersQoS()); - RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParametersQoS."); - } else if(pointCloudQos=="ServicesQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( - pointCloudTopic_, rclcpp::ServicesQoS()); - RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ServicesQoS."); - } else if(pointCloudQos=="ParameterEventsQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( - pointCloudTopic_, rclcpp::ParameterEventsQoS()); - RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParameterEventsQoS."); - } else { - this->pointcloud_pub_ = this->node_->create_publisher( - pointCloudTopic_, rclcpp::SystemDefaultsQoS()); - RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SystemDefaultsQoS."); - } + new camera_info_manager::CameraInfoManager(this->node_.get(), this->GetHandle())); + + this->itnode_.reset(new ImageTransportWithSpecifiedQos(this->node_)); + + itnode_->specify_color_qos(this->color_pub_,cameraParamsMap_[COLOR_CAMERA_NAME].topic_name,colorQos); + itnode_->specify_color_qos(this->ir1_pub_,cameraParamsMap_[IRED1_CAMERA_NAME].topic_name,colorQos); + itnode_->specify_color_qos(this->ir2_pub_,cameraParamsMap_[IRED2_CAMERA_NAME].topic_name,colorQos); + itnode_->specify_color_qos(this->depth_pub_,cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name,colorQos); + + if (pointCloud_) { + if(pointCloudQos=="SensorDataQoS") { + this->pointcloud_pub_ = this->node_->create_publisher( + pointCloudTopic_, rclcpp::SensorDataQoS()); + RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SensorDataQoS."); + } else if(pointCloudQos=="ParametersQoS") { + this->pointcloud_pub_ = this->node_->create_publisher( + pointCloudTopic_, rclcpp::ParametersQoS()); + RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParametersQoS."); + } else if(pointCloudQos=="ServicesQoS") { + this->pointcloud_pub_ = this->node_->create_publisher( + pointCloudTopic_, rclcpp::ServicesQoS()); + RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ServicesQoS."); + } else if(pointCloudQos=="ParameterEventsQoS") { + this->pointcloud_pub_ = this->node_->create_publisher( + pointCloudTopic_, rclcpp::ParameterEventsQoS()); + RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParameterEventsQoS."); + } else { + this->pointcloud_pub_ = this->node_->create_publisher( + pointCloudTopic_, rclcpp::SystemDefaultsQoS()); + RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SystemDefaultsQoS."); } - + } RCLCPP_INFO(node_->get_logger(), "Loaded Realsense Gazebo ROS plugin."); } -void GazeboRosRealsense::OnNewFrame( - const rendering::CameraPtr cam, - const transport::PublisherPtr pub) -{ +void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam, + const transport::PublisherPtr pub) { rclcpp::Time current_time = this->node_->now(); // identify camera std::string camera_id = extractCameraName(cam->Name()); const std::map - camera_publishers = { - {COLOR_CAMERA_NAME, &(this->color_pub_)}, - {IRED1_CAMERA_NAME, &(this->ir1_pub_)}, - {IRED2_CAMERA_NAME, &(this->ir2_pub_)}, - }; + camera_publishers = { + {COLOR_CAMERA_NAME, &(this->color_pub_)}, + {IRED1_CAMERA_NAME, &(this->ir1_pub_)}, + {IRED2_CAMERA_NAME, &(this->ir2_pub_)}, + }; const auto image_pub = camera_publishers.at(camera_id); // copy data into image this->image_msg_.header.frame_id = - this->cameraParamsMap_[camera_id].optical_frame; + this->cameraParamsMap_[camera_id].optical_frame; this->image_msg_.header.stamp.sec = current_time.seconds(); this->image_msg_.header.stamp.nanosec = current_time.nanoseconds(); // set image encoding const std::map supported_image_encodings = { - {"RGB_INT8", sensor_msgs::image_encodings::RGB8}, - {"L_INT8", sensor_msgs::image_encodings::TYPE_8UC1}}; + {"RGB_INT8", sensor_msgs::image_encodings::RGB8}, + {"L_INT8", sensor_msgs::image_encodings::TYPE_8UC1}}; const auto pixel_format = supported_image_encodings.at(cam->ImageFormat()); // copy from simulation image to ROS msg - sensor_msgs::fillImage( - this->image_msg_, pixel_format, cam->ImageHeight(), - cam->ImageWidth(), cam->ImageDepth() * cam->ImageWidth(), - reinterpret_cast(cam->ImageData())); + sensor_msgs::fillImage(this->image_msg_, pixel_format, cam->ImageHeight(), + cam->ImageWidth(), cam->ImageDepth() * cam->ImageWidth(), + reinterpret_cast(cam->ImageData())); // identify camera rendering const std::map cameras = { - {COLOR_CAMERA_NAME, this->colorCam}, - {IRED1_CAMERA_NAME, this->ired1Cam}, - {IRED2_CAMERA_NAME, this->ired2Cam}, + {COLOR_CAMERA_NAME, this->colorCam}, + {IRED1_CAMERA_NAME, this->ired1Cam}, + {IRED2_CAMERA_NAME, this->ired2Cam}, }; // publish to ROS auto camera_info_msg = - cameraInfo(this->image_msg_, cameras.at(camera_id)->HFOV().Radian()); + cameraInfo(this->image_msg_, cameras.at(camera_id)->HFOV().Radian()); image_pub->publish(this->image_msg_, camera_info_msg); } // Referenced from gazebo_plugins // https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_openni_kinect.cpp#L302 // Fill depth information -bool GazeboRosRealsense::FillPointCloudHelper( - sensor_msgs::msg::PointCloud2 & point_cloud_msg, - uint32_t rows_arg, uint32_t cols_arg, - uint32_t step_arg, void * data_arg) +bool GazeboRosRealsense::FillPointCloudHelper(sensor_msgs::msg::PointCloud2 & point_cloud_msg, + uint32_t rows_arg, uint32_t cols_arg, + uint32_t step_arg, void * data_arg) { sensor_msgs::PointCloud2Modifier pcd_modifier(point_cloud_msg); pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); @@ -182,7 +169,9 @@ bool GazeboRosRealsense::FillPointCloudHelper( *iter_x = depth * tan(yAngle); *iter_y = depth * tan(pAngle); *iter_z = depth; - } else { // point in the unseeable range + } + else // point in the unseeable range + { *iter_x = *iter_y = *iter_z = std::numeric_limits::quiet_NaN(); point_cloud_msg.is_dense = false; } @@ -211,7 +200,9 @@ bool GazeboRosRealsense::FillPointCloudHelper( iter_rgb[0] = image_src[i + j * cols_arg]; iter_rgb[1] = image_src[i + j * cols_arg]; iter_rgb[2] = image_src[i + j * cols_arg]; - } else { + } + else + { // no image iter_rgb[0] = 0; iter_rgb[1] = 0; @@ -228,8 +219,7 @@ bool GazeboRosRealsense::FillPointCloudHelper( return true; } -void GazeboRosRealsense::OnNewDepthFrame() -{ +void GazeboRosRealsense::OnNewDepthFrame() { // get current time rclcpp::Time current_time = this->node_->now(); @@ -237,7 +227,7 @@ void GazeboRosRealsense::OnNewDepthFrame() // copy data into image this->depth_msg_.header.frame_id = - this->cameraParamsMap_[DEPTH_CAMERA_NAME].optical_frame; + this->cameraParamsMap_[DEPTH_CAMERA_NAME].optical_frame; this->depth_msg_.header.stamp.sec = current_time.seconds(); this->depth_msg_.header.stamp.nanosec = current_time.nanoseconds(); @@ -245,35 +235,32 @@ void GazeboRosRealsense::OnNewDepthFrame() std::string pixel_format = sensor_msgs::image_encodings::TYPE_16UC1; // copy from simulation image to ROS msg - sensor_msgs::fillImage( - this->depth_msg_, pixel_format, this->depthCam->ImageHeight(), - this->depthCam->ImageWidth(), 2 * this->depthCam->ImageWidth(), - reinterpret_cast(this->depthMap.data())); + sensor_msgs::fillImage(this->depth_msg_, pixel_format, this->depthCam->ImageHeight(), + this->depthCam->ImageWidth(), 2 * this->depthCam->ImageWidth(), + reinterpret_cast(this->depthMap.data())); // publish to ROS auto depth_info_msg = - cameraInfo(this->depth_msg_, this->depthCam->HFOV().Radian()); + cameraInfo(this->depth_msg_, this->depthCam->HFOV().Radian()); this->depth_pub_.publish(this->depth_msg_, depth_info_msg); - if (pointCloud_ && this->pointcloud_pub_->get_subscription_count() > 0) { + if (pointCloud_ && this->pointcloud_pub_->get_subscription_count() > 0) + { this->pointcloud_msg_.header = this->depth_msg_.header; this->pointcloud_msg_.width = this->depthCam->ImageWidth(); this->pointcloud_msg_.height = this->depthCam->ImageHeight(); this->pointcloud_msg_.row_step = - this->pointcloud_msg_.point_step * this->depthCam->ImageWidth(); - FillPointCloudHelper( - this->pointcloud_msg_, this->depthCam->ImageHeight(), - this->depthCam->ImageWidth(), 2 * this->depthCam->ImageWidth(), - (void *)this->depthCam->DepthData()); + this->pointcloud_msg_.point_step * this->depthCam->ImageWidth(); + FillPointCloudHelper(this->pointcloud_msg_, this->depthCam->ImageHeight(), + this->depthCam->ImageWidth(), 2 * this->depthCam->ImageWidth(), + (void *)this->depthCam->DepthData()); this->pointcloud_pub_->publish(this->pointcloud_msg_); } } } -namespace -{ -std::string extractCameraName(const std::string & name) -{ +namespace { +std::string extractCameraName(const std::string & name) { if (name.find(COLOR_CAMERA_NAME) != std::string::npos) { return COLOR_CAMERA_NAME; } @@ -285,14 +272,11 @@ std::string extractCameraName(const std::string & name) } RCLCPP_ERROR(rclcpp::get_logger("realsense_camera"), "Unknown camera name"); - return COLOR_CAMERA_NAME; } -sensor_msgs::msg::CameraInfo cameraInfo( - const sensor_msgs::msg::Image & image, - float horizontal_fov) -{ +sensor_msgs::msg::CameraInfo cameraInfo(const sensor_msgs::msg::Image & image, + float horizontal_fov) { sensor_msgs::msg::CameraInfo info_msg; info_msg.header = image.header; From 6e6f70f5d6e6b268026e146ef86239ad871dba9a Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 14 Dec 2021 13:39:10 +0200 Subject: [PATCH 08/20] -> ros_qos_specifier.hpp manually formatted --- .../ros_qos_specifier.hpp | 25 ++++++++++++------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/include/realsense_gazebo_plugin/ros_qos_specifier.hpp b/include/realsense_gazebo_plugin/ros_qos_specifier.hpp index 423c715..564ebad 100644 --- a/include/realsense_gazebo_plugin/ros_qos_specifier.hpp +++ b/include/realsense_gazebo_plugin/ros_qos_specifier.hpp @@ -12,20 +12,27 @@ #ifndef REALSENSE_GAZEBO_PLUGIN_ROS_QOS_SPECIFIER_HPP #define REALSENSE_GAZEBO_PLUGIN_ROS_QOS_SPECIFIER_HPP -class ImageTransportWithSpecifiedQos: public image_transport::ImageTransport { +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), - queue_size(2),ad_node_p(node){}; - image_transport::CameraPublisher advertise_camera( - const std::string & base_topic, uint32_t queue_size, rmw_qos_profile_t qos_profile); + 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); + 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; + rclcpp::Node::SharedPtr ad_node_p; + uint32_t queue_size; }; +} #endif //REALSENSE_GAZEBO_PLUGIN_ROS_QOS_SPECIFIER_HPP From 94f61d9469d3ae65b38bd3918b78827e193f200a Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 14 Dec 2021 13:46:29 +0200 Subject: [PATCH 09/20] -> gazebo_ros_realsense.h manually formatted --- .../gazebo_ros_realsense.h | 22 +++++-------------- 1 file changed, 6 insertions(+), 16 deletions(-) diff --git a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h index aa9b285..429e800 100644 --- a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h +++ b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h @@ -13,52 +13,43 @@ #include #include -namespace gazebo -{ +namespace gazebo { /// \brief A plugin that simulates Real Sense camera streams. class GazeboRosRealsense : public RealSensePlugin { /// \brief Constructor. - public: GazeboRosRealsense(); /// \brief Destructor. - public: ~GazeboRosRealsense(); // Documentation Inherited. - public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); /// \brief Callback that publishes a received Depth Camera Frame as an /// ImageStamped message. - public: virtual void OnNewDepthFrame(); /// \brief Helper function to fill the pointcloud information - bool FillPointCloudHelper( - sensor_msgs::msg::PointCloud2 & point_cloud_msg, uint32_t rows_arg, - uint32_t cols_arg, uint32_t step_arg, void * data_arg); + bool FillPointCloudHelper(sensor_msgs::msg::PointCloud2 & point_cloud_msg, uint32_t rows_arg, + uint32_t cols_arg, uint32_t step_arg, void * data_arg); /// \brief Callback that publishes a received Camera Frame as an /// ImageStamped message. - public: - virtual void OnNewFrame( - const rendering::CameraPtr cam, - const transport::PublisherPtr pub); + virtual void OnNewFrame(const rendering::CameraPtr cam, + const transport::PublisherPtr pub); protected: boost::shared_ptr - camera_info_manager_; + camera_info_manager_; /// \brief A pointer to the ROS node. /// A node will be instantiated if it does not exist. - protected: rclcpp::Node::SharedPtr node_; @@ -71,7 +62,6 @@ class GazeboRosRealsense : public RealSensePlugin image_transport::CameraPublisher color_pub_, ir1_pub_, ir2_pub_, depth_pub_; /// \brief ROS image messages - protected: sensor_msgs::msg::Image image_msg_, depth_msg_; sensor_msgs::msg::PointCloud2 pointcloud_msg_; From 6db52881e562eb2c8d53a463cc8c8b7b5acb35f4 Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 14 Dec 2021 13:53:09 +0200 Subject: [PATCH 10/20] -> namespace fix --- src/ros_qos_specifier.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ros_qos_specifier.cpp b/src/ros_qos_specifier.cpp index 6369a7e..2e30767 100644 --- a/src/ros_qos_specifier.cpp +++ b/src/ros_qos_specifier.cpp @@ -1,13 +1,13 @@ #include "realsense_gazebo_plugin/ros_qos_specifier.hpp" -image_transport::CameraPublisher ImageTransportWithSpecifiedQos::advertise_camera( +image_transport::CameraPublisher gazebo::ImageTransportWithSpecifiedQos::advertise_camera( const std::string & base_topic, uint32_t queue_size, rmw_qos_profile_t qos_profile) { rmw_qos_profile_t custom_qos = qos_profile; custom_qos.depth = queue_size; return image_transport::CameraPublisher(ad_node_p.get(), base_topic, custom_qos); } -void ImageTransportWithSpecifiedQos::specify_color_qos( +void gazebo::ImageTransportWithSpecifiedQos::specify_color_qos( image_transport::CameraPublisher &cam_color_pub, std::basic_string topic_name, std::string colorQos) { if(colorQos=="SensorDataQoS") { From 8085e8676ce010de301c54a6ca48ffa8f6bffc7e Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 14 Dec 2021 14:05:16 +0200 Subject: [PATCH 11/20] -> this-> "is no need anymore" -> intermediate copy removed --- src/gazebo_ros_realsense.cpp | 18 +++++++++--------- src/ros_qos_specifier.cpp | 5 ++--- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index d65c48b..4310523 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -43,30 +43,30 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { this->itnode_.reset(new ImageTransportWithSpecifiedQos(this->node_)); - itnode_->specify_color_qos(this->color_pub_,cameraParamsMap_[COLOR_CAMERA_NAME].topic_name,colorQos); - itnode_->specify_color_qos(this->ir1_pub_,cameraParamsMap_[IRED1_CAMERA_NAME].topic_name,colorQos); - itnode_->specify_color_qos(this->ir2_pub_,cameraParamsMap_[IRED2_CAMERA_NAME].topic_name,colorQos); - itnode_->specify_color_qos(this->depth_pub_,cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name,colorQos); + 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_) { if(pointCloudQos=="SensorDataQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( + pointcloud_pub_ = node_->create_publisher( pointCloudTopic_, rclcpp::SensorDataQoS()); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SensorDataQoS."); } else if(pointCloudQos=="ParametersQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( + pointcloud_pub_ = node_->create_publisher( pointCloudTopic_, rclcpp::ParametersQoS()); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParametersQoS."); } else if(pointCloudQos=="ServicesQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( + pointcloud_pub_ = node_->create_publisher( pointCloudTopic_, rclcpp::ServicesQoS()); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ServicesQoS."); } else if(pointCloudQos=="ParameterEventsQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( + pointcloud_pub_ = node_->create_publisher( pointCloudTopic_, rclcpp::ParameterEventsQoS()); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParameterEventsQoS."); } else { - this->pointcloud_pub_ = this->node_->create_publisher( + pointcloud_pub_ = node_->create_publisher( pointCloudTopic_, rclcpp::SystemDefaultsQoS()); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SystemDefaultsQoS."); } diff --git a/src/ros_qos_specifier.cpp b/src/ros_qos_specifier.cpp index 2e30767..bd7b5e6 100644 --- a/src/ros_qos_specifier.cpp +++ b/src/ros_qos_specifier.cpp @@ -2,9 +2,8 @@ image_transport::CameraPublisher gazebo::ImageTransportWithSpecifiedQos::advertise_camera( const std::string & base_topic, uint32_t queue_size, rmw_qos_profile_t qos_profile) { - rmw_qos_profile_t custom_qos = qos_profile; - custom_qos.depth = queue_size; - return image_transport::CameraPublisher(ad_node_p.get(), base_topic, custom_qos); + qos_profile.depth = queue_size; + return image_transport::CameraPublisher(ad_node_p.get(), base_topic, qos_profile); } void gazebo::ImageTransportWithSpecifiedQos::specify_color_qos( From 30bd9481dbe5fa595d2b7cac5a467363fbc364b7 Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 14 Dec 2021 17:49:23 +0200 Subject: [PATCH 12/20] -> Reverted commit -> Format_style -> Copy fix -> This removed --- .../realsense_gazebo_plugin/RealSensePlugin.h | 4 +- src/RealSensePlugin.cpp | 8 +-- src/gazebo_ros_realsense.cpp | 40 ++++++------- src/ros_qos_specifier.cpp | 60 +++++++++---------- 4 files changed, 55 insertions(+), 57 deletions(-) diff --git a/include/realsense_gazebo_plugin/RealSensePlugin.h b/include/realsense_gazebo_plugin/RealSensePlugin.h index a96b7bf..c2d01cf 100644 --- a/include/realsense_gazebo_plugin/RealSensePlugin.h +++ b/include/realsense_gazebo_plugin/RealSensePlugin.h @@ -126,8 +126,8 @@ class RealSensePlugin : public ModelPlugin { bool pointCloud_ = false; std::string pointCloudTopic_; - std::string pointCloudQos; - std::string colorQos; + std::basic_string pointCloudQos; + std::basic_stringcolorQos; double pointCloudCutOff_, pointCloudCutOffMax_; double colorUpdateRate_; diff --git a/src/RealSensePlugin.cpp b/src/RealSensePlugin.cpp index 53b2596..144e1d7 100644 --- a/src/RealSensePlugin.cpp +++ b/src/RealSensePlugin.cpp @@ -117,10 +117,10 @@ void RealSensePlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) } 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") { + this->colorQos = _sdf->GetValue()->GetAsString(); + } else if (name == "pointcloud_qos") { + this->pointCloudQos = _sdf->GetValue()->GetAsString(); + } else if (name == "robotNamespace") { break; } else { throw std::runtime_error("Ivalid parameter for RealSensePlugin"); diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index 4c9672a..c370e46 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -49,42 +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 ImageTransportWithSpecifiedQos(this->node_)); - - itnode_->specify_color_qos(this->color_pub_,cameraParamsMap_[COLOR_CAMERA_NAME].topic_name,colorQos); - itnode_->specify_color_qos(this->ir1_pub_,cameraParamsMap_[IRED1_CAMERA_NAME].topic_name,colorQos); - itnode_->specify_color_qos(this->ir2_pub_,cameraParamsMap_[IRED2_CAMERA_NAME].topic_name,colorQos); - itnode_->specify_color_qos(this->depth_pub_,cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name,colorQos); - - if (pointCloud_) { - if(pointCloudQos=="SensorDataQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( - pointCloudTopic_, rclcpp::SensorDataQoS()); - RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SensorDataQoS."); + itnode_.reset(new ImageTransportWithSpecifiedQos(node_)); + + 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_) { + if(pointCloudQos=="SensorDataQoS") { + pointcloud_pub_ = node_->create_publisher(pointCloudTopic_, + rclcpp::SensorDataQoS()); + RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SensorDataQoS."); } else if(pointCloudQos=="ParametersQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( + pointcloud_pub_ = node_->create_publisher( pointCloudTopic_, rclcpp::ParametersQoS()); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParametersQoS."); } else if(pointCloudQos=="ServicesQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( + pointcloud_pub_ = node_->create_publisher( pointCloudTopic_, rclcpp::ServicesQoS()); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ServicesQoS."); } else if(pointCloudQos=="ParameterEventsQoS") { - this->pointcloud_pub_ = this->node_->create_publisher( + pointcloud_pub_ = node_->create_publisher( pointCloudTopic_, rclcpp::ParameterEventsQoS()); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParameterEventsQoS."); - } else { - this->pointcloud_pub_ = this->node_->create_publisher( + } else { + pointcloud_pub_ = node_->create_publisher( pointCloudTopic_, rclcpp::SystemDefaultsQoS()); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SystemDefaultsQoS."); } - } - + } RCLCPP_INFO(node_->get_logger(), "Loaded Realsense Gazebo ROS plugin."); } -void GazeboRosRealsense::OnNewFrame( - const rendering::CameraPtr cam, +void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam, const transport::PublisherPtr pub) { rclcpp::Time current_time = this->node_->now(); diff --git a/src/ros_qos_specifier.cpp b/src/ros_qos_specifier.cpp index 6369a7e..de9ed0c 100644 --- a/src/ros_qos_specifier.cpp +++ b/src/ros_qos_specifier.cpp @@ -1,34 +1,34 @@ #include "realsense_gazebo_plugin/ros_qos_specifier.hpp" -image_transport::CameraPublisher ImageTransportWithSpecifiedQos::advertise_camera( - const std::string & base_topic, uint32_t queue_size, rmw_qos_profile_t qos_profile) { - rmw_qos_profile_t custom_qos = qos_profile; - custom_qos.depth = queue_size; - return image_transport::CameraPublisher(ad_node_p.get(), base_topic, custom_qos); +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 ImageTransportWithSpecifiedQos::specify_color_qos( - image_transport::CameraPublisher &cam_color_pub, - std::basic_string topic_name, std::string colorQos) { - if(colorQos=="SensorDataQoS") { - cam_color_pub = this->advertise_camera( - topic_name, 2,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") { - cam_color_pub = this->advertise_camera( - topic_name, 2,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") { - cam_color_pub = this->advertise_camera( - topic_name, 2,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") { - cam_color_pub = this->advertise_camera( - topic_name, 2,rclcpp::ParameterEventsQoS().get_rmw_qos_profile()); - RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using ParameterEventsQoS."); - } else { - cam_color_pub = this->advertise_camera( - topic_name, 2,rclcpp::SystemDefaultsQoS().get_rmw_qos_profile()); - RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using SystemDefaultsQoS."); - } - } \ No newline at end of file +void gazebo::ImageTransportWithSpecifiedQos::specify_color_qos( + image_transport::CameraPublisher &cam_color_pub, + std::basic_string topic_name, std::string colorQos) +{ + if(colorQos=="SensorDataQoS") { + cam_color_pub = this->advertise_camera(topic_name, 2,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") { + cam_color_pub = this->advertise_camera( + topic_name, 2,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") { + cam_color_pub = this->advertise_camera( + topic_name, 2,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") { + cam_color_pub = this->advertise_camera( + topic_name, 2,rclcpp::ParameterEventsQoS().get_rmw_qos_profile()); + RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using ParameterEventsQoS."); + } else { + cam_color_pub = this->advertise_camera( + topic_name, 2,rclcpp::SystemDefaultsQoS().get_rmw_qos_profile()); + RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using SystemDefaultsQoS."); + } +} \ No newline at end of file From 262d512f023a579b71c5877c61504a4385633548 Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 14 Dec 2021 18:08:22 +0200 Subject: [PATCH 13/20] Revert "-> gazebo_ros_realsense.h manually formatted" This reverts commit 94f61d94 --- .../gazebo_ros_realsense.h | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h index 429e800..aa9b285 100644 --- a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h +++ b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h @@ -13,43 +13,52 @@ #include #include -namespace gazebo { +namespace gazebo +{ /// \brief A plugin that simulates Real Sense camera streams. class GazeboRosRealsense : public RealSensePlugin { /// \brief Constructor. + public: GazeboRosRealsense(); /// \brief Destructor. + public: ~GazeboRosRealsense(); // Documentation Inherited. + public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); /// \brief Callback that publishes a received Depth Camera Frame as an /// ImageStamped message. + public: virtual void OnNewDepthFrame(); /// \brief Helper function to fill the pointcloud information - bool FillPointCloudHelper(sensor_msgs::msg::PointCloud2 & point_cloud_msg, uint32_t rows_arg, - uint32_t cols_arg, uint32_t step_arg, void * data_arg); + bool FillPointCloudHelper( + sensor_msgs::msg::PointCloud2 & point_cloud_msg, uint32_t rows_arg, + uint32_t cols_arg, uint32_t step_arg, void * data_arg); /// \brief Callback that publishes a received Camera Frame as an /// ImageStamped message. + public: - virtual void OnNewFrame(const rendering::CameraPtr cam, - const transport::PublisherPtr pub); + virtual void OnNewFrame( + const rendering::CameraPtr cam, + const transport::PublisherPtr pub); protected: boost::shared_ptr - camera_info_manager_; + camera_info_manager_; /// \brief A pointer to the ROS node. /// A node will be instantiated if it does not exist. + protected: rclcpp::Node::SharedPtr node_; @@ -62,6 +71,7 @@ class GazeboRosRealsense : public RealSensePlugin image_transport::CameraPublisher color_pub_, ir1_pub_, ir2_pub_, depth_pub_; /// \brief ROS image messages + protected: sensor_msgs::msg::Image image_msg_, depth_msg_; sensor_msgs::msg::PointCloud2 pointcloud_msg_; From dd50391b5e0898272f12ad1a81f49bd6b9d383fd Mon Sep 17 00:00:00 2001 From: Oleksandr <86951670+OleksandrMoskaliuk8800@users.noreply.github.com> Date: Tue, 14 Dec 2021 18:12:17 +0200 Subject: [PATCH 14/20] Update ros_qos_specifier.cpp --- src/ros_qos_specifier.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros_qos_specifier.cpp b/src/ros_qos_specifier.cpp index de9ed0c..f216a2b 100644 --- a/src/ros_qos_specifier.cpp +++ b/src/ros_qos_specifier.cpp @@ -31,4 +31,4 @@ void gazebo::ImageTransportWithSpecifiedQos::specify_color_qos( topic_name, 2,rclcpp::SystemDefaultsQoS().get_rmw_qos_profile()); RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using SystemDefaultsQoS."); } -} \ No newline at end of file +} From 5e359effe86e23a7237ae89c015f959eea117795 Mon Sep 17 00:00:00 2001 From: Oleksandr <86951670+OleksandrMoskaliuk8800@users.noreply.github.com> Date: Tue, 14 Dec 2021 18:37:16 +0200 Subject: [PATCH 15/20] Update RealSensePlugin.cpp --- src/RealSensePlugin.cpp | 140 ++++++++++++++++++++-------------------- 1 file changed, 71 insertions(+), 69 deletions(-) diff --git a/src/RealSensePlugin.cpp b/src/RealSensePlugin.cpp index 144e1d7..cb9721d 100644 --- a/src/RealSensePlugin.cpp +++ b/src/RealSensePlugin.cpp @@ -58,75 +58,77 @@ 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") { - _sdf->GetValue()->Get(depthUpdateRate_); - } else if (name == "colorUpdateRate") { - _sdf->GetValue()->Get(colorUpdateRate_); - } else if (name == "infraredUpdateRate") { - _sdf->GetValue()->Get(infraredUpdateRate_); - } else if (name == "depthTopicName") { - cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "depthCameraInfoTopicName") { - cameraParamsMap_[DEPTH_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "colorTopicName") { - cameraParamsMap_[COLOR_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "colorCameraInfoTopicName") { - cameraParamsMap_[COLOR_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared1TopicName") { - cameraParamsMap_[IRED1_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared1CameraInfoTopicName") { - cameraParamsMap_[IRED1_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared2TopicName") { - cameraParamsMap_[IRED2_CAMERA_NAME].topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared2CameraInfoTopicName") { - cameraParamsMap_[IRED2_CAMERA_NAME].camera_info_topic_name = - _sdf->GetValue()->GetAsString(); - } else if (name == "colorOpticalframeName") { - cameraParamsMap_[COLOR_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "depthOpticalframeName") { - cameraParamsMap_[DEPTH_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared1OpticalframeName") { - cameraParamsMap_[IRED1_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "infrared2OpticalframeName") { - cameraParamsMap_[IRED2_CAMERA_NAME].optical_frame = - _sdf->GetValue()->GetAsString(); - } else if (name == "rangeMinDepth") { - _sdf->GetValue()->Get(rangeMinDepth_); - } else if (name == "rangeMaxDepth") { - _sdf->GetValue()->Get(rangeMaxDepth_); - } else if (name == "pointCloud") { - _sdf->GetValue()->Get(pointCloud_); - } else if (name == "pointCloudTopicName") { - pointCloudTopic_ = _sdf->GetValue()->GetAsString(); - } else if (name == "pointCloudCutoff") { - _sdf->GetValue()->Get(pointCloudCutOff_); - } else if (name == "pointCloudCutoffMax") { - _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 { - throw std::runtime_error("Ivalid parameter for RealSensePlugin"); - } - _sdf = _sdf->GetNextElement(); - } while (_sdf); + + do { + std::string name = _sdf->GetName(); + if (name == "depthUpdateRate") { + _sdf->GetValue()->Get(depthUpdateRate_); + } else if (name == "colorUpdateRate") { + _sdf->GetValue()->Get(colorUpdateRate_); + } else if (name == "infraredUpdateRate") { + _sdf->GetValue()->Get(infraredUpdateRate_); + } else if (name == "depthTopicName") { + cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "depthCameraInfoTopicName") { + cameraParamsMap_[DEPTH_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "colorTopicName") { + cameraParamsMap_[COLOR_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "colorCameraInfoTopicName") { + cameraParamsMap_[COLOR_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared1TopicName") { + cameraParamsMap_[IRED1_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared1CameraInfoTopicName") { + cameraParamsMap_[IRED1_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared2TopicName") { + cameraParamsMap_[IRED2_CAMERA_NAME].topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared2CameraInfoTopicName") { + cameraParamsMap_[IRED2_CAMERA_NAME].camera_info_topic_name = + _sdf->GetValue()->GetAsString(); + } else if (name == "colorOpticalframeName") { + cameraParamsMap_[COLOR_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + } else if (name == "depthOpticalframeName") { + cameraParamsMap_[DEPTH_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared1OpticalframeName") { + cameraParamsMap_[IRED1_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + } else if (name == "infrared2OpticalframeName") { + cameraParamsMap_[IRED2_CAMERA_NAME].optical_frame = + _sdf->GetValue()->GetAsString(); + } else if (name == "rangeMinDepth") { + _sdf->GetValue()->Get(rangeMinDepth_); + } else if (name == "rangeMaxDepth") { + _sdf->GetValue()->Get(rangeMaxDepth_); + } else if (name == "pointCloud") { + _sdf->GetValue()->Get(pointCloud_); + } else if (name == "pointCloudTopicName") { + pointCloudTopic_ = _sdf->GetValue()->GetAsString(); + } else if (name == "pointCloudCutoff") { + _sdf->GetValue()->Get(pointCloudCutOff_); + } else if (name == "pointCloudCutoffMax") { + _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 { + throw std::runtime_error("Ivalid parameter for ReakSensePlugin"); + } + + _sdf = _sdf->GetNextElement(); + } while (_sdf); // Store a pointer to the this model this->rsModel = _model; From 9b654f4e62a7ea5a05d69de803b72f8ad3bfcaa6 Mon Sep 17 00:00:00 2001 From: Oleksandr <86951670+OleksandrMoskaliuk8800@users.noreply.github.com> Date: Tue, 14 Dec 2021 18:39:20 +0200 Subject: [PATCH 16/20] Update gazebo_ros_realsense.cpp --- src/gazebo_ros_realsense.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index c370e46..2975814 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -82,7 +82,8 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) RCLCPP_INFO(node_->get_logger(), "Loaded Realsense Gazebo ROS plugin."); } -void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam, +void GazeboRosRealsense::OnNewFrame( + const rendering::CameraPtr cam, const transport::PublisherPtr pub) { rclcpp::Time current_time = this->node_->now(); From fc37a73bfc7b914d6457f12841ca48d13eba0d9d Mon Sep 17 00:00:00 2001 From: Oleksandr <86951670+OleksandrMoskaliuk8800@users.noreply.github.com> Date: Tue, 21 Dec 2021 16:12:42 +0200 Subject: [PATCH 17/20] Update RealSensePlugin.h --- include/realsense_gazebo_plugin/RealSensePlugin.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/realsense_gazebo_plugin/RealSensePlugin.h b/include/realsense_gazebo_plugin/RealSensePlugin.h index c2d01cf..d5e3e77 100644 --- a/include/realsense_gazebo_plugin/RealSensePlugin.h +++ b/include/realsense_gazebo_plugin/RealSensePlugin.h @@ -127,7 +127,7 @@ class RealSensePlugin : public ModelPlugin { bool pointCloud_ = false; std::string pointCloudTopic_; std::basic_string pointCloudQos; - std::basic_stringcolorQos; + std::basic_string colorQos; double pointCloudCutOff_, pointCloudCutOffMax_; double colorUpdateRate_; From f5ac9ded91e233d3cfa31130c6a895d3c9bb58c1 Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 21 Dec 2021 17:12:03 +0200 Subject: [PATCH 18/20] reduce code --- src/ros_qos_specifier.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/ros_qos_specifier.cpp b/src/ros_qos_specifier.cpp index f216a2b..d785a67 100644 --- a/src/ros_qos_specifier.cpp +++ b/src/ros_qos_specifier.cpp @@ -11,24 +11,24 @@ 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") { - cam_color_pub = this->advertise_camera(topic_name, 2,rclcpp::SensorDataQoS().get_rmw_qos_profile()); + 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") { - cam_color_pub = this->advertise_camera( - topic_name, 2,rclcpp::ParametersQoS().get_rmw_qos_profile()); + 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") { - cam_color_pub = this->advertise_camera( - topic_name, 2,rclcpp::ServicesQoS().get_rmw_qos_profile()); + 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") { - cam_color_pub = this->advertise_camera( - topic_name, 2,rclcpp::ParameterEventsQoS().get_rmw_qos_profile()); + 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 { - cam_color_pub = this->advertise_camera( - topic_name, 2,rclcpp::SystemDefaultsQoS().get_rmw_qos_profile()); - RCLCPP_INFO(this->ad_node_p->get_logger(), "Gazebo ROS Realsense plugin -> advertise camera using SystemDefaultsQoS."); + 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); + + } From 5d126632288e52e850548b5fdec4c28b4da50ddf Mon Sep 17 00:00:00 2001 From: Oleksandr Date: Tue, 21 Dec 2021 17:23:18 +0200 Subject: [PATCH 19/20] reduce code 2 --- src/gazebo_ros_realsense.cpp | 20 ++++++++------------ 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index 2975814..37672cd 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -57,27 +57,23 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) itnode_->specify_color_qos(depth_pub_,cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name,colorQos); if (pointCloud_) { - if(pointCloudQos=="SensorDataQoS") { - pointcloud_pub_ = node_->create_publisher(pointCloudTopic_, - rclcpp::SensorDataQoS()); - RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using SensorDataQoS."); + 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") { - pointcloud_pub_ = node_->create_publisher( - pointCloudTopic_, rclcpp::ParametersQoS()); + temp_qos = rclcpp::ParametersQoS(); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParametersQoS."); } else if(pointCloudQos=="ServicesQoS") { - pointcloud_pub_ = node_->create_publisher( - pointCloudTopic_, rclcpp::ServicesQoS()); + temp_qos = rclcpp::ServicesQoS(); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ServicesQoS."); } else if(pointCloudQos=="ParameterEventsQoS") { - pointcloud_pub_ = node_->create_publisher( - pointCloudTopic_, rclcpp::ParameterEventsQoS()); + temp_qos = rclcpp::ParameterEventsQoS(); RCLCPP_INFO(node_->get_logger(), "Gazebo ROS Realsense plugin -> publisher created using ParameterEventsQoS."); } else { - pointcloud_pub_ = node_->create_publisher( - pointCloudTopic_, rclcpp::SystemDefaultsQoS()); 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."); } From 0ef08862cb4eeaa4ac22aaabcfc3664786e65d83 Mon Sep 17 00:00:00 2001 From: Johannes Plapp Date: Sat, 8 Jan 2022 10:21:15 +0100 Subject: [PATCH 20/20] secs and nanosecs are used differently here --- src/gazebo_ros_realsense.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index 0652761..9ce6df3 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -87,8 +87,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.seconds(); - this->image_msg_.header.stamp.nanosec = current_time.nanoseconds(); + this->image_msg_.header.stamp = current_time; // set image encoding const std::map supported_image_encodings = { @@ -223,8 +222,7 @@ void GazeboRosRealsense::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.seconds(); - this->depth_msg_.header.stamp.nanosec = current_time.nanoseconds(); + this->depth_msg_.header.stamp = current_time; // set image encoding std::string pixel_format = sensor_msgs::image_encodings::TYPE_16UC1;