Merge pull request #297 from dic-iit/feature/contact_list_default_con…

…tact

Add the possibility to set the dafault contact in `ContactList` class

CHANGELOG.md

@@ -31,6 +31,7 @@ All notable changes to this project are documented in this file.
 - Implement `SO3Task` in `TSID` component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/281)
 - Implement clone method in ParametersHandler classes (https://github.com/dic-iit/bipedal-locomotion-framework/pull/288)
 - Implement `VariablesHandler::clear()` and `VariablesHandler::initialize()` (https://github.com/dic-iit/bipedal-locomotion-framework/pull/291)
+- Implement the possibility to set the default contact in the `ContactList` class (https://github.com/dic-iit/bipedal-locomotion-framework/pull/297)
 
 ### Changed
 - Move all the Contacts related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/204)

src/Contacts/CMakeLists.txt

@@ -10,7 +10,7 @@ if (FRAMEWORK_COMPILE_Contact)
  SUBDIRECTORIES tests/Contacts
  PUBLIC_HEADERS ${H_PREFIX}/Contact.h ${H_PREFIX}/ContactList.h ${H_PREFIX}/ContactPhase.h ${H_PREFIX}/ContactPhaseList.h
  SOURCES src/Contact.cpp src/ContactList.cpp src/ContactPhase.cpp src/ContactPhaseList.cpp
- PUBLIC_LINK_LIBRARIES MANIF::manif BipedalLocomotion::Math
+ PUBLIC_LINK_LIBRARIES MANIF::manif BipedalLocomotion::Math BipedalLocomotion::TextLogging
  INSTALLATION_FOLDER Contacts)
 
  set(H_PREFIX include/BipedalLocomotion/ContactDetectors)

src/Contacts/include/BipedalLocomotion/Contacts/ContactList.h

@@ -45,6 +45,7 @@ class ContactList
  contacts. **/
  std::string m_defaultName{"ContactList"}; /** Default name for the contact list. **/
  BipedalLocomotion::Contacts::ContactType m_defaultContactType{BipedalLocomotion::Contacts::ContactType::FULL}; /** Default contact type. **/
+ int m_defaultIndex{-1}; /**< Default Frame index of the contact */
 
 public:
  using const_iterator
@@ -70,6 +71,18 @@ class ContactList
  */
  void setDefaultContactType(const BipedalLocomotion::Contacts::ContactType& type);
 
+ /**
+ * @brief Set the default frame index of the contact
+ * @param defaultIndex the default index.
+ */
+ void setDefaultIndex(int defaultIndex);
+
+ /**
+ * @brief Get the default frame index of the contact
+ * @return the default index.
+ */
+ int defaultIndex() const;
+
  /**
  * @brief Get the default contact type.
  * @return the default contact type.

src/Contacts/src/ContactList.cpp

@@ -6,7 +6,8 @@
  */
 
 #include <BipedalLocomotion/Contacts/ContactList.h>
-#include <iostream>
+#include <BipedalLocomotion/TextLogging/Logger.h>
+
 #include <iterator>
 #include <cassert>
 
@@ -37,24 +38,41 @@ const ContactType &ContactList::defaultContactType() const
  return m_defaultContactType;
 }
 
+void ContactList::setDefaultIndex(int defaultIndex)
+{
+ m_defaultIndex = defaultIndex;
+}
+
+int ContactList::defaultIndex() const
+{
+ return m_defaultIndex;
+}
+
 bool ContactList::addContact(const PlannedContact &newContact)
 {
+ constexpr auto errorPrefix = "[ContactList::addContact]";
+
  if (newContact.activationTime > newContact.deactivationTime)
  {
- std::cerr << "[ContactList::addContact] The activation time cannot be greater than the deactivation time." <<std::endl;
+ log()->error("{} The activation time cannot be greater than the deactivation time.",
+ errorPrefix);
  return false;
  }
  using iterator = std::set<PlannedContact, ContactCompare>::iterator;
  std::pair<iterator,bool> res = m_contacts.insert(newContact);
  if (!res.second)
  {
- std::cerr << "[ContactList::addContact] Failed to insert new element.";
+ log()->error("{} Failed to insert new element.", errorPrefix);
  if (res.first != end())
  {
- std::cerr << " The new contact (activationTime: " << newContact.activationTime << " ";
- std::cerr << "deactivationTime: " << newContact.deactivationTime << ") is not compatible";
- std::cerr << " with an element already present in the list (activationTime: " << res.first->activationTime << " ";
- std::cerr << "deactivationTime: " << res.first->deactivationTime << ")" << std::endl;
+ log()->error("{} The new contact (activationTime: {} deactivationTime: {}) is not "
+ "compatible with an element already present in the list (activationTime: "
+ "{} deactivationTime: {}).",
+ errorPrefix,
+ newContact.activationTime,
+ newContact.deactivationTime,
+ res.first->activationTime,
+ res.first->deactivationTime);
  }
  return false;
  }
@@ -68,6 +86,7 @@ bool ContactList::addContact(const manif::SE3d &newTransform, double activationT
  newContact.activationTime = activationTime;
  newContact.deactivationTime = deactivationTime;
  newContact.name = m_defaultName;
+ newContact.index = m_defaultIndex;
  newContact.type = m_defaultContactType;
 
  return addContact(newContact);
@@ -155,7 +174,7 @@ bool ContactList::editContact(ContactList::const_iterator element, const Planned
 {
  if (element == end())
  {
- std::cerr << "[ContactList::addContact] The element is not valid." << std::endl;
+ log()->error("[ContactList::addContact] The element is not valid.");
  return false;
  }
 
@@ -167,7 +186,8 @@ bool ContactList::editContact(ContactList::const_iterator element, const Planned
  --previousElement;
  if (newContact.activationTime < previousElement->deactivationTime)
  {
- std::cerr << "[ContactList::addContact] The new contact cannot have an activation time smaller than the previous contact." << std::endl;
+ log()->error("[ContactList::addContact] The new contact cannot have an activation time "
+ "smaller than the previous contact.");
  return false;
  }
  }
@@ -176,7 +196,8 @@ bool ContactList::editContact(ContactList::const_iterator element, const Planned
  {
  if (newContact.deactivationTime > nextElement->activationTime)
  {
- std::cerr << "[ContactList::addContact] The new contact cannot have a deactivation time greater than the next contact." << std::endl;
+ log()->error("[ContactList::addContact] The new contact cannot have a deactivation "
+ "time greater than the next contact.");
  return false;
  }
  }
@@ -207,7 +228,8 @@ bool ContactList::keepOnlyPresentContact(double time)
 
  if (dropPoint == end())
  {
- std::cerr << "[ContactList::addContact] No contact has activation time lower than the specified time." << std::endl;
+ log()->error("[ContactList::addContact] No contact has activation time lower than the "
+ "specified time.");
  return false;
  }