Fawkes API  Fawkes Development Version
gazsim_depthcam_thread.cpp
1 /***************************************************************************
2  * gazsim_depthcam_plugin.cpp - Plugin simulates a Depthcam in Gazebo and
3  * provides a point cloud
4  *
5  * Created: Fri Feb 19 20:59:05 2016
6  * Copyright 2016 Frederik Zwilling
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #include "gazsim_depthcam_thread.h"
23 
24 #include <aspect/logging.h>
25 #include <tf/types.h>
26 #include <utils/math/angle.h>
27 
28 #include <gazebo/msgs/msgs.hh>
29 #include <gazebo/transport/Node.hh>
30 #include <gazebo/transport/transport.hh>
31 #include <math.h>
32 #include <stdio.h>
33 
34 using namespace fawkes;
35 using namespace gazebo;
36 
37 /** @class DepthcamSimThread "gazsim_depthcam_thread.h"
38  * Thread simulates a number of depthcams in Gazebo
39  * @author Frederik Zwilling
40  */
41 
42 /** Constructor. */
44 : Thread("DepthcamSimThread", Thread::OPMODE_WAITFORWAKEUP),
45  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS)
46 {
47 }
48 
49 void
51 {
52  logger->log_debug(name(), "Initializing Simulation of the Depthcam");
53  topic_name_ = config->get_string("/gazsim/depthcam/topic");
54  width_ = config->get_float("/gazsim/depthcam/width");
55  height_ = config->get_float("/gazsim/depthcam/height");
56  frame_ = config->get_string("/gazsim/depthcam/frame");
57  pcl_id_ = config->get_string("/gazsim/depthcam/pointcloud-id");
58 
59  depthcam_sub_ =
60  gazebo_world_node->Subscribe(topic_name_, &DepthcamSimThread::on_depthcam_data_msg, this);
61 
62  //create pointcloud:
63  pcl_ = new pcl::PointCloud<pcl::PointXYZ>();
64  pcl_->is_dense = false;
65  pcl_->width = width_;
66  pcl_->height = height_;
67  pcl_->points.resize((size_t)width_ * (size_t)height_);
68  pcl_->header.frame_id = frame_;
69 
70  pcl_manager->add_pointcloud(pcl_id_.c_str(), pcl_);
71 }
72 
73 void
75 {
76  pcl_manager->remove_pointcloud(pcl_id_.c_str());
77 }
78 
79 void
81 {
82  //The interesting stuff happens in the callback of the depthcam
83 }
84 
85 void
86 DepthcamSimThread::on_depthcam_data_msg(ConstPointCloudPtr &msg)
87 {
88  // logger->log_info(name(), "Got Point Cloud!");
89 
90  //only write when pcl is used
91  // if (pcl_.use_count() > 1)
92  // {
93  fawkes::Time capture_time = clock->now();
94 
95  pcl::PointCloud<pcl::PointXYZ> &pcl = **pcl_;
96  pcl.header.seq += 1;
97  pcl_utils::set_time(pcl_, capture_time);
98 
99  //insert or update points in pointcloud
100  unsigned int idx = 0;
101  for (unsigned int h = 0; h < height_; ++h) {
102  for (unsigned int w = 0; w < width_; ++w, ++idx) {
103  // Fill in XYZ
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();
107  }
108  }
109  // }
110 }
virtual void finalize()
Finalize the thread.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
Thread aspect to use blocked timing.
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
Time now() const
Get the current time.
Definition: clock.cpp:242
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
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.
Definition: gazebo.h:51
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.
Definition: logging.h:41
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
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.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Fawkes library namespace.