From 16be51272e5435073db1e647cd4ac37c9994f1e7 Mon Sep 17 00:00:00 2001 From: m-zaric <56204944+m-zaric@users.noreply.github.com> Date: Tue, 10 Jan 2023 14:49:18 +0100 Subject: [PATCH 1/2] Fixed point cloud no color issue --- .../gazebo_ros_realsense.h | 2 + src/gazebo_ros_realsense.cpp | 42 +++++++++---------- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h index a1268f5..5f72dae 100644 --- a/include/realsense_gazebo_plugin/gazebo_ros_realsense.h +++ b/include/realsense_gazebo_plugin/gazebo_ros_realsense.h @@ -64,7 +64,9 @@ class GazeboRosRealsense : public RealSensePlugin { /// \brief ROS image messages protected: sensor_msgs::Image image_msg_, depth_msg_; + sensor_msgs::Image image_msg2_; sensor_msgs::PointCloud2 pointcloud_msg_; }; + } #endif /* _GAZEBO_ROS_REALSENSE_PLUGIN_ */ diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index b2d4091..160b252 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -1,6 +1,7 @@ #include "realsense_gazebo_plugin/gazebo_ros_realsense.h" #include #include +#include namespace { std::string extractCameraName(const std::string &name); @@ -40,11 +41,6 @@ void GazeboRosRealsense::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { this->itnode_ = new image_transport::ImageTransport(*this->rosnode_); - // set 'png' compression format for depth images - // default functional parameters for compressed_image_transport to have lossless png compression - rosnode_->setParam(rosnode_->resolveName(cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name) + "/compressed/format", "png"); - rosnode_->setParam(rosnode_->resolveName(cameraParamsMap_[DEPTH_CAMERA_NAME].topic_name) + "/compressed/png_level", 1); - this->color_pub_ = this->itnode_->advertiseCamera( cameraParamsMap_[COLOR_CAMERA_NAME].topic_name, 2); this->ir1_pub_ = this->itnode_->advertiseCamera( @@ -64,20 +60,31 @@ void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam, const transport::PublisherPtr pub) { common::Time current_time = this->world->SimTime(); + // Save Color sensor msg from being overwritten by ir cams + int rows_arg = this->depthCam->ImageHeight(); + int cols_arg = this->depthCam->ImageWidth(); + if (this->image_msg_.data.size() == rows_arg * cols_arg * 3){ + this->image_msg2_ = this->image_msg_; + } + // 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_)}, }; + 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->image_msg_.header.stamp.sec = current_time.sec; + this->image_msg_.header.stamp.nsec = current_time.nsec; // set image encoding @@ -86,11 +93,11 @@ void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam, {"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 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}, @@ -99,6 +106,7 @@ void GazeboRosRealsense::OnNewFrame(const rendering::CameraPtr cam, }; // publish to ROS + auto camera_info_msg = cameraInfo(this->image_msg_, cameras.at(camera_id)->HFOV().Radian()); image_pub->publish(this->image_msg_, camera_info_msg); @@ -164,25 +172,15 @@ bool GazeboRosRealsense::FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cl } // put image color data for each point - uint8_t *image_src = (uint8_t *)(&(this->image_msg_.data[0])); - if (this->image_msg_.data.size() == rows_arg * cols_arg * 3) + uint8_t *image_src = (uint8_t *)(&(this->image_msg2_.data[0])); + if (this->image_msg2_.data.size() == rows_arg * cols_arg * 3) { // color - if (this->image_msg_.encoding == sensor_msgs::image_encodings::RGB8) { - iter_rgb[2] = image_src[i * 3 + j * cols_arg * 3 + 0]; - iter_rgb[1] = image_src[i * 3 + j * cols_arg * 3 + 1]; - iter_rgb[0] = image_src[i * 3 + j * cols_arg * 3 + 2]; - } - else if (this->image_msg_.encoding == sensor_msgs::image_encodings::BGR8) { - iter_rgb[0] = image_src[i * 3 + j * cols_arg * 3 + 0]; - iter_rgb[1] = image_src[i * 3 + j * cols_arg * 3 + 1]; - iter_rgb[2] = image_src[i * 3 + j * cols_arg * 3 + 2]; - } - else { - throw std::runtime_error("unsupported colour encoding: " + this->image_msg_.encoding); - } + iter_rgb[0] = image_src[i * 3 + j * cols_arg * 3 + 2]; // Red and Blue are flipped in sensor msg + iter_rgb[1] = image_src[i * 3 + j * cols_arg * 3 + 1]; + iter_rgb[2] = image_src[i * 3 + j * cols_arg * 3 + 0]; // Red and Blue are flipped in sensor msg } - else if (this->image_msg_.data.size() == rows_arg * cols_arg) + else if (this->image_msg2_.data.size() == rows_arg * cols_arg) { // mono (or bayer? @todo; fix for bayer) iter_rgb[0] = image_src[i + j * cols_arg]; From 3a0ebccc0b3caaa81aa4eead49dba68229775aac Mon Sep 17 00:00:00 2001 From: m-zaric <56204944+m-zaric@users.noreply.github.com> Date: Tue, 10 Jan 2023 14:55:41 +0100 Subject: [PATCH 2/2] Clean up --- src/gazebo_ros_realsense.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/gazebo_ros_realsense.cpp b/src/gazebo_ros_realsense.cpp index 160b252..2f1295a 100644 --- a/src/gazebo_ros_realsense.cpp +++ b/src/gazebo_ros_realsense.cpp @@ -1,7 +1,6 @@ #include "realsense_gazebo_plugin/gazebo_ros_realsense.h" #include #include -#include namespace { std::string extractCameraName(const std::string &name);