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
34using namespace fawkes;
35using 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
49void
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:
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
73void
75{
76 pcl_manager->remove_pointcloud(pcl_id_.c_str());
77}
78
79void
81{
82 //The interesting stuff happens in the callback of the depthcam
83}
84
85void
86DepthcamSimThread::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
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.