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 <fvutils/color/conversions.h>
24 #include <tf/types.h>
25 #include <utils/math/angle.h>
26 
27 #include <gazebo/msgs/msgs.hh>
28 #include <gazebo/transport/Node.hh>
29 #include <gazebo/transport/transport.hh>
30 #include <math.h>
31 #include <stdio.h>
32 
33 using namespace fawkes;
34 
35 /** @class GazsimWebcam "gazsim_webcam.h"
36  * Simulates a single webcam in Gazebo
37  * @author Frederik Zwilling
38  */
39 
40 /** Constructor.
41  * The GazsimWebcam object simulates a single webcam in Gazebo
42  * @param shm_id The shared memory id the simulated webcam should write to and the prefix of the config values for this camera
43  * @param gazebo_world_node gazebo world node to register subscribers
44  * @param config config object to access config values
45  */
46 GazsimWebcam::GazsimWebcam(std::string shm_id,
47  gazebo::transport::NodePtr gazebo_world_node,
48  Configuration * config)
49 {
50  shm_buffer_ = NULL;
51  //read config values
52  std::string robot_name = config->get_string("/gazsim/robot-name");
53  shm_id_ = robot_name + "/" + shm_id;
54  topic_name_ =
55  ("~/" + robot_name
56  + config->get_string((std::string("/gazsim/webcam/topic-suffixes/") + shm_id).c_str()));
57  width_ = config->get_float((std::string("/gazsim/webcam/widths/") + shm_id).c_str());
58  height_ = config->get_float((std::string("/gazsim/webcam/heights/") + shm_id).c_str());
59  frame_ = config->get_string((std::string("/gazsim/webcam/frames/") + shm_id).c_str());
60 
61  format_from_ = firevision::RGB;
62  format_to_ = firevision::YUV422_PLANAR;
63 
64  //subscribing to gazebo publisher
65  //the messages are published by the sensor itself and not by a robot plugin
66  //therefore we have to use the world node
67  webcam_sub_ = gazebo_world_node->Subscribe(topic_name_, &GazsimWebcam::on_webcam_data_msg, this);
68 
69  //initialize shared memory image buffer
70  shm_buffer_ =
71  new firevision::SharedMemoryImageBuffer(shm_id_.c_str(), format_to_, width_, height_);
72  if (!shm_buffer_->is_valid()) {
73  throw fawkes::Exception("Shared memory segment not valid");
74  }
75  shm_buffer_->set_frame_id(frame_.c_str());
76  buffer_ = shm_buffer_->buffer();
77  //enable locking
78  shm_buffer_->add_semaphore();
79 }
80 
81 GazsimWebcam::~GazsimWebcam()
82 {
83  delete this->shm_buffer_;
84 }
85 
86 void
87 GazsimWebcam::on_webcam_data_msg(ConstImageStampedPtr &msg)
88 {
89  //convert image data and write it in the shared memory buffer
90  //lock the shm so noone can read a half written image
91  shm_buffer_->lock_for_write();
92  convert(format_from_,
93  format_to_,
94  (const unsigned char *)msg->image().data().data(),
95  buffer_,
96  width_,
97  height_);
98  shm_buffer_->unlock();
99 }
firevision::SharedMemoryImageBuffer
Definition: shm_image.h:182
fawkes::Configuration
Definition: config.h:68
fawkes
fawkes::Configuration::get_float
virtual float get_float(const char *path)=0
GazsimWebcam::GazsimWebcam
GazsimWebcam(std::string shm_id, gazebo::transport::NodePtr gazebo_world_node, fawkes::Configuration *config)
Constructor.
Definition: gazsim_webcam.cpp:46
fawkes::Configuration::get_string
virtual std::string get_string(const char *path)=0
fawkes::Exception
Definition: exception.h:39