Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
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
00054 GazeboRosProjector::GazeboRosProjector()
00055 {
00056 this->rosnode_ = NULL;
00057 }
00058
00059
00061
00062 GazeboRosProjector::~GazeboRosProjector()
00063 {
00064
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
00075 void GazeboRosProjector::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00076 {
00077 this->world_ = _parent->GetWorld();
00078
00079
00080
00081
00082
00083 this->node_.reset(new transport::Node());
00084
00085 this->node_->Init(this->world_->GetName());
00086
00087 std::string name = std::string("~/") + _parent->GetName() + "/" +
00088 _sdf->Get<std::string>("projector");
00089
00090 this->projector_pub_ = node_->Advertise<msgs::Projector>(name);
00091
00092
00093
00094
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
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
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
00134 this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosProjector::QueueThread,this ) );
00135
00136 }
00137
00138
00140
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
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
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 }