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 }
fawkes::PointCloudManager::remove_pointcloud
void remove_pointcloud(const char *id)
Remove the point cloud.
Definition: pointcloud_manager.cpp:66
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::ClockAspect::clock
Clock * clock
By means of this member access to the clock is given.
Definition: clock.h:42
DepthcamSimThread::finalize
virtual void finalize()
Finalize the thread.
Definition: gazsim_depthcam_thread.cpp:74
DepthcamSimThread::init
virtual void init()
Initialize the thread.
Definition: gazsim_depthcam_thread.cpp:50
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::Clock::now
Time now() const
Get the current time.
Definition: clock.cpp:242
fawkes
Fawkes library namespace.
pcl::PointCloud< pcl::PointXYZ >
DepthcamSimThread::DepthcamSimThread
DepthcamSimThread()
Constructor.
Definition: gazsim_depthcam_thread.cpp:43
fawkes::GazeboAspect::gazebo_world_node
gazebo::transport::NodePtr gazebo_world_node
Gazebo Node for communication with the world (e.g.
Definition: gazebo.h:51
fawkes::ConfigurableAspect::config
Configuration * config
This is the Configuration member used to access the configuration.
Definition: configurable.h:41
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::PointCloudManager::add_pointcloud
void add_pointcloud(const char *id, RefPtr< pcl::PointCloud< PointT >> cloud)
Add point cloud.
Definition: pointcloud_manager.h:78
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
DepthcamSimThread::loop
virtual void loop()
Code to execute in the thread.
Definition: gazsim_depthcam_thread.cpp:80
fawkes::PointCloudAspect::pcl_manager
PointCloudManager * pcl_manager
Manager to distribute and access point clouds.
Definition: pointcloud.h:47
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.