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
33using 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 */
46GazsimWebcam::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
81GazsimWebcam::~GazsimWebcam()
82{
83 delete this->shm_buffer_;
84}
85
86void
87GazsimWebcam::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}
GazsimWebcam(std::string shm_id, gazebo::transport::NodePtr gazebo_world_node, fawkes::Configuration *config)
Constructor.
Interface for configuration handling.
Definition: config.h:68
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.
Base class for exceptions in Fawkes.
Definition: exception.h:36
bool is_valid() const
Check validity of shared memory segment.
Definition: shm.cpp:811
void add_semaphore()
Add semaphore to shared memory segment.
Definition: shm.cpp:852
void lock_for_write()
Lock shared memory segment for writing.
Definition: shm.cpp:959
void unlock()
Unlock memory.
Definition: shm.cpp:1025
Shared memory image buffer.
Definition: shm_image.h:184
unsigned char * buffer() const
Get image buffer.
Definition: shm_image.cpp:228
void set_frame_id(const char *frame_id)
Set frame ID.
Definition: shm_image.cpp:150
Fawkes library namespace.