gazebo_ros_projector.cpp
Go to the documentation of this file.
00001 /*
00002  @mainpage
00003    Desc: GazeboRosTextureProjector plugin that controls texture projection
00004    Author: Jared Duke
00005    Date: 17 Jun 2010
00006    SVN info: $Id$
00007  @htmlinclude manifest.html
00008  @b GazeboRosTextureProjector plugin that controls texture projection
00009  */
00010 
00011 #include <algorithm>
00012 #include <assert.h>
00013 #include <utility>
00014 #include <sstream>
00015 
00016 #include "rendering/Rendering.hh"
00017 #include "rendering/Scene.hh"
00018 #include "rendering/Visual.hh"
00019 #include "rendering/RTShaderSystem.hh"
00020 #include <gazebo_plugins/gazebo_ros_projector.h>
00021 
00022 #include "std_msgs/String.h"
00023 #include "std_msgs/Int32.h"
00024 
00025 #include <Ogre.h>
00026 #include <OgreMath.h>
00027 #include <OgreSceneNode.h>
00028 #include <OgreFrustum.h>
00029 #include <OgreSceneQuery.h>
00030 
00031 namespace gazebo
00032 {
00033 
00034 typedef std::map<std::string,Ogre::Pass*> OgrePassMap;
00035 typedef OgrePassMap::iterator OgrePassMapIterator;
00036 
00037 
00039 // Constructor
00040 GazeboRosProjector::GazeboRosProjector()
00041 {
00042   this->rosnode_ = NULL;
00043 }
00044 
00045 
00047 // Destructor
00048 GazeboRosProjector::~GazeboRosProjector()
00049 {
00050   // Custom Callback Queue
00051   this->queue_.clear();
00052   this->queue_.disable();
00053   this->rosnode_->shutdown();
00054   this->callback_queue_thread_.join();
00055 
00056   delete this->rosnode_;
00057 }
00058 
00060 // Load the controller
00061 void GazeboRosProjector::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00062 {
00063   this->world_ = _parent->GetWorld();
00064 
00065 
00066 
00067 
00068   // Create a new transport node for talking to the projector
00069   this->node_.reset(new transport::Node());
00070   // Initialize the node with the world name
00071   this->node_->Init(this->world_->GetName());
00072   // Setting projector topic
00073   std::string name = std::string("~/") + _parent->GetName() + "/" +
00074                       _sdf->GetValueString("projector");
00075   // Create a publisher on the ~/physics topic
00076   this->projector_pub_ = node_->Advertise<msgs::Projector>(name);
00077 
00078 
00079 
00080   // load parameters
00081   this->robot_namespace_ = "";
00082   if (_sdf->HasElement("robotNamespace"))
00083     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00084 
00085   this->texture_topic_name_ = "";
00086   if (_sdf->HasElement("textureTopicName"))
00087     this->texture_topic_name_ = _sdf->GetElement("textureTopicName")->GetValueString();
00088 
00089   this->projector_topic_name_ = "";
00090   if (_sdf->HasElement("projectorTopicName"))
00091     this->projector_topic_name_ = _sdf->GetElement("projectorTopicName")->GetValueString();
00092 
00093   // initialize ros
00094   if (!ros::isInitialized())
00095   {
00096     int argc = 0;
00097     char** argv = NULL;
00098     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00099   }
00100   
00101   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00102 
00103 
00104   // Custom Callback Queue
00105   ros::SubscribeOptions so = ros::SubscribeOptions::create<std_msgs::Int32>(
00106     this->projector_topic_name_,1,
00107     boost::bind( &GazeboRosProjector::ToggleProjector,this,_1),
00108     ros::VoidPtr(), &this->queue_);
00109   this->projectorSubscriber_ = this->rosnode_->subscribe(so);
00110 
00111   ros::SubscribeOptions so2 = ros::SubscribeOptions::create<std_msgs::String>(
00112     this->texture_topic_name_,1,
00113     boost::bind( &GazeboRosProjector::LoadImage,this,_1),
00114     ros::VoidPtr(), &this->queue_);
00115   this->imageSubscriber_ = this->rosnode_->subscribe(so2);
00116 
00117 
00118   // Custom Callback Queue
00119   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosProjector::QueueThread,this ) );
00120 
00121 }
00122 
00123 
00125 // Load a texture into the projector 
00126 void GazeboRosProjector::LoadImage(const std_msgs::String::ConstPtr& imageMsg)
00127 {
00128   msgs::Projector msg;
00129   msg.set_name("texture_projector");
00130   msg.set_texture(imageMsg->data);
00131   this->projector_pub_->Publish(msg);
00132 }
00133 
00135 // Toggle the activation of the projector
00136 void GazeboRosProjector::ToggleProjector(const std_msgs::Int32::ConstPtr& projectorMsg)
00137 {
00138   msgs::Projector msg;
00139   msg.set_name("texture_projector");
00140   msg.set_enabled(projectorMsg->data);
00141   this->projector_pub_->Publish(msg);
00142 }
00143 
00144 
00146 // Custom callback queue thread
00147 void GazeboRosProjector::QueueThread()
00148 {
00149   static const double timeout = 0.01;
00150 
00151   while (this->rosnode_->ok())
00152   {
00153     this->queue_.callAvailable(ros::WallDuration(timeout));
00154   }
00155 }
00156 
00157 GZ_REGISTER_MODEL_PLUGIN(GazeboRosProjector);
00158 
00159 }


gazebo_plugins
Author(s): Sachin Chitta, Stu Glaser, John Hsu
autogenerated on Mon Oct 6 2014 12:15:44