21 #include "gazsim_webcam.h" 23 #include <fvutils/color/conversions.h> 25 #include <utils/math/angle.h> 27 #include <gazebo/msgs/msgs.hh> 28 #include <gazebo/transport/Node.hh> 29 #include <gazebo/transport/transport.hh> 47 gazebo::transport::NodePtr gazebo_world_node,
52 std::string robot_name = config->
get_string(
"/gazsim/robot-name");
53 shm_id_ = robot_name +
"/" + shm_id;
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());
61 format_from_ = firevision::RGB;
62 format_to_ = firevision::YUV422_PLANAR;
67 webcam_sub_ = gazebo_world_node->Subscribe(topic_name_, &GazsimWebcam::on_webcam_data_msg,
this);
72 if (!shm_buffer_->is_valid()) {
75 shm_buffer_->set_frame_id(frame_.c_str());
76 buffer_ = shm_buffer_->buffer();
78 shm_buffer_->add_semaphore();
81 GazsimWebcam::~GazsimWebcam()
83 delete this->shm_buffer_;
87 GazsimWebcam::on_webcam_data_msg(ConstImageStampedPtr &msg)
91 shm_buffer_->lock_for_write();
94 (
const unsigned char *)msg->image().data().data(),
98 shm_buffer_->unlock();
Fawkes library namespace.
Base class for exceptions in Fawkes.
Shared memory image buffer.
GazsimWebcam(std::string shm_id, gazebo::transport::NodePtr gazebo_world_node, fawkes::Configuration *config)
Constructor.
Interface for configuration handling.
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.