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();