gazebo_ros_elevator.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 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 #include "gazebo_plugins/gazebo_ros_elevator.h"
00018 
00019 using namespace gazebo;
00020 
00021 GZ_REGISTER_MODEL_PLUGIN(GazeboRosElevator);
00022 
00024 GazeboRosElevator::GazeboRosElevator()
00025 {
00026 }
00027 
00029 GazeboRosElevator::~GazeboRosElevator()
00030 {
00031   this->queue_.clear();
00032   this->queue_.disable();
00033   this->rosnode_->shutdown();
00034   this->callbackQueueThread_.join();
00035 
00036   delete this->rosnode_;
00037 }
00038 
00040 void GazeboRosElevator::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00041 {
00042   // load parameters
00043   this->robotNamespace_ = "";
00044   if (_sdf->HasElement("robotNamespace"))
00045   {
00046     this->robotNamespace_ = _sdf->GetElement(
00047         "robotNamespace")->Get<std::string>() + "/";
00048   }
00049 
00050   // Make sure the ROS node for Gazebo has already been initialized
00051   if (!ros::isInitialized())
00052   {
00053     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized,"
00054         << "unable to load plugin. Load the Gazebo system plugin "
00055         << "'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00056     return;
00057   }
00058 
00059   std::string topic = "elevator";
00060   if (_sdf->HasElement("topic"))
00061     topic = _sdf->Get<std::string>("topic");
00062 
00063   ElevatorPlugin::Load(_parent, _sdf);
00064 
00065   this->rosnode_ = new ros::NodeHandle(this->robotNamespace_);
00066 
00067   ros::SubscribeOptions so =
00068     ros::SubscribeOptions::create<std_msgs::String>(topic, 1,
00069         boost::bind(&GazeboRosElevator::OnElevator, this, _1),
00070         ros::VoidPtr(), &this->queue_);
00071 
00072   this->elevatorSub_ = this->rosnode_->subscribe(so);
00073 
00074   // start custom queue for elevator
00075   this->callbackQueueThread_ =
00076     boost::thread(boost::bind(&GazeboRosElevator::QueueThread, this));
00077 }
00078 
00080 void GazeboRosElevator::OnElevator(const std_msgs::String::ConstPtr &_msg)
00081 {
00082   this->MoveToFloor(std::stoi(_msg->data));
00083 }
00084 
00086 void GazeboRosElevator::QueueThread()
00087 {
00088   static const double timeout = 0.01;
00089 
00090   while (this->rosnode_->ok())
00091     this->queue_.callAvailable(ros::WallDuration(timeout));
00092 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:22