gazebo_ros_projector.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: GazeboRosTextureProjector plugin that controls texture projection
00020    Author: Jared Duke
00021    Date: 17 Jun 2010
00022  */
00023 
00024 #include <algorithm>
00025 #include <assert.h>
00026 #include <utility>
00027 #include <sstream>
00028 
00029 #include <gazebo/rendering/RenderingIface.hh>
00030 #include <gazebo/rendering/Scene.hh>
00031 #include <gazebo/rendering/Visual.hh>
00032 #include <gazebo/rendering/RTShaderSystem.hh>
00033 #include <gazebo_plugins/gazebo_ros_projector.h>
00034 
00035 #include <std_msgs/String.h>
00036 #include <std_msgs/Int32.h>
00037 
00038 #include <Ogre.h>
00039 #include <OgreMath.h>
00040 #include <OgreSceneNode.h>
00041 #include <OgreFrustum.h>
00042 #include <OgreSceneQuery.h>
00043 
00044 namespace gazebo
00045 {
00046 GZ_REGISTER_MODEL_PLUGIN(GazeboRosProjector);
00047 
00048 typedef std::map<std::string,Ogre::Pass*> OgrePassMap;
00049 typedef OgrePassMap::iterator OgrePassMapIterator;
00050 
00051 
00053 // Constructor
00054 GazeboRosProjector::GazeboRosProjector()
00055 {
00056   this->rosnode_ = NULL;
00057 }
00058 
00059 
00061 // Destructor
00062 GazeboRosProjector::~GazeboRosProjector()
00063 {
00064   // Custom Callback Queue
00065   this->queue_.clear();
00066   this->queue_.disable();
00067   this->rosnode_->shutdown();
00068   this->callback_queue_thread_.join();
00069 
00070   delete this->rosnode_;
00071 }
00072 
00074 // Load the controller
00075 void GazeboRosProjector::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00076 {
00077   this->world_ = _parent->GetWorld();
00078 
00079 
00080 
00081 
00082   // Create a new transport node for talking to the projector
00083   this->node_.reset(new transport::Node());
00084   // Initialize the node with the world name
00085   this->node_->Init(this->world_->GetName());
00086   // Setting projector topic
00087   std::string name = std::string("~/") + _parent->GetName() + "/" +
00088                       _sdf->Get<std::string>("projector");
00089   // Create a publisher on the ~/physics topic
00090   this->projector_pub_ = node_->Advertise<msgs::Projector>(name);
00091 
00092 
00093 
00094   // load parameters
00095   this->robot_namespace_ = "";
00096   if (_sdf->HasElement("robotNamespace"))
00097     this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
00098 
00099   this->texture_topic_name_ = "";
00100   if (_sdf->HasElement("textureTopicName"))
00101     this->texture_topic_name_ = _sdf->GetElement("textureTopicName")->Get<std::string>();
00102 
00103   this->projector_topic_name_ = "";
00104   if (_sdf->HasElement("projectorTopicName"))
00105     this->projector_topic_name_ = _sdf->GetElement("projectorTopicName")->Get<std::string>();
00106 
00107   // Make sure the ROS node for Gazebo has already been initialized
00108   if (!ros::isInitialized())
00109   {
00110     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00111       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00112     return;
00113   }
00114 
00115 
00116   this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
00117 
00118 
00119   // Custom Callback Queue
00120   ros::SubscribeOptions so = ros::SubscribeOptions::create<std_msgs::Int32>(
00121     this->projector_topic_name_,1,
00122     boost::bind( &GazeboRosProjector::ToggleProjector,this,_1),
00123     ros::VoidPtr(), &this->queue_);
00124   this->projectorSubscriber_ = this->rosnode_->subscribe(so);
00125 
00126   ros::SubscribeOptions so2 = ros::SubscribeOptions::create<std_msgs::String>(
00127     this->texture_topic_name_,1,
00128     boost::bind( &GazeboRosProjector::LoadImage,this,_1),
00129     ros::VoidPtr(), &this->queue_);
00130   this->imageSubscriber_ = this->rosnode_->subscribe(so2);
00131 
00132 
00133   // Custom Callback Queue
00134   this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosProjector::QueueThread,this ) );
00135 
00136 }
00137 
00138 
00140 // Load a texture into the projector
00141 void GazeboRosProjector::LoadImage(const std_msgs::String::ConstPtr& imageMsg)
00142 {
00143   msgs::Projector msg;
00144   msg.set_name("texture_projector");
00145   msg.set_texture(imageMsg->data);
00146   this->projector_pub_->Publish(msg);
00147 }
00148 
00150 // Toggle the activation of the projector
00151 void GazeboRosProjector::ToggleProjector(const std_msgs::Int32::ConstPtr& projectorMsg)
00152 {
00153   msgs::Projector msg;
00154   msg.set_name("texture_projector");
00155   msg.set_enabled(projectorMsg->data);
00156   this->projector_pub_->Publish(msg);
00157 }
00158 
00159 
00161 // Custom callback queue thread
00162 void GazeboRosProjector::QueueThread()
00163 {
00164   static const double timeout = 0.01;
00165 
00166   while (this->rosnode_->ok())
00167   {
00168     this->queue_.callAvailable(ros::WallDuration(timeout));
00169   }
00170 }
00171 
00172 }


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