Aliged Participants

Messages/NetworkIdMsg.cpp

@@ -0,0 +1,25 @@
+#include "NetworkIdMsg.h"
+
+namespace RoboidControl {
+
+NetworkIdMsg::NetworkIdMsg(const char* buffer) {
+  this->networkId = buffer[1];
+}
+
+NetworkIdMsg::NetworkIdMsg(unsigned char networkId) {
+  this->networkId = networkId;
+}
+
+NetworkIdMsg::~NetworkIdMsg() {}
+
+unsigned char NetworkIdMsg::Serialize(char* buffer) {
+#if defined(DEBUG)
+  std::cout << "Send NetworkIdMsg [" << (int)this->networkId << "] " << std::endl;
+#endif
+  unsigned char ix = 0;
+  buffer[ix++] = this->id;
+  buffer[ix++] = this->networkId;
+  return NetworkIdMsg::length;
+}
+
+}  // namespace RoboidControl

Messages/NetworkIdMsg.h

@@ -3,7 +3,7 @@
 namespace RoboidControl {
 
 /// @brief A message communicating the network ID for that participant
-class SiteMsg : public IMessage {
+class NetworkIdMsg : public IMessage {
 public:
   /// @brief The message ID
   static const unsigned char id = 0xA1;
@@ -14,11 +14,11 @@ public:
 
   /// @brief Create a new message for sending
   /// @param networkId The network ID for the participant
-  SiteMsg(unsigned char networkId);
+  NetworkIdMsg(unsigned char networkId);
   /// @copydoc RoboidControl::IMessage::IMessage(char*)
-  SiteMsg(const char *buffer);
+  NetworkIdMsg(const char *buffer);
   /// @brief Destructor for the message
-  virtual ~SiteMsg();
+  virtual ~NetworkIdMsg();
 
   /// @copydoc RoboidControl::IMessage::Serialize
   virtual unsigned char Serialize(char *buffer) override;

Messages/SiteMsg.cpp

@@ -1,25 +0,0 @@
-#include "SiteMsg.h"
-
-namespace RoboidControl {
-
-SiteMsg::SiteMsg(const char* buffer) {
-  this->networkId = buffer[1];
-}
-
-SiteMsg::SiteMsg(unsigned char networkId) {
-  this->networkId = networkId;
-}
-
-SiteMsg::~SiteMsg() {}
-
-unsigned char SiteMsg::Serialize(char* buffer) {
-#if defined(DEBUG)
-  std::cout << "Send SiteMsg [" << (int)this->networkId << "] " << std::endl;
-#endif
-  unsigned char ix = 0;
-  buffer[ix++] = this->id;
-  buffer[ix++] = this->networkId;
-  return SiteMsg::length;
-}
-
-}  // namespace RoboidControl

Participants/ParticipantUDP.cpp

@@ -325,8 +325,8 @@ void ParticipantUDP::ReceiveData(unsigned char bufferSize,
       Process(sender, msg);
       delete msg;
     } break;
-    case SiteMsg::id: {
+    case NetworkIdMsg::id: {
-      SiteMsg* msg = new SiteMsg(this->buffer);
+      NetworkIdMsg* msg = new NetworkIdMsg(this->buffer);
       bufferSize -= msg->length;
       Process(sender, msg);
       delete msg;
@@ -382,7 +382,7 @@ void ParticipantUDP::Process(Participant* sender, ParticipantMsg* msg) {
 #endif
 }
 
-void ParticipantUDP::Process(Participant* sender, SiteMsg* msg) {
+void ParticipantUDP::Process(Participant* sender, NetworkIdMsg* msg) {
 #if defined(DEBUG)
   std::cout << this->name << ": process SiteMsg " << (int)this->networkId
             << " -> " << (int)msg->networkId << "\n";
@@ -452,9 +452,9 @@ void ParticipantUDP::Process(Participant* sender, ModelUrlMsg* msg) {
 }
 
 void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
-#if defined(DEBUG)
+#if !defined(DEBUG)
   std::cout << this->name << ": process PoseMsg [" << (int)this->networkId
-            << "/" << (int)msg->networkId << "]\n";
+            << "/" << (int)msg->networkId << "] " << (int)msg->poseType << "\n";
 #endif
   Participant* owner = Participant::GetParticipant(msg->networkId);
   if (owner == nullptr)
@@ -466,7 +466,12 @@ void ParticipantUDP::Process(Participant* sender, PoseMsg* msg) {
 
   if ((msg->poseType & PoseMsg::Pose_Position) != 0)
     thing->SetPosition(msg->position);
-  std::cout << "update position for" << (int)thing->id << std::endl;
+  if ((msg->poseType & PoseMsg::Pose_Orientation) != 0)
+    thing->SetOrientation(msg->orientation);
+  if ((msg->poseType & PoseMsg::Pose_LinearVelocity) != 0)
+    thing->SetLinearVelocity(msg->linearVelocity);
+  if ((msg->poseType & PoseMsg::Pose_AngularVelocity) != 0)
+    thing->SetAngularVelocity(msg->angularVelocity);
 }
 
 void ParticipantUDP::Process(Participant* sender, BinaryMsg* msg) {

Participants/ParticipantUDP.h

@@ -1,13 +1,13 @@
 #pragma once
 
 #include "Messages/BinaryMsg.h"
-#include "Messages/InvestigateMsg.h"
 #include "Messages/DestroyMsg.h"
+#include "Messages/InvestigateMsg.h"
 #include "Messages/ModelUrlMsg.h"
 #include "Messages/NameMsg.h"
 #include "Messages/ParticipantMsg.h"
 #include "Messages/PoseMsg.h"
-#include "Messages/SiteMsg.h"
+#include "Messages/NetworkIdMsg.h"
 #include "Messages/ThingMsg.h"
 #include "Participant.h"
 
@@ -30,7 +30,8 @@ namespace RoboidControl {
 
 constexpr int MAX_SENDER_COUNT = 256;
 
-/// @brief A local participant is the local device which can communicate with
+/// @brief A participant using UDP communication
+/// A local participant is the local device which can communicate with
 /// other participants It manages all local things and communcation with other
 /// participants. Each application has a local participant which is usually
 /// explicit in the code. An participant can be isolated. In that case it is
@@ -42,6 +43,8 @@ constexpr int MAX_SENDER_COUNT = 256;
 /// RoboidControl::IsolatedParticipant::Isolated().
 /// @sa RoboidControl::Thing::Thing()
 class ParticipantUDP : public Participant {
+#pragma region Init
+
  public:
   /// @brief Create a participant without connecting to a site
   /// @param port The port on which the participant communicates
@@ -52,11 +55,8 @@ class ParticipantUDP : public Participant {
   /// @brief Create a participant which will try to connect to a site.
   /// @param ipAddress The IP address of the site
   /// @param port The port used by the site
-  ParticipantUDP(const char* ipAddress,
+  /// @param localPort The port used by the local participant
-                   int port = 7681,
+  ParticipantUDP(const char* ipAddress, int port = 7681, int localPort = 7681);
-                   int localPort = 7681);
-  // Note to self: one cannot specify the port used by the local participant
-  // now!!
 
   /// @brief Isolated participant is used when the application is run without
   /// networking
@@ -65,7 +65,14 @@ class ParticipantUDP : public Participant {
 
   /// @brief True if the participant is running isolated.
   /// Isolated participants do not communicate with other participants
+
+#pragma endregion Init
+
+  /// @brief True if the participant is running isolated.
+  /// Isolated participants do not communicate with other participants
   bool isIsolated = false;
+  /// @brief The remote site when this participant is connected to a site
+  Participant* remoteSite = nullptr;
 
   /// The interval in milliseconds for publishing (broadcasting) data on the
   /// local network
@@ -74,19 +81,10 @@ class ParticipantUDP : public Participant {
   /// @brief The name of the participant
   const char* name = "ParticipantUDP";
 
-  // int localPort = 0;
+ protected:
-
+  char buffer[1024];
-  /// @brief The remote site when this participant is connected to a site
-  Participant* remoteSite = nullptr;
-
-#if defined(ARDUINO)
-  // const char* remoteIpAddress = nullptr;
-  // unsigned short remotePort = 0;
-  // char* broadcastIpAddress = nullptr;
-
-  // WiFiUDP udp;
-#else
 
+#if !defined(ARDUINO)
 #if defined(__unix__) || defined(__APPLE__)
   int sock;
 #elif defined(_WIN32) || defined(_WIN64)
@@ -94,48 +92,48 @@ class ParticipantUDP : public Participant {
   sockaddr_in server_addr;
   sockaddr_in broadcast_addr;
 #endif
-
 #endif
-
+ public:
   void begin();
   bool connected = false;
 
+#pragma region Update
+
+ public:
   virtual void Update(unsigned long currentTimeMs = 0) override;
+
+ protected:
+  unsigned long nextPublishMe = 0;
+
   virtual void UpdateMyThings(unsigned long currentTimeMs);
   virtual void UpdateOtherThings(unsigned long currentTimeMs);
 
+#pragma endregion Update
+
+#pragma region Send
+
   void SendThingInfo(Participant* remoteParticipant, Thing* thing);
   void PublishThingInfo(Thing* thing);
 
   bool Send(Participant* remoteParticipant, IMessage* msg);
   bool Publish(IMessage* msg);
 
+#pragma endregion Send
+
+#pragma region Receive
+
+protected:
   void ReceiveData(unsigned char bufferSize,
                    char* senderIpAddress,
                    unsigned int senderPort);
   void ReceiveData(unsigned char bufferSize, Participant* remoteParticipant);
 
-// #if defined(NO_STD)
-//   unsigned char senderCount = 0;
-//   Participant* senders[MAX_SENDER_COUNT];
-// #else
-//   std::list<Participant*> senders;
-// #endif
-
- protected:
-  unsigned long nextPublishMe = 0;
-
-  char buffer[1024];
-
   void SetupUDP(int localPort, const char* remoteIpAddress, int remotePort);
 
-  //Participant* GetParticipant(const char* ipAddress, int port);
-  // Participant* AddParticipant(const char* ipAddress, int port);
-
   void ReceiveUDP();
 
   virtual void Process(Participant* sender, ParticipantMsg* msg);
-  virtual void Process(Participant* sender, SiteMsg* msg);
+  virtual void Process(Participant* sender, NetworkIdMsg* msg);
   virtual void Process(Participant* sender, InvestigateMsg* msg);
   virtual void Process(Participant* sender, ThingMsg* msg);
   virtual void Process(Participant* sender, NameMsg* msg);
@@ -143,34 +141,8 @@ class ParticipantUDP : public Participant {
   virtual void Process(Participant* sender, PoseMsg* msg);
   virtual void Process(Participant* sender, BinaryMsg* msg);
 
-#if !defined(NO_STD)
+#pragma endregion Receive
-//  public:
-//   using ThingConstructor = std::function<Thing*(Participant* participant,
-//                                                 unsigned char networkId,
-//                                                 unsigned char thingId)>;
 
-//   template <typename ThingClass>
-//   void Register(unsigned char thingType) {
-//     thingMsgProcessors[thingType] = [](Participant* participant,
-//                                        unsigned char networkId,
-//                                        unsigned char thingId) {
-//       return new ThingClass(participant, networkId, thingId);
-//     };
-//   };
-
-//   template <typename ThingClass>
-//   void Register2(unsigned char thingType, ThingConstructor f) {
-//     thingMsgProcessors[thingType] = [f](Participant* participant,
-//                                        unsigned char networkId,
-//                                        unsigned char thingId) {
-//       return f(participant, networkId, thingId);
-//     };
-//   };
-
-//  protected:
-//   std::unordered_map<unsigned char, ThingConstructor> thingMsgProcessors;
-
-#endif
 };
 
 }  // namespace RoboidControl

Participants/SiteServer.cpp

@@ -58,27 +58,26 @@ void SiteServer::Process(Participant* sender, ParticipantMsg* msg) {
     // std::cout << this->name << " received New Client -> " <<
     // sender->ipAddress
     //           << ":" << (int)sender->port << "\n";
-    SiteMsg* msg = new SiteMsg(sender->networkId);
+    NetworkIdMsg* msg = new NetworkIdMsg(sender->networkId);
     this->Send(sender, msg);
     delete msg;
   }
 }
 
-void SiteServer::Process(Participant* sender, SiteMsg* msg) {}
+void SiteServer::Process(Participant* sender, NetworkIdMsg* msg) {}
 
 void SiteServer::Process(Participant* sender, ThingMsg* msg) {
   Thing* thing = sender->Get(msg->thingId);
-  if (thing == nullptr) {
+  if (thing == nullptr)
     new Thing(sender, (Thing::Type)msg->thingType, msg->thingId);
 
-    if (msg->parentId != 0) {
+  if (msg->parentId != 0) {
-      thing->SetParent(Get(msg->parentId));
+    thing->SetParent(Get(msg->parentId));
-      if (thing->GetParent() != nullptr)
+    if (thing->GetParent() != nullptr)
-        std::cout << "Could not find parent [" << (int)msg->networkId << "/"
+      std::cout << "Could not find parent [" << (int)msg->networkId << "/"
-                  << (int)msg->parentId << "]\n";
+                << (int)msg->parentId << "]\n";
-    } else
+  } else
-      thing->SetParent(nullptr);
+    thing->SetParent(nullptr);
-  }
 }
 
 #pragma endregion Receive

Participants/SiteServer.h

@@ -12,17 +12,33 @@ namespace RoboidControl {
 
 /// @brief A participant is device which can communicate with other participants
 class SiteServer : public ParticipantUDP {
+  
+#pragma region Init
+
  public:
+  /// @brief Create a new site server
+  /// @param port The port of which to receive the messages
   SiteServer(int port = 7681);
 
+#pragma endregion Init
+
+#pragma region Update
+
   virtual void UpdateMyThings(unsigned long currentTimeMs) override;
 
+#pragma endregion Update
+
+#pragma region Receive
+
  protected:
   unsigned long nextPublishMe = 0;
 
   virtual void Process(Participant* sender, ParticipantMsg* msg) override;
-  virtual void Process(Participant* sender, SiteMsg* msg) override;
+  virtual void Process(Participant* sender, NetworkIdMsg* msg) override;
   virtual void Process(Participant* sender, ThingMsg* msg) override;
+
+#pragma endregion Receive
+
 };
 
 }  // namespace RoboidControl