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 <tf/types.h>
25 #include <stdio.h>
26 #include <math.h>
27 #include <utils/math/angle.h>
28 
29 #include <gazebo/transport/Node.hh>
30 #include <gazebo/msgs/msgs.hh>
31 #include <gazebo/transport/transport.hh>
32 #include <aspect/logging.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 
50 {
51  logger->log_debug(name(), "Initializing Simulation of the Depthcam");
52  topic_name_ = config->get_string("/gazsim/depthcam/topic");
53  width_ = config->get_float("/gazsim/depthcam/width");
54  height_ = config->get_float("/gazsim/depthcam/height");
55  frame_ = config->get_string("/gazsim/depthcam/frame");
56  pcl_id_ = config->get_string("/gazsim/depthcam/pointcloud-id");
57 
58  depthcam_sub_ = gazebo_world_node->Subscribe(topic_name_, &DepthcamSimThread::on_depthcam_data_msg, this);
59 
60  //create pointcloud:
61  pcl_ = new pcl::PointCloud<pcl::PointXYZ>();
62  pcl_->is_dense = false;
63  pcl_->width = width_;
64  pcl_->height = height_;
65  pcl_->points.resize(width_ * height_);
66  pcl_->header.frame_id = frame_;
67 
68  pcl_manager->add_pointcloud(pcl_id_.c_str(), pcl_);
69 }
70 
72 {
73  pcl_manager->remove_pointcloud(pcl_id_.c_str());
74 }
75 
77 {
78  //The interesting stuff happens in the callback of the depthcam
79 }
80 
81 void DepthcamSimThread::on_depthcam_data_msg(ConstPointCloudPtr &msg)
82 {
83  // logger->log_info(name(), "Got Point Cloud!");
84 
85  //only write when pcl is used
86  // if (pcl_.use_count() > 1)
87  // {
88  fawkes::Time capture_time = clock->now();
89 
91  pcl.header.seq += 1;
92  pcl_utils::set_time(pcl_, capture_time);
93 
94  //insert or update points in pointcloud
95  unsigned int idx = 0;
96  for (unsigned int h = 0; h < height_; ++h) {
97  for (unsigned int w = 0; w < width_; ++w, ++idx) {
98  // Fill in XYZ
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();
102  }
103  }
104  // }
105 }
Definition: gps.h:30
void remove_pointcloud(const char *id)
Remove the point cloud.
Definition: pointcloud.h:30
Fawkes library namespace.
gazebo::transport::NodePtr gazebo_world_node
Gazebo Node for communication with the world (e.g.
Definition: gazebo.h:52
virtual void finalize()
Finalize the thread.
A class for handling time.
Definition: time.h:91
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT > > cloud)
Add point cloud.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:45
Time now() const
Get the current time.
Definition: clock.cpp:269
Thread aspect to use blocked timing.
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:50
virtual void init()
Initialize the thread.
const char * name() const
Get name of thread.
Definition: thread.h:95
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.
Definition: configurable.h:44
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.