Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
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
00043 this->robotNamespace_ = "";
00044 if (_sdf->HasElement("robotNamespace"))
00045 {
00046 this->robotNamespace_ = _sdf->GetElement(
00047 "robotNamespace")->Get<std::string>() + "/";
00048 }
00049
00050
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
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 }