source: finroc_plugins_composite_ports/tInterfaceBase.cpp @ 10:475d7a686fd6

Last change on this file since 10:475d7a686fd6 was 10:475d7a686fd6, checked in by Max Reichardt <mreichardt@…>, 2 years ago

Adds support for inheritance among port composite interfaces (sub and superclass interfaces can be connected). Change also includes that connecting secondary backends (of port composite interfaces with multiple port types) will not connect the primary. This adds more flexibility/convenience w.r.t. connecting only parts of port composite interfaces.

File size: 19.7 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.operation_on_all_elements_pending)
392  {
393    return;
394  }
395
396  tBackend& other = static_cast<tBackend&>(partner);
397  bool connect_all_backends = this->IsPrimaryBackend() && other.IsPrimaryBackend();
398
399  primary_backend.operation_on_all_elements_pending = true;
400  try
401  {
402    if (primary_backend.custom_connect_function)
403    {
404      if (partner_is_destination)
405      {
406        primary_backend.custom_connect_function(*this, other);
407      }
408      else
409      {
410        primary_backend.custom_connect_function(other, *this);
411      }
412      primary_backend.operation_on_all_elements_pending = false;
413      return;
414    }
415
416    // Connect all matching backends and ports
417    for (auto this_backend : primary_backend.relation_backend_mapping)
418    {
419      for (auto other_backend : other.primary_backend.primary_backend.relation_backend_mapping)
420      {
421        if (this_backend.first - primary_backend.primary_relation_id == other_backend.first - other.primary_backend.primary_relation_id && (connect_all_backends || this_backend.second == this || other_backend.second == &other))
422        {
423          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;
424          this_backend.second->ConnectTo(*other_backend.second, direction);
425          this_backend.second->ConnectChildPortsByName(*other_backend.second, direction);
426        }
427      }
428    }
429  }
430  catch (const std::exception& e)
431  {
432    FINROC_LOG_PRINT(WARNING, "Connecting failed: ", e);
433  }
434  primary_backend.operation_on_all_elements_pending = false;
435}
436
437void tInterfaceBase::tBackend::OnDisconnect(tAbstractPort& partner, bool partner_is_destination)
438{
439  bool& pending = primary_backend.primary_backend.operation_on_all_elements_pending;
440  tBackend& other = static_cast<tBackend&>(partner).primary_backend;
441  if (pending || other.operation_on_all_elements_pending || partner_is_destination)
442  {
443    return;
444  }
445
446  pending = true;
447  try
448  {
449    // Update other backend connection info
450    for (auto this_backend : this->primary_backend.all_backends)
451    {
452      for (auto other_backend : other.primary_backend.all_backends)
453      {
454        this_backend->DisconnectFrom(*other_backend);
455      }
456    }
457
458    // Disconnect all ports to partner backends (this should also cover cases with custom connect function)
459    for (auto backend : primary_backend.all_backends)
460    {
461      for (auto port = backend->ChildPortsBegin(); port != backend->ChildPortsEnd(); ++port)
462      {
463        for (auto partner = port->IncomingConnectionsBegin(); partner != port->IncomingConnectionsEnd(); ++partner)
464        {
465          if (std::count(other.all_backends.begin(), other.all_backends.end(), partner->Source().GetParent()))
466          {
467            port->DisconnectFrom(partner->Source());
468          }
469        }
470        for (auto partner = port->OutgoingConnectionsBegin(); partner != port->OutgoingConnectionsEnd(); ++partner)
471        {
472          if (std::count(other.all_backends.begin(), other.all_backends.end(), partner->Destination().GetParent()))
473          {
474            port->DisconnectFrom(partner->Destination());
475          }
476        }
477      }
478    }
479  }
480  catch (const std::exception& e)
481  {
482    FINROC_LOG_PRINT(WARNING, "Disconnecting failed: ", e);
483  }
484  pending = false;
485}
486
487void tInterfaceBase::tBackend::OnInitialization()
488{
489  bool& pending = primary_backend.operation_on_all_elements_pending;
490  if (!pending)
491  {
492    pending = true;
493    for (auto backend : primary_backend.all_backends)
494    {
495      if (backend != this)
496      {
497        backend->Init();
498      }
499    }
500    pending = false;
501  }
502}
503
504void tInterfaceBase::tBackend::OnManagedDelete()
505{
506  rrlib::thread::tLock lock1(GetStructureMutex());
507
508  if (!core::tRuntimeEnvironment::ShuttingDown())
509  {
510    bool& pending = primary_backend.operation_on_all_elements_pending;
511    if (!pending)
512    {
513      pending = true;
514      for (auto backend : primary_backend.all_backends)
515      {
516        if (backend != this)
517        {
518          backend->ManagedDelete();
519        }
520      }
521      pending = false;
522    }
523  }
524
525  tAbstractPort::OnManagedDelete();
526}
527
528
529//----------------------------------------------------------------------
530// End of namespace declaration
531//----------------------------------------------------------------------
532}
533}
Note: See TracBrowser for help on using the repository browser.