Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
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
00092 this->model = _parent;
00093
00094
00095 this->world = _parent->GetWorld();
00096
00097
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
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
00118 GazeboActivity *activity = new GazeboActivity(name);
00119 activity->Load(world, _sdf);
00120 taskContext->setActivity(activity);
00121
00122
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
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
00157 if (!taskContext->configure()) {
00158 gzerr << "Failed to configure TaskContext " << taskContext->getName() << std::endl;
00159 return;
00160 }
00161
00162
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
00190
00191 this->updateConnection = event::Events::ConnectWorldUpdateStart(
00192 boost::bind(&RTTPlugin::OnUpdate, this));
00193 }
00194
00195 void RTTPlugin::Init()
00196 {
00197
00198 }
00199
00200
00201 void RTTPlugin::OnUpdate()
00202 {
00203 if (!taskContext) return;
00204
00205
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
00223 GZ_REGISTER_MODEL_PLUGIN(RTTPlugin)
00224
00225 }