gazebo_ros_harness.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2016 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/physics/World.hh>
00018 #include <gazebo/physics/Model.hh>
00019 #include <sdf/sdf.hh>
00020 
00021 #include "gazebo_plugins/gazebo_ros_harness.h"
00022 
00023 namespace gazebo
00024 {
00025 
00026 // Register this plugin with the simulator
00027 GZ_REGISTER_MODEL_PLUGIN(GazeboRosHarness)
00028 
00029 
00030 GazeboRosHarness::GazeboRosHarness()
00031 {
00032 }
00033 
00035 GazeboRosHarness::~GazeboRosHarness()
00036 {
00037   // Custom Callback Queue
00038   this->queue_.clear();
00039   this->queue_.disable();
00040 
00041   this->rosnode_->shutdown();
00042   delete this->rosnode_;
00043 }
00044 
00046 // Load the controller
00047 void GazeboRosHarness::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00048 {
00049   // Load the plugin
00050   HarnessPlugin::Load(_parent, _sdf);
00051 
00052   this->robotNamespace_ = "";
00053   if (_sdf->HasElement("robotNamespace"))
00054     this->robotNamespace_ = _sdf->Get<std::string>("robotNamespace") + "/";
00055 
00056   // Init ROS
00057   if (!ros::isInitialized())
00058   {
00059     ROS_FATAL_STREAM("Not loading plugin since ROS hasn't been "
00060           << "properly initialized.  Try starting gazebo with ros plugin:\n"
00061           << "  gazebo -s libgazebo_ros_api_plugin.so\n");
00062     return;
00063   }
00064 
00065   this->rosnode_ = new ros::NodeHandle(this->robotNamespace_);
00066 
00067   ros::SubscribeOptions so = ros::SubscribeOptions::create<std_msgs::Float32>(
00068       "/" + _parent->GetName() + "/harness/velocity", 1,
00069     boost::bind(&GazeboRosHarness::OnVelocity, this, _1),
00070     ros::VoidPtr(), &this->queue_);
00071   this->velocitySub_ = this->rosnode_->subscribe(so);
00072 
00073   so = ros::SubscribeOptions::create<std_msgs::Bool>(
00074     "/" + _parent->GetName() + "/harness/detach", 1,
00075     boost::bind(&GazeboRosHarness::OnDetach, this, _1),
00076     ros::VoidPtr(), &this->queue_);
00077   this->detachSub_ = this->rosnode_->subscribe(so);
00078 
00079   // Custom Callback Queue
00080   this->callbackQueueThread_ =
00081     boost::thread(boost::bind(&GazeboRosHarness::QueueThread, this));
00082 }
00083 
00085 void GazeboRosHarness::OnVelocity(const std_msgs::Float32::ConstPtr &msg)
00086 {
00087   // Set the target winch velocity
00088   this->SetWinchVelocity(msg->data);
00089 }
00090 
00092 void GazeboRosHarness::OnDetach(const std_msgs::Bool::ConstPtr &msg)
00093 {
00094   // Detach if true
00095   if (msg->data)
00096     this->Detach();
00097 }
00098 
00100 void GazeboRosHarness::QueueThread()
00101 {
00102   static const double timeout = 0.01;
00103 
00104   while (this->rosnode_->ok())
00105   {
00106     this->queue_.callAvailable(ros::WallDuration(timeout));
00107   }
00108 }
00109 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 18:41:09