|
| 1 | +#include <memory> |
| 2 | +#include <rtt/typelib/TypelibMarshallerBase.hpp> |
| 3 | +#include <orocos_cpp/CorbaNameService.hpp> |
| 4 | +#include <stdlib.h> |
| 5 | + |
| 6 | +#include <orocos_cpp_base/OrocosHelpers.hpp> |
| 7 | +#include "orocos_cpp.hpp" |
| 8 | +#include "NameService.hpp" |
| 9 | +#include <lib_config/Configuration.hpp> |
| 10 | +#include <lib_config/TypelibConfiguration.hpp> |
| 11 | + |
| 12 | +using namespace orocos_cpp; |
| 13 | + |
| 14 | + |
| 15 | +RTT::base::InputPortInterface* createInputPort(RTT::TaskContext* task_context, RTT::types::TypeInfo const * type, ::std::string const & port_name) { |
| 16 | + std::string name = port_name + "_input"; |
| 17 | + RTT::base::PortInterface *pi = task_context->getPort(port_name); |
| 18 | + if (pi) { |
| 19 | + // Port exists. Returns success. |
| 20 | + RTT::log(RTT::Info) << "Port " << port_name << " is already registered." << RTT::endlog(); |
| 21 | + return 0; |
| 22 | + } |
| 23 | + /* Create port */ |
| 24 | + RTT::base::InputPortInterface *new_port = type->inputPort(name); |
| 25 | + if (!new_port) { |
| 26 | + RTT::log(RTT::Error) << "An error occurred during port generation." << RTT::endlog(); |
| 27 | + return NULL; |
| 28 | + } |
| 29 | + task_context->ports()->addEventPort(new_port->getName(), *(new_port) ); |
| 30 | + return new_port; |
| 31 | +} |
| 32 | + |
| 33 | +bool is_number(const std::string& s) |
| 34 | +{ |
| 35 | + return !s.empty() && std::find_if(s.begin(), |
| 36 | + s.end(), [](unsigned char c) { return !std::isdigit(c); }) == s.end(); |
| 37 | +} |
| 38 | + |
| 39 | + |
| 40 | +std::shared_ptr<libConfig::ConfigValue> extract_field(const std::shared_ptr<libConfig::ConfigValue> data, const std::vector<std::string>& path) |
| 41 | +{ |
| 42 | + //No path specified --> return whole type |
| 43 | + if(path.size() == 0) |
| 44 | + return data; |
| 45 | + |
| 46 | + std::shared_ptr<libConfig::ConfigValue> parent = data; |
| 47 | + std::string prev_field = ""; |
| 48 | + for(const std::string& field : path){ |
| 49 | + if(field != ""){ |
| 50 | + if(is_number(field)){ |
| 51 | + std::shared_ptr<libConfig::ArrayConfigValue> aparent = std::dynamic_pointer_cast<libConfig::ArrayConfigValue>(parent); |
| 52 | + if(!aparent) |
| 53 | + throw std::runtime_error("Tried to access element index "+field+" of data "+ parent->getCxxTypeName()+", but data is not an array."); |
| 54 | + |
| 55 | + int idx = std::stoi(field); |
| 56 | + if(idx < 0){ |
| 57 | + idx = aparent->getValues().size()-idx; |
| 58 | + } |
| 59 | + |
| 60 | + if((int) aparent->getValues().size() <= idx){ |
| 61 | + std::cout << "WARNING: Trying to access index "+std::to_string(idx)+" of field "+prev_field+", but the field has only the size of "+std::to_string(aparent->getValues().size()); |
| 62 | + throw std::runtime_error("Invalid field access"); |
| 63 | + } |
| 64 | + parent = aparent->getValues().at(idx); |
| 65 | + |
| 66 | + }else{ |
| 67 | + std::shared_ptr<libConfig::ComplexConfigValue> cparent = std::dynamic_pointer_cast<libConfig::ComplexConfigValue>(parent); |
| 68 | + if(!cparent) |
| 69 | + throw std::runtime_error("Tried to access field "+field+" of data "+ parent->getCxxTypeName()+", but data is not a complex structure."); |
| 70 | + parent = cparent->getValues().at(field); |
| 71 | + } |
| 72 | + } |
| 73 | + prev_field = field; |
| 74 | + } |
| 75 | + return parent; |
| 76 | +} |
| 77 | + |
| 78 | + |
| 79 | + |
| 80 | +int main(int argc, char **argv) |
| 81 | +{ |
| 82 | + |
| 83 | + std::unique_ptr<orocos_cpp::NameService> ns = std::make_unique<orocos_cpp::CorbaNameService>(); |
| 84 | + |
| 85 | + if (!ns->connect()) { |
| 86 | + std::cout << "Could not connect to Nameserver " << std::endl; |
| 87 | + return 0; |
| 88 | + } |
| 89 | + |
| 90 | + orocos_cpp::OrocosCppConfig config; |
| 91 | + config.load_all_packages = false; |
| 92 | + config.load_typekits = true; |
| 93 | + config.package_initialization_whitelist = {"base", "base-types"}; |
| 94 | + std::shared_ptr<orocos_cpp::OrocosCpp> orocos = std::make_shared<orocos_cpp::OrocosCpp>(); |
| 95 | + orocos->initialize(config, false); |
| 96 | + |
| 97 | + RTT::TaskContext* localtask = new RTT::TaskContext("dummy"); |
| 98 | + RTT::DataFlowInterface df; |
| 99 | + |
| 100 | + std::map<std::string, RTT::base::InputPortInterface*> input_ports; |
| 101 | + |
| 102 | + std::vector<std::string> tasks = ns->getRegisteredTasks(); |
| 103 | + for (const std::string &tname : tasks) { |
| 104 | + RTT::TaskContext* tc = ns->getTaskContext(tname); |
| 105 | + if (tc) { |
| 106 | + for (auto& portname : tc->ports()->getPortNames()) { |
| 107 | + RTT::base::OutputPortInterface* portInterfacePtr = dynamic_cast<RTT::base::OutputPortInterface*>(tc->getPort(portname)); |
| 108 | + if (portInterfacePtr) { |
| 109 | + std::string type = portInterfacePtr->getTypeInfo()->getTypeName(); |
| 110 | + // if (type == "/base/samples/Joints") { |
| 111 | + // create local port the "/" is the identifier for subgroups |
| 112 | + std::string identifier = tname+"/"+portname; |
| 113 | + RTT::base::InputPortInterface* input_port = dynamic_cast<RTT::base::InputPortInterface *>(portInterfacePtr->antiClone()); |
| 114 | + df.addPort(identifier+"-reader", *input_port); |
| 115 | + input_ports[identifier] = input_port; |
| 116 | + // connect all ports |
| 117 | + portInterfacePtr->connectTo(input_port, RTT::ConnPolicy::data()); |
| 118 | + // } |
| 119 | + } |
| 120 | + } |
| 121 | + } |
| 122 | + } |
| 123 | + |
| 124 | + //while (true) { |
| 125 | + for (const auto& port : input_ports) { |
| 126 | + if (port.second != nullptr) { |
| 127 | + std::string identifier = port.first; |
| 128 | + |
| 129 | + const RTT::types::TypeInfo* typeinfo = port.second->getTypeInfo(); |
| 130 | + orogen_transports::TypelibMarshallerBase * transport = dynamic_cast<orogen_transports::TypelibMarshallerBase *>(typeinfo->getProtocol(orogen_transports::TYPELIB_MARSHALLER_ID)); |
| 131 | + orogen_transports::TypelibMarshallerBase::Handle* transportHandle = transport->createSample(); |
| 132 | + const Typelib::Registry& registry = transport->getRegistry(); |
| 133 | + const Typelib::Type *typeptr = registry.get(transport->getMarshallingType()); |
| 134 | + RTT::base::DataSourceBase::shared_ptr datasource = transport->getDataSource(transportHandle); |
| 135 | + |
| 136 | + while (port.second->read(datasource) == RTT::NewData) { |
| 137 | + // convert the native, Orocos sample back to its marshallable and inspectable Typelib form |
| 138 | + transport->refreshTypelibSample(transportHandle); |
| 139 | + Typelib::Value val(transport->getTypelibSample(transportHandle), *(typeptr)); |
| 140 | + |
| 141 | + //orocos_cpp::TypeWrapper wrap(val); |
| 142 | + libConfig::TypelibConfiguration tlconf; |
| 143 | + std::shared_ptr<libConfig::ConfigValue> sample; |
| 144 | + std::shared_ptr<libConfig::ConfigValue> field; |
| 145 | + sample = tlconf.getFromValue(val); |
| 146 | + |
| 147 | + //wrap.printType(); |
| 148 | + sample->print(std::cout); |
| 149 | + |
| 150 | + if (sample->getName() == "/base/samples/joints") { |
| 151 | + std::cout << "printing subtype" << std::endl; |
| 152 | + field = extract_field(sample, {"elements", 0}); |
| 153 | + field->print(std::cout); |
| 154 | + |
| 155 | + } |
| 156 | + |
| 157 | + |
| 158 | + // example to get all numeric values |
| 159 | + std::shared_ptr<libConfig::ConfigValue> wrapptr = tlconf.getFromValue(val); |
| 160 | + std::list<std::shared_ptr<libConfig::ConfigValue>> typelist; |
| 161 | + std::list<std::string> namelist; |
| 162 | + typelist.push_back(wrapptr); |
| 163 | + namelist.push_back(identifier); |
| 164 | + while (typelist.size()) { |
| 165 | + std::shared_ptr<libConfig::ConfigValue> current = typelist.front(); |
| 166 | + std::string name = namelist.front(); |
| 167 | + typelist.pop_front(); |
| 168 | + if (current->getType() == libConfig::ConfigValue::Type::SIMPLE) { |
| 169 | + // set value |
| 170 | + std::shared_ptr<libConfig::SimpleConfigValue> val = std::dynamic_pointer_cast<libConfig::SimpleConfigValue>(current); |
| 171 | + double data = atof(val->getValue().c_str()); |
| 172 | + std::cout << name << ": " << data << std::endl; |
| 173 | + } else { |
| 174 | + // add other entries |
| 175 | + if (current->getType() == libConfig::ConfigValue::Type::ARRAY){ |
| 176 | + std::shared_ptr<libConfig::ArrayConfigValue> val = std::dynamic_pointer_cast<libConfig::ArrayConfigValue>(current); |
| 177 | + typelist.insert(typelist.end(), val->getValues().begin(), val->getValues().end()); |
| 178 | + } |
| 179 | + if (current->getType() == libConfig::ConfigValue::Type::COMPLEX){ |
| 180 | + std::shared_ptr<libConfig::ComplexConfigValue> val = std::dynamic_pointer_cast<libConfig::ComplexConfigValue>(current); |
| 181 | + for( const auto& [key, value] : val->getValues()){ |
| 182 | + typelist.push_back(value); |
| 183 | + std::string newname = name + "/" + key; |
| 184 | + namelist.push_back(newname); |
| 185 | + } |
| 186 | + } |
| 187 | + } |
| 188 | + } |
| 189 | + |
| 190 | + } |
| 191 | + } |
| 192 | + } |
| 193 | + //} |
| 194 | + |
| 195 | + |
| 196 | + return 0; |
| 197 | +} |
0 commit comments