source: finroc_plugins_composite_ports/tInterfaceBase.cpp @ 8:1cdb6fac7784

Last change on this file since 8:1cdb6fac7784 was 8:1cdb6fac7784, checked in by Max Reichardt <mreichardt@…>, 2 years ago

Makes port composite interface auto-create component interfaces that have not-yet been created (currently, this is a semi-pretty workaround that supports all components defined in finroc_plugins_structure - notably without a depedency to this plugin and also memory-efficient)

File size: 19.6 KB
Line 
1//
2// You received this file as part of Finroc
3// A framework for intelligent robot control
4//
5// Copyright (C) Finroc GbR (finroc.org)
6//
7// This program is free software; you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation; either version 2 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License along
18// with this program; if not, write to the Free Software Foundation, Inc.,
19// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
20//
21//----------------------------------------------------------------------
22/*!\file    plugins/composite_ports/tInterfaceBase.cpp
23 *
24 * \author  Max Reichardt
25 *
26 * \date    2020-01-14
27 *
28 */
29//----------------------------------------------------------------------
30#include "plugins/composite_ports/tInterfaceBase.h"
31
32//----------------------------------------------------------------------
33// External includes (system with <>, local with "")
34//----------------------------------------------------------------------
35#include "core/tFrameworkElementTags.h"
36#include "core/tRuntimeEnvironment.h"
37#include "rrlib/util/string.h"
38
39//----------------------------------------------------------------------
40// Internal includes with ""
41//----------------------------------------------------------------------
42#include "plugins/composite_ports/internal/tInterfaceTypeInfo.h"
43#include "plugins/composite_ports/internal/type_traits.h"
44
45//----------------------------------------------------------------------
46// Debugging
47//----------------------------------------------------------------------
48#include <cassert>
49
50//----------------------------------------------------------------------
51// Namespace usage
52//----------------------------------------------------------------------
53
54//----------------------------------------------------------------------
55// Namespace declaration
56//----------------------------------------------------------------------
57namespace finroc
58{
59namespace composite_ports
60{
61
62//----------------------------------------------------------------------
63// Forward declarations / typedefs / enums
64//----------------------------------------------------------------------
65typedef core::tFrameworkElementFlag tFlag;
66
67//----------------------------------------------------------------------
68// Const values
69//----------------------------------------------------------------------
70const std::string cINTERFACE_TAG_SINGLE_BACKEND_INSTANTIATION = "finroc.composite_ports.single_backend_instantiation";
71
72//----------------------------------------------------------------------
73// Implementation
74//----------------------------------------------------------------------
75
76namespace
77{
78
79const core::tFrameworkElement* GetAggregator(const core::tFrameworkElement& element, core::tFrameworkElementFlags* extend_with_default_ports_flags = nullptr)
80{
81  if (element.GetFlag(tFlag::EDGE_AGGREGATOR) && (!element.IsPort()))
82  {
83    if (element.GetFlag(tFlag::INTERFACE) && extend_with_default_ports_flags)
84    {
85      (*extend_with_default_ports_flags) |= static_cast<const core::tPortGroup&>(element).GetDefaultPortFlags();
86      if (element.GetFlag(tFlag::SENSOR_DATA))
87      {
88        (*extend_with_default_ports_flags) |= tFlag::SENSOR_DATA;
89      }
90      if (element.GetFlag(tFlag::CONTROLLER_DATA))
91      {
92        (*extend_with_default_ports_flags) |= tFlag::CONTROLLER_DATA;
93      }
94      if (element.GetFlag(tFlag::INTERFACE_FOR_INPUTS) || element.GetFlag(tFlag::PARAMETER_INTERFACE))
95      {
96        (*extend_with_default_ports_flags) |= data_ports::cDEFAULT_INPUT_PORT_FLAGS;
97      }
98      if (element.GetFlag(tFlag::INTERFACE_FOR_OUTPUTS))
99      {
100        (*extend_with_default_ports_flags) |= data_ports::cDEFAULT_OUTPUT_PORT_FLAGS;
101      }
102      if (element.GetFlag(tFlag::PROXY_INTERFACE))
103      {
104        (*extend_with_default_ports_flags) |= tFlag::EMITS_DATA | tFlag::ACCEPTS_DATA;
105      }
106    }
107    return &element;
108  }
109  return element.GetParent() ? GetAggregator(*element.GetParent(), extend_with_default_ports_flags) : nullptr;
110}
111
112core::tAbstractPortCreationInfo MakeCreationInfo(const rrlib::rtti::tType& interface_type, const std::string& name, core::tFrameworkElement& parent_interface)
113{
114  core::tAbstractPortCreationInfo result;
115  result.data_type = interface_type;
116  result.parent = &parent_interface;
117  result.flags |= tFlag::INTERFACE;
118  GetAggregator(parent_interface, &result.flags);
119  result.name = name;
120  assert(result.flags.Get(tFlag::EMITS_DATA) || result.flags.Get(tFlag::ACCEPTS_DATA));
121  return result;
122}
123
124//void ReplaceAny(std::string& s, const std::string& seach_string, const std::string& replace_with)  // TODO
125//{
126//  auto index = s.find(seach_string);
127//  while (index != std::string::npos)
128//  {
129//    s.replace(index, seach_string.length(), replace_with);
130//    index = s.find(seach_string);
131//  }
132//}
133
134}
135
136void tInterfaceBase::operator()(core::tAbstractPortCreationInfo& creation_info) const
137{
138  if ((!Backend()) || (Backend() && Backend()->convenience_port_type))
139  {
140    creation_info.flags |= tFlag::READY; // tComponent::cNO_PORT_NAME_BUILDER;
141  }
142  if (!Backend())
143  {
144    creation_info.flags |= tFlag::DELETED;
145    return;
146  }
147  creation_info.parent = &Backend()->primary_backend;
148  return;
149}
150
151
152tInterfaceBase::tBackend::tBackend(const rrlib::rtti::tType& interface_type, core::tFrameworkElement* parent_component, core::tFrameworkElement* parent, int primary_relation_id, const std::string& name, bool convenience_port_type, const tConnectFunction& custom_connect_function, tCreateMissingComponentInterfaceFunction create_missing_component_interface_function) :
153  tAbstractPort(MakeCreationInfo(interface_type, name, *parent)),
154  parent_component(parent_component),
155  primary_backend(*this),
156  custom_connect_function(custom_connect_function),
157  convenience_port_type(convenience_port_type),
158  primary_relation_id(primary_relation_id),
159  create_missing_component_interface_function(create_missing_component_interface_function)
160{
161  all_backends.emplace_back(this);
162  relation_backend_mapping.emplace_back(primary_relation_id, this);
163}
164
165tInterfaceBase::tBackend::tBackend(core::tAbstractPortCreationInfo& creation_info, tBackend& primary) :
166  tAbstractPort(creation_info),
167  primary_backend(primary),
168  convenience_port_type(false),
169  primary_relation_id(primary.primary_relation_id),
170  create_missing_component_interface_function(nullptr)
171{
172}
173
174void tInterfaceBase::tBackend::ConnectChildPortsByName(tBackend& destination, const core::tConnectOptions& connect_options)
175{
176  for (auto port = this->ChildPortsBegin(); port != this->ChildPortsEnd(); ++port)
177  {
178    auto other_port = destination.GetChild(port->GetName());
179    if (other_port && other_port->IsPort())
180    {
181      port->ConnectTo(static_cast<core::tAbstractPort&>(*other_port), connect_options);
182    }
183  }
184}
185
186tInterfaceBase::tBackend* tInterfaceBase::tBackend::GetBackend(int primary_relation_id)
187{
188  if (&primary_backend != this)
189  {
190    return primary_backend.GetBackend(primary_relation_id);
191  }
192
193  for (auto & entry : relation_backend_mapping)
194  {
195    if (entry.first == primary_relation_id)
196    {
197      return entry.second;
198    }
199  }
200
201  core::tFrameworkElement* parent = nullptr;
202  if (GetParent() && typeid(*GetParent()).name() == typeid(tBackend).name())
203  {
204    parent = static_cast<tBackend&>(*GetParent()).GetBackend(primary_relation_id);
205  }
206  else if (core::tFrameworkElementTags::IsTagged(*this->GetParent(), cINTERFACE_TAG_SINGLE_BACKEND_INSTANTIATION))
207  {
208    parent = this->GetParent();
209  }
210  else
211  {
212    // Actually process relation
213    std::vector<int> relation_list;
214    tFrameworkElement* component_interface = this->GetParent();
215    int relation_id = primary_relation_id;
216    if ((!component_interface) || (!internal::IsComponent(component_interface->GetParent())))
217    {
218      FINROC_LOG_PRINT(WARNING, "Generic creation of port composite interfaces with multiple port types only works on components");
219      throw std::runtime_error("Generic creation of port composite interfaces with multiple port types only works on components");
220    }
221
222    const core::tFrameworkElementFlags relevant_flags = tFlag::SENSOR_DATA | tFlag::CONTROLLER_DATA | tFlag::INTERFACE_FOR_INPUTS | tFlag::INTERFACE_FOR_OUTPUTS | tFlag::PROXY_INTERFACE | tFlag::PARAMETER_INTERFACE;
223
224    // Compute opcode sequence
225    {
226      while (relation_id)
227      {
228        int opcode = relation_id & 0xFF;
229        relation_list.push_back(opcode);
230        relation_id = relation_id >> 8;
231      }
232      std::reverse(relation_list.begin(), relation_list.end());
233    }
234
235    for (int opcode : relation_list)
236    {
237      core::tFrameworkElementFlags original_flags = component_interface->GetAllFlags();
238      core::tFrameworkElementFlags target_set_flags = core::tFrameworkElementFlags(original_flags.Raw() & relevant_flags.Raw());
239      core::tFrameworkElementFlags target_unset_flags = core::tFrameworkElementFlags((~original_flags.Raw()) & relevant_flags.Raw());
240
241      typedef internal::tPortTypeRelation tOpCode;
242      switch (opcode)
243      {
244      case tOpCode::ePTR_COUNTER_PART:
245      case tOpCode::ePTR_COUNTER_PART_PARAMETER_IF_INPUT:
246        if (original_flags.Get(tFlag::CONTROLLER_DATA) && (!original_flags.Get(tFlag::SENSOR_DATA)))
247        {
248          target_set_flags.Set(tFlag::SENSOR_DATA);
249          target_set_flags.Set(tFlag::CONTROLLER_DATA, false);
250          target_unset_flags.Set(tFlag::CONTROLLER_DATA);
251          target_unset_flags.Set(tFlag::SENSOR_DATA, false);
252        }
253        if (original_flags.Get(tFlag::SENSOR_DATA) && (!original_flags.Get(tFlag::CONTROLLER_DATA)))
254        {
255          target_set_flags.Set(tFlag::CONTROLLER_DATA);
256          target_set_flags.Set(tFlag::SENSOR_DATA, false);
257          target_unset_flags.Set(tFlag::SENSOR_DATA);
258          target_unset_flags.Set(tFlag::CONTROLLER_DATA, false);
259        }
260        if (original_flags.Get(tFlag::INTERFACE_FOR_INPUTS) && (!original_flags.Get(tFlag::INTERFACE_FOR_OUTPUTS)))
261        {
262          target_set_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS);
263          target_set_flags.Set(tFlag::INTERFACE_FOR_INPUTS, false);
264          target_set_flags.Set(tFlag::PARAMETER_INTERFACE, false);
265          target_unset_flags.Set(tFlag::INTERFACE_FOR_INPUTS);
266          target_unset_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS, false);
267        }
268        if (original_flags.Get(tFlag::INTERFACE_FOR_OUTPUTS) && (!original_flags.Get(tFlag::INTERFACE_FOR_INPUTS)))
269        {
270          if (opcode == tOpCode::ePTR_COUNTER_PART_PARAMETER_IF_INPUT)
271          {
272            target_set_flags = tFlag::PARAMETER_INTERFACE;
273            target_unset_flags = core::tFrameworkElementFlags();
274          }
275          else
276          {
277            target_set_flags.Set(tFlag::INTERFACE_FOR_INPUTS);
278            target_set_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS, false);
279            target_unset_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS);
280            target_unset_flags.Set(tFlag::INTERFACE_FOR_INPUTS, false);
281          }
282        }
283        break;
284      case internal::tPortTypeRelation::ePTR_INPUT:
285        target_set_flags.Set(tFlag::INTERFACE_FOR_INPUTS);
286        target_set_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS, false);
287        target_unset_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS);
288        target_unset_flags.Set(tFlag::INTERFACE_FOR_INPUTS, false);
289        break;
290      case internal::tPortTypeRelation::ePTR_OUTPUT:
291        target_set_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS);
292        target_set_flags.Set(tFlag::INTERFACE_FOR_INPUTS, false);
293        target_set_flags.Set(tFlag::PARAMETER_INTERFACE, false);
294        target_unset_flags.Set(tFlag::INTERFACE_FOR_INPUTS);
295        target_unset_flags.Set(tFlag::INTERFACE_FOR_OUTPUTS, false);
296        break;
297      case internal::tPortTypeRelation::ePTR_PARAMETER:
298        target_set_flags = tFlag::PARAMETER_INTERFACE;
299        target_unset_flags = core::tFrameworkElementFlags();
300        break;
301      default:
302        break;
303      }
304
305      core::tFrameworkElement* new_candidate = nullptr;
306      size_t candidate_name_length_difference = std::numeric_limits<size_t>::max();
307      auto component = component_interface->GetParent();
308      for (auto it = component->ChildrenBegin(); it != component->ChildrenEnd(); ++it)
309      {
310        if ((!it->IsPort()) && it->GetFlag(tFlag::INTERFACE) && (it->GetAllFlags().Raw() & target_set_flags.Raw()) == target_set_flags.Raw() && (it->GetAllFlags().Raw() & target_unset_flags.Raw()) == 0)
311        {
312          size_t name_length_difference = std::abs(static_cast<long>(component_interface->GetName().length()) - static_cast<long>(it->GetName().length()));
313          if (name_length_difference < candidate_name_length_difference)
314          {
315            new_candidate = &*it;
316            candidate_name_length_difference = name_length_difference;
317          }
318        }
319      }
320
321      // Not-yet-created component interfaces - until there is a generic way to do this
322      if ((!new_candidate) && create_missing_component_interface_function)
323      {
324        new_candidate = (*create_missing_component_interface_function)(component_interface->GetParent(), target_set_flags);
325      }
326
327      if (new_candidate)
328      {
329        component_interface = new_candidate;
330      }
331      else
332      {
333        std::stringstream message;
334        message << "Generic creation of port composite interface failed: No interface with relation " << make_builder::GetEnumString(static_cast<tOpCode>(opcode)) << " found below " << *component_interface->GetParent();
335        FINROC_LOG_PRINT(WARNING, message.str());
336        throw std::runtime_error(message.str());
337      }
338    }
339
340    parent = component_interface;
341  }
342
343  for (auto & entry : relation_backend_mapping)
344  {
345    if (entry.second->GetParent() == parent)
346    {
347      auto backend = entry.second;
348      relation_backend_mapping.emplace_back(primary_relation_id, backend);
349      return backend;
350    }
351  }
352
353  // Create new backend below this parent
354  core::tAbstractPortCreationInfo secondary_creation_info;
355  secondary_creation_info.data_type = this->GetDataType();
356  secondary_creation_info.name = GetName();
357  secondary_creation_info.parent = parent;
358  secondary_creation_info.flags = core::tFrameworkElementFlags(tFlag::INTERFACE);
359  GetAggregator(*parent, &secondary_creation_info.flags);
360  all_backends.push_back(new tBackend(secondary_creation_info, *this));
361  relation_backend_mapping.emplace_back(primary_relation_id, all_backends.back());
362  return all_backends.back();
363}
364
365core::tFrameworkElementFlags tInterfaceBase::tBackend::GetDefaultPortFlags(bool generic_port_backend) const
366{
367  core::tFrameworkElementFlags result;
368  if (generic_port_backend)
369  {
370    GetAggregator(*this, &result);
371  }
372  else
373  {
374    auto aggregator = GetAggregator(*this);
375    if (aggregator && aggregator->GetFlag(tFlag::INTERFACE))
376    {
377      if (aggregator->GetFlag(tFlag::PROXY_INTERFACE))
378      {
379        result |= (tFlag::ACCEPTS_DATA | tFlag::EMITS_DATA | tFlag::PUSH_STRATEGY);
380      }
381      result |= static_cast<const core::tPortGroup&>(*aggregator).GetDefaultPortFlags();
382    }
383    const int cRELEVANT_FLAGS = (tFlag::SHARED | tFlag::SENSOR_DATA_PORT | tFlag::CONTROLLER_DATA_PORT).Raw();
384    result |= core::tFrameworkElementFlags(this->GetAllFlags().Raw() & cRELEVANT_FLAGS);
385  }
386  return result;
387}
388
389void tInterfaceBase::tBackend::OnConnect(tAbstractPort& partner, bool partner_is_destination)
390{
391  if (&primary_backend != this)
392  {
393    primary_backend.OnConnect(partner, this->IsOutputPort() == primary_backend.IsOutputPort() ? partner_is_destination : (!partner_is_destination));
394    return;
395  }
396  if (this->operation_on_all_elements_pending)
397  {
398    return;
399  }
400
401  this->operation_on_all_elements_pending = true;
402  try
403  {
404    tBackend& other = static_cast<tBackend&>(partner).primary_backend;
405    if (this->custom_connect_function)
406    {
407      if (partner_is_destination)
408      {
409        custom_connect_function(*this, other);
410      }
411      else
412      {
413        custom_connect_function(other, *this);
414      }
415      this->operation_on_all_elements_pending = false;
416      return;
417    }
418
419    // Connect all matching backends and ports
420    for (auto this_backend : this->relation_backend_mapping)
421    {
422      for (auto other_backend : other.primary_backend.relation_backend_mapping)
423      {
424        if (this_backend.first - this->primary_relation_id == other_backend.first - other.primary_relation_id)
425        {
426          core::tConnectionFlag direction = (this->IsOutputPort() == this_backend.second->IsOutputPort() ? partner_is_destination : (!partner_is_destination)) ? core::tConnectionFlag::DIRECTION_TO_DESTINATION : core::tConnectionFlag::DIRECTION_TO_SOURCE;
427          this_backend.second->ConnectTo(*other_backend.second, direction);
428          this_backend.second->ConnectChildPortsByName(*other_backend.second, direction);
429        }
430      }
431    }
432  }
433  catch (const std::exception& e)
434  {
435    FINROC_LOG_PRINT(WARNING, "Connecting failed: ", e);
436  }
437  this->operation_on_all_elements_pending = false;
438}
439
440void tInterfaceBase::tBackend::OnDisconnect(tAbstractPort& partner, bool partner_is_destination)
441{
442  bool& pending = primary_backend.primary_backend.operation_on_all_elements_pending;
443  tBackend& other = static_cast<tBackend&>(partner).primary_backend;
444  if (pending || other.operation_on_all_elements_pending || partner_is_destination)
445  {
446    return;
447  }
448
449  pending = true;
450  try
451  {
452    // Update other backend connection info
453    for (auto this_backend : this->primary_backend.all_backends)
454    {
455      for (auto other_backend : other.primary_backend.all_backends)
456      {
457        this_backend->DisconnectFrom(*other_backend);
458      }
459    }
460
461    // Disconnect all ports to partner backends (this should also cover cases with custom connect function)
462    for (auto backend : primary_backend.all_backends)
463    {
464      for (auto port = backend->ChildPortsBegin(); port != backend->ChildPortsEnd(); ++port)
465      {
466        for (auto partner = port->IncomingConnectionsBegin(); partner != port->IncomingConnectionsEnd(); ++partner)
467        {
468          if (std::count(other.all_backends.begin(), other.all_backends.end(), partner->Source().GetParent()))
469          {
470            port->DisconnectFrom(partner->Source());
471          }
472        }
473        for (auto partner = port->OutgoingConnectionsBegin(); partner != port->OutgoingConnectionsEnd(); ++partner)
474        {
475          if (std::count(other.all_backends.begin(), other.all_backends.end(), partner->Destination().GetParent()))
476          {
477            port->DisconnectFrom(partner->Destination());
478          }
479        }
480      }
481    }
482  }
483  catch (const std::exception& e)
484  {
485    FINROC_LOG_PRINT(WARNING, "Disconnecting failed: ", e);
486  }
487  pending = false;
488}
489
490void tInterfaceBase::tBackend::OnInitialization()
491{
492  bool& pending = primary_backend.operation_on_all_elements_pending;
493  if (!pending)
494  {
495    pending = true;
496    for (auto backend : primary_backend.all_backends)
497    {
498      if (backend != this)
499      {
500        backend->Init();
501      }
502    }
503    pending = false;
504  }
505}
506
507void tInterfaceBase::tBackend::OnManagedDelete()
508{
509  rrlib::thread::tLock lock1(GetStructureMutex());
510
511  if (!core::tRuntimeEnvironment::ShuttingDown())
512  {
513    bool& pending = primary_backend.operation_on_all_elements_pending;
514    if (!pending)
515    {
516      pending = true;
517      for (auto backend : primary_backend.all_backends)
518      {
519        if (backend != this)
520        {
521          backend->ManagedDelete();
522        }
523      }
524      pending = false;
525    }
526  }
527
528  tAbstractPort::OnManagedDelete();
529}
530
531
532//----------------------------------------------------------------------
533// End of namespace declaration
534//----------------------------------------------------------------------
535}
536}
Note: See TracBrowser for help on using the repository browser.