22 #include "gazsim_depthcam_thread.h"
24 #include <aspect/logging.h>
26 #include <utils/math/angle.h>
28 #include <gazebo/msgs/msgs.hh>
29 #include <gazebo/transport/Node.hh>
30 #include <gazebo/transport/transport.hh>
35 using namespace gazebo;
44 :
Thread(
"DepthcamSimThread",
Thread::OPMODE_WAITFORWAKEUP),
60 gazebo_world_node->Subscribe(topic_name_, &DepthcamSimThread::on_depthcam_data_msg,
this);
64 pcl_->is_dense =
false;
66 pcl_->height = height_;
67 pcl_->points.resize((
size_t)width_ * (
size_t)height_);
68 pcl_->header.frame_id = frame_;
86 DepthcamSimThread::on_depthcam_data_msg(ConstPointCloudPtr &msg)
97 pcl_utils::set_time(pcl_, capture_time);
100 unsigned int idx = 0;
101 for (
unsigned int h = 0; h < height_; ++h) {
102 for (
unsigned int w = 0; w < width_; ++w, ++idx) {
104 pcl.points[idx].x = msg->points(idx).z();
105 pcl.points[idx].y = -msg->points(idx).x();
106 pcl.points[idx].z = msg->points(idx).y();