Fawkes API  Fawkes Development Version
gazsim_webcam.cpp
1 /***************************************************************************
2  * gazsim_webcam.cpp - Class to simulate a single webcam from gazebo
3  *
4  * Created: Mon Mar 16 19:43:05 2015
5  * Copyright 2015 Frederik Zwilling
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "gazsim_webcam.h"
22 
23 #include <tf/types.h>
24 #include <stdio.h>
25 #include <math.h>
26 #include <utils/math/angle.h>
27 
28 #include <gazebo/transport/Node.hh>
29 #include <gazebo/msgs/msgs.hh>
30 #include <gazebo/transport/transport.hh>
31 
32 #include <fvutils/color/conversions.h>
33 
34 using namespace fawkes;
35 
36 /** @class GazsimWebcam "gazsim_webcam.h"
37  * Simulates a single webcam in Gazebo
38  * @author Frederik Zwilling
39  */
40 
41 
42 /** Constructor.
43  * The GazsimWebcam object simulates a single webcam in Gazebo
44  * @param shm_id The shared memory id the simulated webcam should write to and the prefix of the config values for this camera
45  * @param gazebo_world_node gazebo world node to register subscribers
46  * @param config config object to access config values
47  */
48 GazsimWebcam::GazsimWebcam(std::string shm_id,
49  gazebo::transport::NodePtr gazebo_world_node,
50  Configuration* config)
51 {
52  shm_buffer_ = NULL;
53  //read config values
54  std::string robot_name = config->get_string("/gazsim/robot-name");
55  shm_id_ = robot_name + "/" + shm_id;
56  topic_name_ = ("~/"
57  + robot_name
58  + config->get_string((std::string("/gazsim/webcam/topic-suffixes/")
59  + shm_id).c_str()));
60  width_ = config->get_float((std::string("/gazsim/webcam/widths/") + shm_id).c_str());
61  height_ = config->get_float((std::string("/gazsim/webcam/heights/") + shm_id).c_str());
62  frame_ = config->get_string((std::string("/gazsim/webcam/frames/") + shm_id).c_str());
63 
64  format_from_ = firevision::RGB;
65  format_to_ = firevision::YUV422_PLANAR;
66 
67  //subscribing to gazebo publisher
68  //the messages are published by the sensor itself and not by a robot plugin
69  //therefore we have to use the world node
70  webcam_sub_ = gazebo_world_node->Subscribe(topic_name_, &GazsimWebcam::on_webcam_data_msg, this);
71 
72  //initialize shared memory image buffer
73  shm_buffer_ = new firevision::SharedMemoryImageBuffer( shm_id_.c_str(),
74  format_to_,
75  width_,
76  height_
77  );
78  if (!shm_buffer_->is_valid()) {
79  throw fawkes::Exception("Shared memory segment not valid");
80  }
81  shm_buffer_->set_frame_id(frame_.c_str());
82  buffer_ = shm_buffer_->buffer();
83  //enable locking
84  shm_buffer_->add_semaphore();
85 }
86 
87 GazsimWebcam::~GazsimWebcam()
88 {
89  delete this->shm_buffer_;
90 }
91 
92 void GazsimWebcam::on_webcam_data_msg(ConstImageStampedPtr &msg)
93 {
94  //convert image data and write it in the shared memory buffer
95  //lock the shm so noone can read a half written image
96  shm_buffer_->lock_for_write();
97  convert(format_from_, format_to_,
98  (const unsigned char*) msg->image().data().data(), buffer_,
99  width_, height_);
100  shm_buffer_->unlock();
101 }
Fawkes library namespace.
Base class for exceptions in Fawkes.
Definition: exception.h:36
Shared memory image buffer.
Definition: shm_image.h:181
GazsimWebcam(std::string shm_id, gazebo::transport::NodePtr gazebo_world_node, fawkes::Configuration *config)
Constructor.
Interface for configuration handling.
Definition: config.h:67
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.