gazebo_ros_elevator.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2015 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 */
18 
19 using namespace gazebo;
20 
22 
25 {
26 }
27 
30 {
31  this->queue_.clear();
32  this->queue_.disable();
33  this->rosnode_->shutdown();
34  this->callbackQueueThread_.join();
35 
36  delete this->rosnode_;
37 }
38 
40 void GazeboRosElevator::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
41 {
42  // load parameters
43  this->robotNamespace_ = "";
44  if (_sdf->HasElement("robotNamespace"))
45  {
46  this->robotNamespace_ = _sdf->GetElement(
47  "robotNamespace")->Get<std::string>() + "/";
48  }
49 
50  // Make sure the ROS node for Gazebo has already been initialized
51  if (!ros::isInitialized())
52  {
53  ROS_FATAL_STREAM_NAMED("elevator", "A ROS node for Gazebo has not been initialized,"
54  << "unable to load plugin. Load the Gazebo system plugin "
55  << "'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
56  return;
57  }
58 
59  std::string topic = "elevator";
60  if (_sdf->HasElement("topic"))
61  topic = _sdf->Get<std::string>("topic");
62 
63  ElevatorPlugin::Load(_parent, _sdf);
64 
65  this->rosnode_ = new ros::NodeHandle(this->robotNamespace_);
66 
68  ros::SubscribeOptions::create<std_msgs::String>(topic, 1,
69  boost::bind(&GazeboRosElevator::OnElevator, this, _1),
70  ros::VoidPtr(), &this->queue_);
71 
72  this->elevatorSub_ = this->rosnode_->subscribe(so);
73 
74  // start custom queue for elevator
75  this->callbackQueueThread_ =
76  boost::thread(boost::bind(&GazeboRosElevator::QueueThread, this));
77 }
78 
80 void GazeboRosElevator::OnElevator(const std_msgs::String::ConstPtr &_msg)
81 {
82  this->MoveToFloor(std::stoi(_msg->data));
83 }
84 
87 {
88  static const double timeout = 0.01;
89 
90  while (this->rosnode_->ok())
91  this->queue_.callAvailable(ros::WallDuration(timeout));
92 }
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
virtual ~GazeboRosElevator()
Destructor.
ROSCPP_DECL bool isInitialized()
ros::CallbackQueue queue_
Custom Callback Queue.
void OnElevator(const std_msgs::String::ConstPtr &_msg)
Receives messages on the elevator&#39;s topic.
ros::NodeHandle * rosnode_
ros node handle
ros::Subscriber elevatorSub_
Subscribes to a topic that controls the elevator.
GZ_REGISTER_MODEL_PLUGIN(GazeboRosF3D)
#define ROS_FATAL_STREAM_NAMED(name, args)
ROS implementation of the Elevator plugin.
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
std::string robotNamespace_
for setting ROS name space
bool ok() const
boost::shared_ptr< void > VoidPtr
void QueueThread()
Queu to handle callbacks.


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39