21 #include "gazsim_webcam.h" 26 #include <utils/math/angle.h> 28 #include <gazebo/transport/Node.hh> 29 #include <gazebo/msgs/msgs.hh> 30 #include <gazebo/transport/transport.hh> 32 #include <fvutils/color/conversions.h> 49 gazebo::transport::NodePtr gazebo_world_node,
54 std::string robot_name = config->
get_string(
"/gazsim/robot-name");
55 shm_id_ = robot_name +
"/" + shm_id;
58 + config->
get_string((std::string(
"/gazsim/webcam/topic-suffixes/")
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());
64 format_from_ = firevision::RGB;
65 format_to_ = firevision::YUV422_PLANAR;
70 webcam_sub_ = gazebo_world_node->Subscribe(topic_name_, &GazsimWebcam::on_webcam_data_msg,
this);
78 if (!shm_buffer_->is_valid()) {
81 shm_buffer_->set_frame_id(frame_.c_str());
82 buffer_ = shm_buffer_->buffer();
84 shm_buffer_->add_semaphore();
87 GazsimWebcam::~GazsimWebcam()
89 delete this->shm_buffer_;
92 void GazsimWebcam::on_webcam_data_msg(ConstImageStampedPtr &msg)
96 shm_buffer_->lock_for_write();
97 convert(format_from_, format_to_,
98 (
const unsigned char*) msg->image().data().data(), buffer_,
100 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.