25 #include <boost/lexical_cast.hpp> 31 const std::string &name, rendering::VisualPtr parent,
32 int height,
int width) :
33 rendering::Visual(name, parent), height_(height), width_(width)
36 texture_ = Ogre::TextureManager::getSingleton().createManual(
37 name +
"__VideoTexture__",
38 Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
43 Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);
45 Ogre::MaterialPtr material =
46 Ogre::MaterialManager::getSingleton().create(
47 name +
"__VideoMaterial__",
"General");
48 material->getTechnique(0)->getPass(0)->createTextureUnitState(
49 name +
"__VideoTexture__");
50 material->setReceiveShadows(
false);
54 Ogre::ManualObject mo(name +
"__VideoObject__");
55 mo.begin(name +
"__VideoMaterial__",
56 Ogre::RenderOperation::OT_TRIANGLE_LIST);
58 mo.position(-factor / 2, factor / 2, 0.51);
59 mo.textureCoord(0, 0);
61 mo.position(factor / 2, factor / 2, 0.51);
62 mo.textureCoord(1, 0);
64 mo.position(factor / 2, -factor / 2, 0.51);
65 mo.textureCoord(1, 1);
67 mo.position(-factor / 2, -factor / 2, 0.51);
68 mo.textureCoord(0, 1);
74 mo.convertToMesh(name +
"__VideoMesh__");
76 Ogre::MovableObject *obj = (Ogre::MovableObject*)
77 GetSceneNode()->getCreator()->createEntity(
78 name +
"__VideoEntity__",
79 name +
"__VideoMesh__");
80 obj->setCastShadows(
false);
90 const cv::Mat* image_ptr = ℑ
91 cv::Mat converted_image;
94 cv::resize(*image_ptr, converted_image, cv::Size(
width_,
height_));
95 image_ptr = &converted_image;
99 Ogre::HardwarePixelBufferSharedPtr pixelBuffer =
103 pixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD);
104 const Ogre::PixelBox& pixelBox = pixelBuffer->getCurrentLock();
105 uint8_t* pDest =
static_cast<uint8_t*
>(pixelBox.data);
110 pixelBuffer->unlock();
118 update_connection_.reset();
123 rosnode_->shutdown();
124 callback_queue_thread_.join();
131 rendering::VisualPtr parent, sdf::ElementPtr sdf)
135 sdf::ElementPtr p_sdf;
136 if (sdf->HasElement(
"sdf"))
138 p_sdf = sdf->GetElement(
"sdf");
145 robot_namespace_ =
"";
146 if (!p_sdf->HasElement(
"robotNamespace"))
148 ROS_WARN_NAMED(
"video",
"GazeboRosVideo plugin missing <robotNamespace>, " 149 "defaults to \"%s\".", robot_namespace_.c_str());
154 p_sdf->GetElement(
"robotNamespace")->Get<std::string>();
157 topic_name_ =
"image_raw";
158 if (!p_sdf->HasElement(
"topicName"))
160 ROS_WARN_NAMED(
"video",
"GazeboRosVideo Plugin (ns = %s) missing <topicName>, " 161 "defaults to \"%s\".", robot_namespace_.c_str(), topic_name_.c_str());
165 topic_name_ = p_sdf->GetElement(
"topicName")->Get<std::string>();
169 if (!p_sdf->HasElement(
"height")) {
170 ROS_WARN_NAMED(
"video",
"GazeboRosVideo Plugin (ns = %s) missing <height>, " 171 "defaults to %i.", robot_namespace_.c_str(), height);
175 height = p_sdf->GetElement(
"height")->Get<
int>();
179 if (!p_sdf->HasElement(
"width")) {
180 ROS_WARN_NAMED(
"video",
"GazeboRosVideo Plugin (ns = %s) missing <width>, " 181 "defaults to %i", robot_namespace_.c_str(), width);
185 width = p_sdf->GetElement(
"width")->Get<
int>();
188 std::string name = robot_namespace_ +
"_visual";
200 std::string gazebo_source =
206 ros::SubscribeOptions::create<sensor_msgs::Image>(topic_name_, 1,
209 camera_subscriber_ = rosnode_->subscribe(so);
211 new_image_available_ =
false;
213 callback_queue_thread_ =
217 event::Events::ConnectPreRender(
220 ROS_INFO_NAMED(
"video",
"GazeboRosVideo (%s, ns = %s) has started",
221 gazebo_source.c_str(), robot_namespace_.c_str());
227 boost::mutex::scoped_lock scoped_lock(m_image_);
228 if (new_image_available_)
230 video_visual_->render(image_->image);
232 new_image_available_ =
false;
238 boost::mutex::scoped_lock scoped_lock(m_image_);
241 new_image_available_ =
true;
246 static const double timeout = 0.01;
247 while (rosnode_->ok())
#define ROS_INFO_NAMED(name,...)
void processImage(const sensor_msgs::ImageConstPtr &msg)
#define ROS_WARN_NAMED(name,...)
VideoVisual(const std::string &name, rendering::VisualPtr parent, int height, int width)
ROSCPP_DECL bool isInitialized()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
virtual void UpdateChild()
virtual ~GazeboRosVideo()
void render(const cv::Mat &image)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
GZ_REGISTER_VISUAL_PLUGIN(GazeboRosVideo)
Ogre::TexturePtr texture_
void Load(rendering::VisualPtr parent, sdf::ElementPtr sdf)
boost::shared_ptr< void > VoidPtr