gazebo_rtt_plugin.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <gazebo_rtt_plugin/gazebo_rtt_plugin.h>
00030 #include <gazebo_rtt_plugin/gazebo_activity.h>
00031 
00032 #include <rtt/deployment/ComponentLoader.hpp>
00033 #include <rtt/os/main.h>
00034 
00035 #include <rtt/os/targets/gnulinux-config.h>
00036 #ifdef PLUGINS_ENABLE_MARSHALLING
00037   #include <rtt/marsh/Marshalling.hpp>
00038 #endif
00039 
00040 #ifndef ORO_ROS_PROTOCOL_ID
00041   #define ORO_ROS_PROTOCOL_ID 3
00042 #endif
00043 
00044 namespace gazebo
00045 {
00046 
00047 class RTTPlugin::RTTOSInitializer
00048 {
00049 public:
00050   RTTOSInitializer() {
00051     __os_init(0, 0);
00052     RTT::Logger::Instance()->setLogLevel(RTT::Logger::Info);
00053     RTT::os::TimeService::Instance()->enableSystemClock(false);
00054   }
00055 
00056   ~RTTOSInitializer() {
00057     __os_exit();
00058   }
00059 
00060   static boost::shared_ptr<RTTOSInitializer> Instance() {
00061     boost::shared_ptr<RTTOSInitializer> ptr;
00062     if (weak_ptr.expired()) {
00063       ptr.reset(new RTTOSInitializer());
00064       weak_ptr = ptr;
00065     } else {
00066       ptr = weak_ptr.lock();
00067     }
00068     return ptr;
00069   }
00070 
00071 private:
00072   static boost::weak_ptr<RTTPlugin::RTTOSInitializer> weak_ptr;
00073 };
00074 boost::weak_ptr<RTTPlugin::RTTOSInitializer> RTTPlugin::RTTOSInitializer::weak_ptr;
00075 
00076 RTTPlugin::RTTPlugin()
00077   : rttOsInitializer(RTTOSInitializer::Instance())
00078 {
00079 }
00080 
00081 RTTPlugin::~RTTPlugin()
00082 {
00083   if (taskContext) {
00084     taskContext->stop();
00085     taskContext->cleanup();
00086   }
00087 }
00088 
00089 void RTTPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
00090 {
00091   // Store the pointer to the model
00092   this->model = _parent;
00093 
00094   // Store the pointer to the world
00095   this->world = _parent->GetWorld();
00096 
00097   // Load RTT component packages and libraries
00098   RTT::ComponentLoader::Instance()->import("rtt_rosnode", "");
00099   if (_sdf->HasElement("import")) {
00100     RTT::ComponentLoader::Instance()->import(_sdf->GetValueString("import"), "");
00101   }
00102   if (_sdf->HasElement("library")) {
00103     RTT::ComponentLoader::Instance()->loadLibrary(_sdf->GetValueString("library"));
00104   }
00105 
00106   // Create an instance of the component
00107   std::string component = _sdf->GetValueString("component");
00108   std::string name = _sdf->GetValueString("name");
00109   if (name.empty()) name = component;
00110 
00111   taskContext.reset(RTT::ComponentLoader::Instance()->loadComponent(name, component));
00112   if (!taskContext) {
00113     gzerr << "Could not create a TaskContext for component " << component << std::endl;
00114     return;
00115   }
00116 
00117   // set Activity
00118   GazeboActivity *activity = new GazeboActivity(name);
00119   activity->Load(world, _sdf);
00120   taskContext->setActivity(activity);
00121 
00122   // load configuration settings from marshalling
00123 #ifdef PLUGINS_ENABLE_MARSHALLING
00124   if (_sdf->HasElement("configuration")) {
00125     taskContext->getProvider<RTT::Marshalling>("marshalling")->loadProperties(_sdf->GetValueString("configuration"));
00126   }
00127 #endif
00128 
00129   // set TaskContext's properties
00130   sdf::ElementPtr property = _sdf->GetElement("property");
00131   while(property) {
00132     std::string name = property->GetValueString("name");
00133     RTT::base::PropertyBase *prop = taskContext->getProperty(name);
00134     if (!prop) continue;
00135 
00136     if (prop->getType() == "string")
00137       RTT::Property<std::string>(prop).set(property->GetValueString());
00138     else if (prop->getType() == "double")
00139       RTT::Property<double>(prop).set(property->GetValueDouble());
00140     else if (prop->getType() == "float")
00141       RTT::Property<float>(prop).set(property->GetValueFloat());
00142     else if (prop->getType() == "int")
00143       RTT::Property<int>(prop).set(property->GetValueInt());
00144     else if (prop->getType() == "uint")
00145       RTT::Property<uint>(prop).set(property->GetValueUInt());
00146     else if (prop->getType() == "char")
00147       RTT::Property<char>(prop).set(property->GetValueChar());
00148     else if (prop->getType() == "bool")
00149       RTT::Property<bool>(prop).set(property->GetValueBool());
00150     else
00151       gzerr << "Property " << name << " has an unknown type. Will not use it." << std::endl;
00152 
00153     property = property->GetNextElement("property");
00154   }
00155 
00156   // configure the TaskContext
00157   if (!taskContext->configure()) {
00158     gzerr << "Failed to configure TaskContext " << taskContext->getName() << std::endl;
00159     return;
00160   }
00161 
00162   // create Streams
00163   sdf::ElementPtr port = _sdf->GetElement("port");
00164   while(port) {
00165     std::string name;
00166     RTT::ConnPolicy conn_policy;
00167     conn_policy.transport = ORO_ROS_PROTOCOL_ID;
00168 
00169     if (!port->HasAttribute("name")) continue;
00170     name = port->GetAttribute("name")->GetAsString();
00171     if (port->HasAttribute("topic"))
00172       conn_policy.name_id = port->GetAttribute("topic")->GetAsString();
00173     else
00174       conn_policy.name_id = name;
00175     if (port->HasAttribute("queue_size")) conn_policy.size = boost::lexical_cast<int>(port->GetAttribute("queue_size")->GetAsString());
00176     conn_policy.type = conn_policy.size > 1 ? RTT::ConnPolicy::BUFFER : RTT::ConnPolicy::DATA;
00177 
00178     RTT::base::PortInterface *port_interface = taskContext->getPort(name);
00179     if (port_interface) {
00180       port_interface->createStream(conn_policy);
00181 
00182     } else {
00183       gzwarn << "Component '" << taskContext->getName() << "' has no port named '" << name << "'" << std::endl;
00184     }
00185 
00186     port = port->GetNextElement("port");
00187   }
00188 
00189   // Listen to the update event. This event is broadcast every
00190   // simulation iteration.
00191   this->updateConnection = event::Events::ConnectWorldUpdateStart(
00192       boost::bind(&RTTPlugin::OnUpdate, this));
00193 }
00194 
00195 void RTTPlugin::Init()
00196 {
00197   // do nothing for now
00198 }
00199 
00200 // Called by the world update start event
00201 void RTTPlugin::OnUpdate()
00202 {
00203   if (!taskContext) return;
00204 
00205   // start TaskContext
00206   if (!taskContext->isRunning()) {
00207     if (!taskContext->start()) {
00208       gzerr << "Failed to start TaskContext " << taskContext->getName() << std::endl;
00209       return;
00210     }
00211   }
00212 
00213   taskContext->update();
00214 }
00215 
00216 void RTTPlugin::Reset()
00217 {
00218   if (!taskContext) return;
00219   taskContext->stop();
00220 }
00221 
00222 // Register this plugin with the simulator
00223 GZ_REGISTER_MODEL_PLUGIN(RTTPlugin)
00224 
00225 } // namespace gazebo
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


gazebo_rtt_plugin
Author(s): Johannes Meyer
autogenerated on Mon Jul 15 2013 16:54:25