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>
35using 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_;
86DepthcamSimThread::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();
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
DepthcamSimThread()
Constructor.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Time now() const
Get the current time.
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.
gazebo::transport::NodePtr gazebo_world_node
Gazebo Node for communication with the world (e.g.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
Logger * logger
This is the Logger member used to access the logger.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
void remove_pointcloud(const char *id)
Remove the point cloud.
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
A class for handling time.
Fawkes library namespace.