Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fixed Pointcloud without color issue #48 #52

Open
wants to merge 2 commits into
base: melodic-devel
Choose a base branch
from
Open

Fixed Pointcloud without color issue #48 #52

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
16be512
Fixed point cloud no color issue
marko-zaric Jan 10, 2023
3a0ebcc
Clean up
marko-zaric Jan 10, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions include/realsense_gazebo_plugin/gazebo_ros_realsense.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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_ */
41 changes: 19 additions & 22 deletions src/gazebo_ros_realsense.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,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(
Expand All @@ -64,20 +59,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<std::string, image_transport::CameraPublisher *>
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
Expand All @@ -86,11 +92,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<const void *>(cam->ImageData()));

// identify camera rendering
const std::map<std::string, rendering::CameraPtr> cameras = {
{COLOR_CAMERA_NAME, this->colorCam},
Expand All @@ -99,6 +105,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);
Expand Down Expand Up @@ -164,25 +171,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];
Expand Down