gazebo_ros_projector.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: GazeboRosTextureProjector plugin that controls texture projection
20  Author: Jared Duke
21  Date: 17 Jun 2010
22  */
23 
24 #include <algorithm>
25 #include <assert.h>
26 #include <utility>
27 #include <sstream>
28 
29 #include <gazebo/rendering/RenderingIface.hh>
30 #include <gazebo/rendering/Scene.hh>
31 #include <gazebo/rendering/Visual.hh>
32 #include <gazebo/rendering/RTShaderSystem.hh>
34 
35 #include <std_msgs/String.h>
36 #include <std_msgs/Int32.h>
37 
38 #include <Ogre.h>
39 #include <OgreMath.h>
40 #include <OgreSceneNode.h>
41 #include <OgreFrustum.h>
42 #include <OgreSceneQuery.h>
43 
44 namespace gazebo
45 {
46 GZ_REGISTER_MODEL_PLUGIN(GazeboRosProjector);
47 
48 typedef std::map<std::string,Ogre::Pass*> OgrePassMap;
49 typedef OgrePassMap::iterator OgrePassMapIterator;
50 
51 
53 // Constructor
55 {
56  this->rosnode_ = NULL;
57 }
58 
59 
61 // Destructor
63 {
64  // Custom Callback Queue
65  this->queue_.clear();
66  this->queue_.disable();
67  this->rosnode_->shutdown();
68  this->callback_queue_thread_.join();
69 
70  delete this->rosnode_;
71 }
72 
74 // Load the controller
75 void GazeboRosProjector::Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf )
76 {
77  this->world_ = _parent->GetWorld();
78 
79 
80 
81 
82  // Create a new transport node for talking to the projector
83  this->node_.reset(new transport::Node());
84  // Initialize the node with the world name
85 #if GAZEBO_MAJOR_VERSION >= 8
86  this->node_->Init(this->world_->Name());
87 #else
88  this->node_->Init(this->world_->GetName());
89 #endif
90  // Setting projector topic
91  std::string name = std::string("~/") + _parent->GetName() + "/" +
92  _sdf->Get<std::string>("projector");
93  // Create a publisher on the ~/physics topic
94  this->projector_pub_ = node_->Advertise<msgs::Projector>(name);
95 
96 
97 
98  // load parameters
99  this->robot_namespace_ = "";
100  if (_sdf->HasElement("robotNamespace"))
101  this->robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
102 
103  this->texture_topic_name_ = "";
104  if (_sdf->HasElement("textureTopicName"))
105  this->texture_topic_name_ = _sdf->GetElement("textureTopicName")->Get<std::string>();
106 
107  this->projector_topic_name_ = "";
108  if (_sdf->HasElement("projectorTopicName"))
109  this->projector_topic_name_ = _sdf->GetElement("projectorTopicName")->Get<std::string>();
110 
111  // Make sure the ROS node for Gazebo has already been initialized
112  if (!ros::isInitialized())
113  {
114  ROS_FATAL_STREAM_NAMED("projector", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
115  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
116  return;
117  }
118 
119 
120  this->rosnode_ = new ros::NodeHandle(this->robot_namespace_);
121 
122 
123  // Custom Callback Queue
124  ros::SubscribeOptions so = ros::SubscribeOptions::create<std_msgs::Int32>(
125  this->projector_topic_name_,1,
126  boost::bind( &GazeboRosProjector::ToggleProjector,this,_1),
127  ros::VoidPtr(), &this->queue_);
128  this->projectorSubscriber_ = this->rosnode_->subscribe(so);
129 
130  ros::SubscribeOptions so2 = ros::SubscribeOptions::create<std_msgs::String>(
131  this->texture_topic_name_,1,
132  boost::bind( &GazeboRosProjector::LoadImage,this,_1),
133  ros::VoidPtr(), &this->queue_);
134  this->imageSubscriber_ = this->rosnode_->subscribe(so2);
135 
136 
137  // Custom Callback Queue
138  this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosProjector::QueueThread,this ) );
139 
140 }
141 
142 
144 // Load a texture into the projector
145 void GazeboRosProjector::LoadImage(const std_msgs::String::ConstPtr& imageMsg)
146 {
147  msgs::Projector msg;
148  msg.set_name("texture_projector");
149  msg.set_texture(imageMsg->data);
150  this->projector_pub_->Publish(msg);
151 }
152 
154 // Toggle the activation of the projector
155 void GazeboRosProjector::ToggleProjector(const std_msgs::Int32::ConstPtr& projectorMsg)
156 {
157  msgs::Projector msg;
158  msg.set_name("texture_projector");
159  msg.set_enabled(projectorMsg->data);
160  this->projector_pub_->Publish(msg);
161 }
162 
163 
165 // Custom callback queue thread
167 {
168  static const double timeout = 0.01;
169 
170  while (this->rosnode_->ok())
171  {
172  this->queue_.callAvailable(ros::WallDuration(timeout));
173  }
174 }
175 
176 }
msg
virtual ~GazeboRosProjector()
Destructor.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL bool isInitialized()
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
std::string robot_namespace_
For setting ROS name space.
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
#define ROS_FATAL_STREAM_NAMED(name, args)
std::string projector_topic_name_
ROS projector topic name.
physics::WorldPtr world_
pointer to the world
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
OgrePassMap::iterator OgrePassMapIterator
void ToggleProjector(const std_msgs::Int32::ConstPtr &projectorMsg)
Callbakc when a projector toggle is published.
transport::PublisherPtr projector_pub_
bool ok() const
boost::shared_ptr< void > VoidPtr
std::string texture_topic_name_
ROS texture topic name.
void LoadImage(const std_msgs::String::ConstPtr &imageMsg)
Callback when a texture is published.
std::map< std::string, Ogre::Pass * > OgrePassMap


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27