22 #include "gazsim_depthcam_thread.h" 27 #include <utils/math/angle.h> 29 #include <gazebo/transport/Node.hh> 30 #include <gazebo/msgs/msgs.hh> 31 #include <gazebo/transport/transport.hh> 32 #include <aspect/logging.h> 44 :
Thread(
"DepthcamSimThread",
Thread::OPMODE_WAITFORWAKEUP),
58 depthcam_sub_ =
gazebo_world_node->Subscribe(topic_name_, &DepthcamSimThread::on_depthcam_data_msg,
this);
62 pcl_->is_dense =
false;
64 pcl_->height = height_;
65 pcl_->points.resize(width_ * height_);
66 pcl_->header.frame_id = frame_;
81 void DepthcamSimThread::on_depthcam_data_msg(ConstPointCloudPtr &msg)
92 pcl_utils::set_time(pcl_, capture_time);
96 for (
unsigned int h = 0; h < height_; ++h) {
97 for (
unsigned int w = 0; w < width_; ++w, ++idx) {
99 pcl.points[idx].x = msg->points(idx).z();
100 pcl.points[idx].y = -msg->points(idx).x();
101 pcl.points[idx].z = msg->points(idx).y();
void remove_pointcloud(const char *id)
Remove the point cloud.
Fawkes library namespace.
gazebo::transport::NodePtr gazebo_world_node
Gazebo Node for communication with the world (e.g.
virtual void finalize()
Finalize the thread.
A class for handling time.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
Clock * clock
By means of this member access to the clock is given.
Time now() const
Get the current time.
Thread aspect to use blocked timing.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
virtual void init()
Initialize the thread.
const char * name() const
Get name of thread.
DepthcamSimThread()
Constructor.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void loop()
Code to execute in the thread.
Configuration * config
This is the Configuration member used to access the configuration.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.