gazebo_ros_video.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Open Source Robotics Foundation
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 /*
00019  * Desc: Video plugin for displaying ROS image topics on Ogre textures 
00020  * Author: Piyush Khandelwal
00021  * Date: 26 July 2013
00022  */
00023 
00024 #include <gazebo_plugins/gazebo_ros_video.h>
00025 #include <boost/lexical_cast.hpp>
00026 
00027 namespace gazebo 
00028 {
00029 
00030   VideoVisual::VideoVisual(
00031       const std::string &name, rendering::VisualPtr parent, 
00032       int height, int width) : 
00033       rendering::Visual(name, parent), height_(height), width_(width) 
00034   {
00035 
00036     texture_ = Ogre::TextureManager::getSingleton().createManual(
00037         name + "__VideoTexture__",
00038         Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
00039         Ogre::TEX_TYPE_2D,
00040         width_, height_,
00041         0,
00042         Ogre::PF_BYTE_BGRA,
00043         Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);
00044 
00045     Ogre::MaterialPtr material =
00046       Ogre::MaterialManager::getSingleton().create(
00047           name + "__VideoMaterial__", "General");
00048     material->getTechnique(0)->getPass(0)->createTextureUnitState(
00049         name + "__VideoTexture__");
00050     material->setReceiveShadows(false);
00051 
00052     double factor = 1.0;
00053 
00054     Ogre::ManualObject mo(name + "__VideoObject__");
00055     mo.begin(name + "__VideoMaterial__",
00056         Ogre::RenderOperation::OT_TRIANGLE_LIST);
00057 
00058     mo.position(-factor / 2, factor / 2, 0.51);
00059     mo.textureCoord(0, 0);
00060 
00061     mo.position(factor / 2, factor / 2, 0.51);
00062     mo.textureCoord(1, 0);
00063 
00064     mo.position(factor / 2, -factor / 2, 0.51);
00065     mo.textureCoord(1, 1);
00066 
00067     mo.position(-factor / 2, -factor / 2, 0.51);
00068     mo.textureCoord(0, 1);
00069 
00070     mo.triangle(0, 3, 2);
00071     mo.triangle(2, 1, 0);
00072     mo.end();
00073 
00074     mo.convertToMesh(name + "__VideoMesh__");
00075 
00076     Ogre::MovableObject *obj = (Ogre::MovableObject*)
00077       this->GetSceneNode()->getCreator()->createEntity(
00078           name + "__VideoEntity__",
00079           name + "__VideoMesh__");
00080     obj->setCastShadows(false);
00081     this->AttachObject(obj);
00082   }
00083 
00084   VideoVisual::~VideoVisual() {}
00085 
00086   void VideoVisual::render(const cv::Mat& image) 
00087   {
00088 
00089     // Fix image size if necessary
00090     const cv::Mat* image_ptr = &image;
00091     cv::Mat converted_image;
00092     if (image_ptr->rows != height_ || image_ptr->cols != width_) 
00093     {
00094       cv::resize(*image_ptr, converted_image, cv::Size(width_, height_));
00095       image_ptr = &converted_image;
00096     }
00097 
00098     // Get the pixel buffer
00099     Ogre::HardwarePixelBufferSharedPtr pixelBuffer = 
00100       this->texture_->getBuffer();
00101 
00102     // Lock the pixel buffer and get a pixel box
00103     pixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD);
00104     const Ogre::PixelBox& pixelBox = pixelBuffer->getCurrentLock();
00105     uint8_t* pDest = static_cast<uint8_t*>(pixelBox.data);
00106 
00107     memcpy(pDest, image_ptr->data, height_ * width_ * 4);
00108 
00109     // Unlock the pixel buffer
00110     pixelBuffer->unlock();
00111   }
00112 
00113   // Constructor
00114   GazeboRosVideo::GazeboRosVideo() {}
00115 
00116   // Destructor
00117   GazeboRosVideo::~GazeboRosVideo() {}
00118 
00119   // Load the controller
00120   void GazeboRosVideo::Load(
00121       rendering::VisualPtr parent, sdf::ElementPtr sdf) 
00122   {
00123 
00124     model_ = parent;
00125 
00126     robot_namespace_ = "";
00127     if (!sdf->HasElement("robotNamespace")) 
00128     {
00129       ROS_WARN("GazeboRosVideo plugin missing <robotNamespace>, "
00130           "defaults to \"%s\".", robot_namespace_.c_str());
00131     }
00132     else 
00133     {
00134       robot_namespace_ = 
00135         sdf->GetElement("robotNamespace")->Get<std::string>();
00136     }
00137 
00138     topic_name_ = "image_raw";
00139     if (!sdf->HasElement("topicName")) 
00140     {
00141       ROS_WARN("GazeboRosVideo Plugin (ns = %s) missing <topicName>, "
00142           "defaults to \"%s\".", robot_namespace_.c_str(), topic_name_.c_str());
00143     } 
00144     else 
00145     {
00146       topic_name_ = sdf->GetElement("topicName")->Get<std::string>();
00147     }
00148 
00149     int height = 240;
00150     if (!sdf->HasElement("height")) {
00151       ROS_WARN("GazeboRosVideo Plugin (ns = %s) missing <height>, "
00152           "defaults to %i.", robot_namespace_.c_str(), height);
00153     } 
00154     else 
00155     {
00156       height = sdf->GetElement("height")->Get<int>();
00157     }
00158 
00159     int width = 320;
00160     if (!sdf->HasElement("width")) {
00161       ROS_WARN("GazeboRosVideo Plugin (ns = %s) missing <width>, "
00162           "defaults to %i", robot_namespace_.c_str(), width);
00163     } 
00164     else 
00165     {
00166       width = sdf->GetElement("width")->Get<int>();
00167     }
00168 
00169     std::string name = robot_namespace_ + "_visual";
00170     video_visual_.reset(
00171         new VideoVisual(name, parent, height, width));
00172 
00173     // Initialize the ROS node for the gazebo client if necessary
00174     if (!ros::isInitialized()) 
00175     {
00176       int argc = 0;
00177       char** argv = NULL;
00178       ros::init(argc, argv, "gazebo_client", 
00179           ros::init_options::NoSigintHandler);
00180     }
00181     std::string gazebo_source = 
00182       (ros::this_node::getName() == "/gazebo_client") ? "gzclient" : "gzserver";
00183     rosnode_.reset(new ros::NodeHandle(robot_namespace_));
00184 
00185     // Subscribe to the image topic
00186     ros::SubscribeOptions so =
00187       ros::SubscribeOptions::create<sensor_msgs::Image>(topic_name_, 1,
00188           boost::bind(&GazeboRosVideo::processImage, this, _1),
00189           ros::VoidPtr(), &queue_);
00190     camera_subscriber_ = 
00191       rosnode_->subscribe(so);
00192     ROS_INFO("GazeboRosVideo (%s, ns = %s) has started!", 
00193         gazebo_source.c_str(), robot_namespace_.c_str());
00194     new_image_available_ = false;
00195 
00196     this->callback_queue_thread_ = 
00197       boost::thread(boost::bind(&GazeboRosVideo::QueueThread, this));
00198 
00199     this->update_connection_ = 
00200       event::Events::ConnectPreRender(
00201           boost::bind(&GazeboRosVideo::UpdateChild, this));
00202   }
00203 
00204   // Update the controller
00205   void GazeboRosVideo::UpdateChild() 
00206   {
00207     boost::mutex::scoped_lock scoped_lock(m_image_);
00208     if (new_image_available_) 
00209     {
00210       video_visual_->render(image_->image);
00211     }
00212     new_image_available_ = false;
00213   }
00214 
00215   void GazeboRosVideo::processImage(const sensor_msgs::ImageConstPtr &msg) 
00216   {
00217     // Get a reference to the image from the image message pointer
00218     boost::mutex::scoped_lock scoped_lock(m_image_);
00219     // We get image with alpha channel as it allows memcpy onto ogre texture
00220     image_ = cv_bridge::toCvCopy(msg, "bgra8");
00221     new_image_available_ = true;
00222   }
00223 
00224   void GazeboRosVideo::QueueThread() 
00225   {
00226     static const double timeout = 0.01;
00227     while (rosnode_->ok()) 
00228     {
00229       queue_.callAvailable(ros::WallDuration(timeout));
00230     }
00231   }
00232 
00233   GZ_REGISTER_VISUAL_PLUGIN(GazeboRosVideo);
00234 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Fri Aug 28 2015 10:47:25