diff --git a/ros1_ign_gazebo_demos/README.md b/ros1_ign_gazebo_demos/README.md index c7d200dc..302d8737 100644 --- a/ros1_ign_gazebo_demos/README.md +++ b/ros1_ign_gazebo_demos/README.md @@ -44,7 +44,7 @@ Then send a command ## Depth camera -Publishes depth camera image. +Publishes depth camera images and point clouds. roslaunch ros1_ign_gazebo_demos depth_camera.launch @@ -52,7 +52,7 @@ Publishes depth camera image. ## GPU lidar -Publishes 2D laser scans. +Publishes 2D laser scans and point clouds. roslaunch ros1_ign_gazebo_demos gpu_lidar.launch diff --git a/ros1_ign_gazebo_demos/images/depth_camera_demo.png b/ros1_ign_gazebo_demos/images/depth_camera_demo.png index bf6c726b..9b4d27d3 100644 Binary files a/ros1_ign_gazebo_demos/images/depth_camera_demo.png and b/ros1_ign_gazebo_demos/images/depth_camera_demo.png differ diff --git a/ros1_ign_gazebo_demos/images/gpu_lidar_demo.png b/ros1_ign_gazebo_demos/images/gpu_lidar_demo.png index 5309aa03..a5ae7e76 100644 Binary files a/ros1_ign_gazebo_demos/images/gpu_lidar_demo.png and b/ros1_ign_gazebo_demos/images/gpu_lidar_demo.png differ diff --git a/ros1_ign_gazebo_demos/launch/depth_camera.launch b/ros1_ign_gazebo_demos/launch/depth_camera.launch index 2b63afe9..e60fa396 100644 --- a/ros1_ign_gazebo_demos/launch/depth_camera.launch +++ b/ros1_ign_gazebo_demos/launch/depth_camera.launch @@ -2,7 +2,7 @@ - + + + diff --git a/ros1_ign_gazebo_demos/launch/gpu_lidar.launch b/ros1_ign_gazebo_demos/launch/gpu_lidar.launch index 0827bfa8..05bb6391 100644 --- a/ros1_ign_gazebo_demos/launch/gpu_lidar.launch +++ b/ros1_ign_gazebo_demos/launch/gpu_lidar.launch @@ -2,7 +2,7 @@ - + - + + Value: Orbit (rviz) + Yaw: 4.723598957061768 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 568 + Hide Left Dock: false + Hide Right Dock: false + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000001e2fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001e2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d0065007200610000000272000000c1000000000000000000000001000001b8000001e2fc0200000006fb0000000a0049006d006100670065010000003b000001e20000001600fffffffb0000000c00430061006d00650072006100000001ae000001850000000000000000fb0000000c00430061006d00650072006101000001ba000001790000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002f8000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b000000052fc0100000002fb0000000800540069006d006502000001a70000029a000004b000000052fb0000000800540069006d0065010000000000000450000000000000000000000196000001e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 405 + Y: 378 diff --git a/ros1_ign_gazebo_demos/rviz/gpu_lidar.rviz b/ros1_ign_gazebo_demos/rviz/gpu_lidar.rviz index 153e6227..9161d0c8 100644 --- a/ros1_ign_gazebo_demos/rviz/gpu_lidar.rviz +++ b/ros1_ign_gazebo_demos/rviz/gpu_lidar.rviz @@ -1,13 +1,14 @@ Panels: - Class: rviz/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /LaserScan1 + - /PointCloud21 Splitter Ratio: 0.5 - Tree Height: 288 + Tree Height: 366 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -82,11 +83,41 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.029999999329447746 + Style: Flat Squares + Topic: /custom_params/pc2 + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 Default Light: true - Fixed Frame: model_with_lidar/link + Fixed Frame: custom_params/link Frame Rate: 30 Name: root Tools: @@ -110,25 +141,25 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 13.194476127624512 + Distance: 10.596034049987793 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 1.7377599477767944 - Y: -0.8083325624465942 - Z: 0.38970595598220825 + X: 1.855783224105835 + Y: -0.6233639717102051 + Z: 0.7489486932754517 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6902022361755371 + Pitch: 0.5402030348777771 Target Frame: Value: Orbit (rviz) - Yaw: 3.6185994148254395 + Yaw: 3.7535903453826904 Saved: ~ Window Geometry: Displays: @@ -146,5 +177,5 @@ Window Geometry: Views: collapsed: false Width: 701 - X: 492 - Y: 396 + X: 352 + Y: 269 diff --git a/ros1_ign_point_cloud/examples/depth_camera.sdf b/ros1_ign_point_cloud/examples/depth_camera.sdf new file mode 100644 index 00000000..27fbf207 --- /dev/null +++ b/ros1_ign_point_cloud/examples/depth_camera.sdf @@ -0,0 +1,316 @@ + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/ros1_ign/control + /world/ros1_ign/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/ros1_ign/stats + + + + + docked + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 20 20 0.1 + + + + + + + 20 20 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 -1 0.5 0 0 0.7 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 3 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 0.2 + + + + + + + + 0.2 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + + + 5 0 0.5 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + R_FLOAT32 + + + 0.1 + 100 + + + 1 + 10 + true + depth_camera + + points + map + + + + + + + 9 0 0.5 0 0.0 3.14 + true + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + + + 0 1 3 0.0 0.0 1.57 + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Construction Cone + + + + + diff --git a/ros1_ign_point_cloud/examples/gpu_lidar.sdf b/ros1_ign_point_cloud/examples/gpu_lidar.sdf new file mode 100644 index 00000000..8a4eff19 --- /dev/null +++ b/ros1_ign_point_cloud/examples/gpu_lidar.sdf @@ -0,0 +1,299 @@ + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/ros1_ign/control + /world/ros1_ign/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/ros1_ign/stats + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 20 20 0.1 + + + + + + + 20 20 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 -1 0.5 0 0 0.7 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 3 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 0.2 + + + + + + + + 0.2 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + + + 5 0 0.5 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + lidar + 10 + + + + 640 + 1 + -1.396263 + 1.396263 + + + 16 + 1 + -0.261799 + 0.261799 + + + + 0.08 + 10.0 + 0.01 + + + 1 + true + + custom_params + pc2 + custom_params/link + + + + + + + 0 1 3 0.0 0.0 1.57 + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Construction Cone + + + + + diff --git a/ros1_ign_point_cloud/examples/ros1_ign_point_cloud.sdf b/ros1_ign_point_cloud/examples/rgbd_camera.sdf similarity index 99% rename from ros1_ign_point_cloud/examples/ros1_ign_point_cloud.sdf rename to ros1_ign_point_cloud/examples/rgbd_camera.sdf index ad7dde98..5577f421 100644 --- a/ros1_ign_point_cloud/examples/ros1_ign_point_cloud.sdf +++ b/ros1_ign_point_cloud/examples/rgbd_camera.sdf @@ -16,7 +16,7 @@ 3. Load the example world, unpaused: - ign gazebo -r examples/ros1_ign_point_cloud.sdf + ign gazebo -r examples/rgbd_camera.sdf 4. Launch RViz to visualize the point cloud: diff --git a/ros1_ign_point_cloud/src/point_cloud.cc b/ros1_ign_point_cloud/src/point_cloud.cc index 35ab1b05..1627add0 100644 --- a/ros1_ign_point_cloud/src/point_cloud.cc +++ b/ros1_ign_point_cloud/src/point_cloud.cc @@ -15,15 +15,17 @@ #include "point_cloud.hh" #include #include +#include +#include #include #include #include #include #include +#include #include #include #include -#include #include #include @@ -39,6 +41,18 @@ IGNITION_ADD_PLUGIN( using namespace ros1_ign_point_cloud; +/// \brief Types of sensors supported by this plugin +enum class SensorType { + /// \brief A camera which combines an RGB and a depth camera + RGBD_CAMERA, + + /// \brief Depth camera + DEPTH_CAMERA, + + /// \brief GPU lidar rays + GPU_LIDAR +}; + ////////////////////////////////////////////////// class ros1_ign_point_cloud::PointCloudPrivate { @@ -62,6 +76,10 @@ class ros1_ign_point_cloud::PointCloudPrivate /// \param[in] _ecm Immutable reference to ECM. public: void LoadRgbCamera(const ignition::gazebo::EntityComponentManager &_ecm); + /// \brief Get GPU rays from rendering. + /// \param[in] _ecm Immutable reference to ECM. + public: void LoadGpuRays(const ignition::gazebo::EntityComponentManager &_ecm); + /// \brief Rendering scene which manages the cameras. public: ignition::rendering::ScenePtr scene_; @@ -74,6 +92,9 @@ class ros1_ign_point_cloud::PointCloudPrivate /// \brief Rendering RGB camera public: std::shared_ptr rgb_camera_; + /// \brief Rendering GPU lidar + public: std::shared_ptr gpu_rays_; + /// \brief Keep latest image from RGB camera. public: ignition::rendering::Image rgb_image_; @@ -83,6 +104,9 @@ class ros1_ign_point_cloud::PointCloudPrivate /// \brief Connection to depth frame event. public: ignition::common::ConnectionPtr depth_connection_; + /// \brief Connection to GPU rays frame event. + public: ignition::common::ConnectionPtr gpu_rays_connection_; + /// \brief Node to publish ROS messages. public: std::unique_ptr rosnode_; @@ -100,6 +124,9 @@ class ros1_ign_point_cloud::PointCloudPrivate /// \brief Render scene name public: std::string scene_name_; + + /// \brief Type of sensor which this plugin is attached to. + public: SensorType type_; }; ////////////////////////////////////////////////// @@ -115,6 +142,25 @@ void PointCloud::Configure(const ignition::gazebo::Entity &_entity, { this->dataPtr->entity_ = _entity; + if (_ecm.Component(_entity) != nullptr) + { + this->dataPtr->type_ = SensorType::RGBD_CAMERA; + } + else if (_ecm.Component(_entity) != nullptr) + { + this->dataPtr->type_ = SensorType::DEPTH_CAMERA; + } + else if (_ecm.Component(_entity) != nullptr) + { + this->dataPtr->type_ = SensorType::GPU_LIDAR; + } + else + { + ROS_ERROR_NAMED("ros1_ign_point_cloud", + "Point cloud plugin must be attached to an RGBD camera, depth camera or GPU lidar."); + return; + } + // Initialize ROS if (!ros::isInitialized()) { @@ -161,15 +207,23 @@ void PointCloud::PostUpdate(const ignition::gazebo::UpdateInfo &_info, return; } - // Get rendering cameras - if (!this->dataPtr->depth_camera_) + // Get rendering objects + if (!this->dataPtr->depth_camera_ && + (this->dataPtr->type_ == SensorType::RGBD_CAMERA || + this->dataPtr->type_ == SensorType::DEPTH_CAMERA)) { this->dataPtr->LoadDepthCamera(_ecm); } - if (!this->dataPtr->rgb_camera_) + if (!this->dataPtr->rgb_camera_ && + this->dataPtr->type_ == SensorType::RGBD_CAMERA) { this->dataPtr->LoadRgbCamera(_ecm); } + if (!this->dataPtr->gpu_rays_ && + this->dataPtr->type_ == SensorType::GPU_LIDAR) + { + this->dataPtr->LoadGpuRays(_ecm); + } } ////////////////////////////////////////////////// @@ -179,13 +233,17 @@ void PointCloudPrivate::LoadDepthCamera( // Sensor name scoped from the model auto sensor_name = ignition::gazebo::scopedName(this->entity_, _ecm, "::", false); - sensor_name = sensor_name.substr(sensor_name.find("::") + 2) + "_depth"; + sensor_name = sensor_name.substr(sensor_name.find("::") + 2); // Get sensor - auto sensor = this->scene_->SensorByName(sensor_name); + auto sensor = this->scene_->SensorByName(sensor_name + "_depth"); if (!sensor) { - return; + sensor = this->scene_->SensorByName(sensor_name); + if (!sensor) + { + return; + } } this->depth_camera_ = @@ -230,6 +288,37 @@ void PointCloudPrivate::LoadRgbCamera( this->rgb_image_ = this->rgb_camera_->CreateImage(); } +////////////////////////////////////////////////// +void PointCloudPrivate::LoadGpuRays( + const ignition::gazebo::EntityComponentManager &_ecm) +{ + // Sensor name scoped from the model + auto sensor_name = + ignition::gazebo::scopedName(this->entity_, _ecm, "::", false); + sensor_name = sensor_name.substr(sensor_name.find("::") + 2); + + // Get sensor + auto sensor = this->scene_->SensorByName(sensor_name); + if (!sensor) + { + return; + } + + this->gpu_rays_ = + std::dynamic_pointer_cast(sensor); + if (!this->gpu_rays_) + { + ROS_ERROR_NAMED("ros1_ign_point_cloud", + "Rendering sensor named [%s] is not a depth camera", sensor_name.c_str()); + return; + } + + this->gpu_rays_connection_ = this->gpu_rays_->ConnectNewGpuRaysFrame( + std::bind(&PointCloudPrivate::OnNewDepthFrame, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); +} + ////////////////////////////////////////////////// void PointCloudPrivate::OnNewDepthFrame(const float *_scan, unsigned int _width, unsigned int _height, @@ -239,17 +328,28 @@ void PointCloudPrivate::OnNewDepthFrame(const float *_scan, if (this->pc_pub_.getNumSubscribers() <= 0 || _height == 0 || _width == 0) return; - // Just sanity checks, but don't prevent publishing - if (_channels != 1) + // Just sanity check, but don't prevent publishing + if (this->type_ == SensorType::RGBD_CAMERA && _channels != 1) { ROS_WARN_NAMED("ros1_ign_point_cloud", "Expected depth image to have 1 channel, but it has [%i]", _channels); } - if (_format != "FLOAT32") + if (this->type_ == SensorType::GPU_LIDAR && _channels != 3) + { + ROS_WARN_NAMED("ros1_ign_point_cloud", + "Expected GPU rays to have 3 channels, but it has [%i]", _channels); + } + if ((this->type_ == SensorType::RGBD_CAMERA || + this->type_ == SensorType::DEPTH_CAMERA) && _format != "FLOAT32") { ROS_WARN_NAMED("ros1_ign_point_cloud", "Expected depth image to have [FLOAT32] format, but it has [%s]", _format.c_str()); } + if (this->type_ == SensorType::GPU_LIDAR && _format != "PF_FLOAT32_RGB") + { + ROS_WARN_NAMED("ros1_ign_point_cloud", + "Expected GPU rays to have [PF_FLOAT32_RGB] format, but it has [%s]", _format.c_str()); + } // Fill message // Logic borrowed from @@ -283,53 +383,91 @@ void PointCloudPrivate::OnNewDepthFrame(const float *_scan, _width, 3 * _width, this->rgb_image_.Data()); } - double hfov = this->depth_camera_->HFOV().Radian(); - double fl = _width / (2.0 * tan(hfov / 2.0)); - int index{0}; - uint8_t * image_src = (uint8_t *)(&(this->rgb_image_msg_.data[0])); + // For depth calculation from image + double fl{0.0}; + if (nullptr != this->depth_camera_) + { + auto hfov = this->depth_camera_->HFOV().Radian(); + fl = _width / (2.0 * tan(hfov / 2.0)); + } + + // For depth calculation from laser scan + double angle_step{0.0}; + double vertical_angle_step{0.0}; + double inclination{0.0}; + double azimuth{0.0}; + if (nullptr != this->gpu_rays_) + { + angle_step = (this->gpu_rays_->AngleMax() - this->gpu_rays_->AngleMin()).Radian() / + (this->gpu_rays_->RangeCount()-1); + vertical_angle_step = (this->gpu_rays_->VerticalAngleMax() - + this->gpu_rays_->VerticalAngleMin()).Radian() / (this->gpu_rays_->VerticalRangeCount()-1); + + // Angles of ray currently processing, azimuth is horizontal, inclination is vertical + inclination = this->gpu_rays_->VerticalAngleMin().Radian(); + azimuth = this->gpu_rays_->AngleMin().Radian(); + } + + // For color calculation + uint8_t * image_src; + if (nullptr != this->rgb_camera_) + { + image_src = (uint8_t *)(&(this->rgb_image_msg_.data[0])); + } - // Convert depth to point cloud + // Iterate over scan and populate point cloud for (uint32_t j = 0; j < _height; ++j) { - double p_angle; - if (_height>1) - p_angle = atan2( (double)j - 0.5*(double)(_height-1), fl); - else - p_angle = 0.0; + double p_angle{0.0}; + if (fl > 0 && _height > 1) + p_angle = atan2((double)j - 0.5 * (double)(_height-1), fl); - for (uint32_t i=0; i<_width; i++, ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) + if (nullptr != this->gpu_rays_) { - double y_angle; - if (_width>1) - y_angle = atan2( (double)i - 0.5*(double)(_width-1), fl); - else - y_angle = 0.0; + azimuth = this->gpu_rays_->AngleMin().Radian(); + } + for (uint32_t i = 0; i < _width; ++i, ++iter_x, ++iter_y, ++iter_z, ++iter_r, ++iter_g, ++iter_b) + { + // Index of current point + auto index = j * _width * _channels + i * _channels; + double depth = _scan[index]; - double depth = _scan[index++]; + double y_angle{0.0}; + if (fl > 0 && _width > 1) + y_angle = atan2((double)i - 0.5 * (double)(_width-1), fl); - // in optical frame - // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in - // to urdf, where the *_optical_frame should have above relative - // rotation from the physical camera *_frame - *iter_x = depth * tan(y_angle); - *iter_y = depth * tan(p_angle); - if (depth > this->depth_camera_->FarClipPlane()) - { - *iter_z = ignition::math::INF_D; - msg.is_dense = false; - } - if (depth < this->depth_camera_->NearClipPlane()) + if (nullptr != this->depth_camera_) { - *iter_z = -ignition::math::INF_D; - msg.is_dense = false; + // in optical frame + // hardcoded rotation rpy(-M_PI/2, 0, -M_PI/2) is built-in + // to urdf, where the *_optical_frame should have above relative + // rotation from the physical camera *_frame + *iter_x = depth * tan(y_angle); + *iter_y = depth * tan(p_angle); + *iter_z = depth; + + // Clamp according to REP 117 + if (depth > this->depth_camera_->FarClipPlane()) + { + *iter_z = ignition::math::INF_D; + msg.is_dense = false; + } + if (depth < this->depth_camera_->NearClipPlane()) + { + *iter_z = -ignition::math::INF_D; + msg.is_dense = false; + } } - else + else if (nullptr != this->gpu_rays_) { - *iter_z = depth; + // Convert spherical coordinates to Cartesian for pointcloud + // See https://en.wikipedia.org/wiki/Spherical_coordinate_system + *iter_x = depth * cos(inclination) * cos(azimuth); + *iter_y = depth * cos(inclination) * sin(azimuth); + *iter_z = depth * sin(inclination); } // Put image color data for each point - // \TODO(anyone) RGB image seems offset from depth image by 3~4 pixels in both directions if (this->rgb_image_msg_.data.size() == _height * _width * 3) { // color @@ -351,7 +489,9 @@ void PointCloudPrivate::OnNewDepthFrame(const float *_scan, *iter_g = 0; *iter_b = 0; } + azimuth += angle_step; } + inclination += vertical_angle_step; } this->pc_pub_.publish(msg); diff --git a/ros1_ign_point_cloud/src/point_cloud.hh b/ros1_ign_point_cloud/src/point_cloud.hh index c29073ef..4d1c711b 100644 --- a/ros1_ign_point_cloud/src/point_cloud.hh +++ b/ros1_ign_point_cloud/src/point_cloud.hh @@ -23,9 +23,9 @@ namespace ros1_ign_point_cloud // Forward declarations. class PointCloudPrivate; - /// \brief System which publishes ROS PointCloud2 messages for RGBD sensors. + /// \brief System which publishes ROS PointCloud2 messages for RGBD or GPU lidar sensors. /// - /// This plugin should be attached to an RGBD sensor (i.e. ) + /// This plugin should be attached to an RGBD or GPU lidar sensor (i.e. ) /// /// Important: load `ignition::gazebo::systems::Sensors` as well, which will create the sensor. ///