gazebo_ros_video.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 /*
19  * Desc: Video plugin for displaying ROS image topics on Ogre textures
20  * Author: Piyush Khandelwal
21  * Date: 26 July 2013
22  */
23 
25 #include <boost/lexical_cast.hpp>
26 
27 namespace gazebo
28 {
29 
31  const std::string &name, rendering::VisualPtr parent,
32  int height, int width) :
33  rendering::Visual(name, parent), height_(height), width_(width)
34  {
35 
36  texture_ = Ogre::TextureManager::getSingleton().createManual(
37  name + "__VideoTexture__",
38  Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
39  Ogre::TEX_TYPE_2D,
40  width_, height_,
41  0,
42  Ogre::PF_BYTE_BGRA,
43  Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);
44 
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);
51 
52  double factor = 1.0;
53 
54  Ogre::ManualObject mo(name + "__VideoObject__");
55  mo.begin(name + "__VideoMaterial__",
56  Ogre::RenderOperation::OT_TRIANGLE_LIST);
57 
58  mo.position(-factor / 2, factor / 2, 0.51);
59  mo.textureCoord(0, 0);
60 
61  mo.position(factor / 2, factor / 2, 0.51);
62  mo.textureCoord(1, 0);
63 
64  mo.position(factor / 2, -factor / 2, 0.51);
65  mo.textureCoord(1, 1);
66 
67  mo.position(-factor / 2, -factor / 2, 0.51);
68  mo.textureCoord(0, 1);
69 
70  mo.triangle(0, 3, 2);
71  mo.triangle(2, 1, 0);
72  mo.end();
73 
74  mo.convertToMesh(name + "__VideoMesh__");
75 
76  Ogre::MovableObject *obj = (Ogre::MovableObject*)
77  GetSceneNode()->getCreator()->createEntity(
78  name + "__VideoEntity__",
79  name + "__VideoMesh__");
80  obj->setCastShadows(false);
81  AttachObject(obj);
82  }
83 
85 
86  void VideoVisual::render(const cv::Mat& image)
87  {
88 
89  // Fix image size if necessary
90  const cv::Mat* image_ptr = &image;
91  cv::Mat converted_image;
92  if (image_ptr->rows != height_ || image_ptr->cols != width_)
93  {
94  cv::resize(*image_ptr, converted_image, cv::Size(width_, height_));
95  image_ptr = &converted_image;
96  }
97 
98  // Get the pixel buffer
99  Ogre::HardwarePixelBufferSharedPtr pixelBuffer =
100  texture_->getBuffer();
101 
102  // Lock the pixel buffer and get a pixel box
103  pixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD);
104  const Ogre::PixelBox& pixelBox = pixelBuffer->getCurrentLock();
105  uint8_t* pDest = static_cast<uint8_t*>(pixelBox.data);
106 
107  memcpy(pDest, image_ptr->data, height_ * width_ * 4);
108 
109  // Unlock the pixel buffer
110  pixelBuffer->unlock();
111  }
112 
113  // Constructor
115 
116  // Destructor
118  update_connection_.reset();
119 
120  // Custom Callback Queue
121  queue_.clear();
122  queue_.disable();
123  rosnode_->shutdown();
124  callback_queue_thread_.join();
125 
126  delete rosnode_;
127  }
128 
129  // Load the controller
131  rendering::VisualPtr parent, sdf::ElementPtr sdf)
132  {
133 
134  model_ = parent;
135  sdf::ElementPtr p_sdf;
136  if (sdf->HasElement("sdf"))
137  {
138  p_sdf = sdf->GetElement("sdf");
139  }
140  else
141  {
142  p_sdf = sdf;
143  }
144 
145  robot_namespace_ = "";
146  if (!p_sdf->HasElement("robotNamespace"))
147  {
148  ROS_WARN_NAMED("video", "GazeboRosVideo plugin missing <robotNamespace>, "
149  "defaults to \"%s\".", robot_namespace_.c_str());
150  }
151  else
152  {
153  robot_namespace_ =
154  p_sdf->GetElement("robotNamespace")->Get<std::string>();
155  }
156 
157  topic_name_ = "image_raw";
158  if (!p_sdf->HasElement("topicName"))
159  {
160  ROS_WARN_NAMED("video", "GazeboRosVideo Plugin (ns = %s) missing <topicName>, "
161  "defaults to \"%s\".", robot_namespace_.c_str(), topic_name_.c_str());
162  }
163  else
164  {
165  topic_name_ = p_sdf->GetElement("topicName")->Get<std::string>();
166  }
167 
168  int height = 240;
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);
172  }
173  else
174  {
175  height = p_sdf->GetElement("height")->Get<int>();
176  }
177 
178  int width = 320;
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);
182  }
183  else
184  {
185  width = p_sdf->GetElement("width")->Get<int>();
186  }
187 
188  std::string name = robot_namespace_ + "_visual";
189  video_visual_.reset(
190  new VideoVisual(name, parent, height, width));
191 
192  // Initialize the ROS node for the gazebo client if necessary
193  if (!ros::isInitialized())
194  {
195  int argc = 0;
196  char** argv = NULL;
197  ros::init(argc, argv, "gazebo_client",
199  }
200  std::string gazebo_source =
201  (ros::this_node::getName() == "/gazebo_client") ? "gzclient" : "gzserver";
202  rosnode_ = new ros::NodeHandle(robot_namespace_);
203 
204  // Subscribe to the image topic
206  ros::SubscribeOptions::create<sensor_msgs::Image>(topic_name_, 1,
207  boost::bind(&GazeboRosVideo::processImage, this, _1),
208  ros::VoidPtr(), &queue_);
209  camera_subscriber_ = rosnode_->subscribe(so);
210 
211  new_image_available_ = false;
212 
213  callback_queue_thread_ =
214  boost::thread(boost::bind(&GazeboRosVideo::QueueThread, this));
215 
216  update_connection_ =
217  event::Events::ConnectPreRender(
218  boost::bind(&GazeboRosVideo::UpdateChild, this));
219 
220  ROS_INFO_NAMED("video", "GazeboRosVideo (%s, ns = %s) has started",
221  gazebo_source.c_str(), robot_namespace_.c_str());
222  }
223 
224  // Update the controller
226  {
227  boost::mutex::scoped_lock scoped_lock(m_image_);
228  if (new_image_available_)
229  {
230  video_visual_->render(image_->image);
231  }
232  new_image_available_ = false;
233  }
234 
235  void GazeboRosVideo::processImage(const sensor_msgs::ImageConstPtr &msg)
236  {
237  // Get a reference to the image from the image message pointer
238  boost::mutex::scoped_lock scoped_lock(m_image_);
239  // We get image with alpha channel as it allows memcpy onto ogre texture
240  image_ = cv_bridge::toCvCopy(msg, "bgra8");
241  new_image_available_ = true;
242  }
243 
245  {
246  static const double timeout = 0.01;
247  while (rosnode_->ok())
248  {
249  queue_.callAvailable(ros::WallDuration(timeout));
250  }
251  }
252 
254 }
#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()
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


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39