ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/robots/foot-bot/control_interface/ci_footbot_range_and_bearing_sensor.h
Go to the documentation of this file.
00001 
00043 #ifndef CCI_FOOTBOT_RANGE_AND_BEARING_SENSOR_H
00044 #define CCI_FOOTBOT_RANGE_AND_BEARING_SENSOR_H
00045 
00046 /* To avoid dependency problems when including */
00047 namespace argos {
00048    class CCI_FootBotRangeAndBearingSensor;
00049 }
00050 
00051 #include <string>
00052 #include <vector>
00053 #include <iostream>
00054 
00055 #include <argos3/core/control_interface/ci_sensor.h>
00056 #include <argos3/core/utility/math/angles.h>
00057 
00058 namespace argos {
00059 
00071    struct SRABPacket {
00072 
00073       typedef UInt8 TData[10];
00074       typedef UInt16 TRawValues[12];
00075 
00076       /* The robot's id. */
00077       std::string RobotId;
00078 
00079       /* Distance in meters. */
00080       Real Range;
00081 
00082       /* Horizontal angle, in radians. */
00083       CRadians BearingHorizontal;
00084 
00085       /* Vertical angle, in radians. */
00086       CRadians BearingVertical;
00087 
00088       /* Datatype for the range and bearing payload. */
00089       TData Data;
00090 
00091       /* Raw values used for the calibration of the 3D bearing. Don't remove */
00092       TRawValues RawValues;
00093 
00094       /* The id of the packet. */
00095       UInt64 Id;
00096 
00097       /* Constructor. */
00098       SRABPacket() :
00099          Range(0.0),
00100          Id(0) {
00101       }
00102 
00103       SRABPacket(const SRABPacket& t_packet) :
00104          RobotId(t_packet.RobotId), Range(t_packet.Range),
00105          BearingHorizontal(t_packet.BearingHorizontal),
00106          BearingVertical(t_packet.BearingVertical), Id(t_packet.Id) {
00107          ::memcpy(Data, &t_packet.Data, sizeof(TRangeAndBearingData));
00108          ::memcpy(RawValues, &t_packet.RawValues, sizeof(TRawValues));
00109       }
00110 
00111       SRABPacket& operator=(const SRABPacket& t_packet) {
00112          if (&t_packet != this) {
00113             RobotId = t_packet.RobotId;
00114             Range = t_packet.Range;
00115             BearingHorizontal = t_packet.BearingHorizontal;
00116             BearingVertical = t_packet.BearingVertical;
00117             Id = t_packet.Id;
00118             ::memcpy(Data, &t_packet.Data, sizeof(TRangeAndBearingData));
00119             ::memcpy(RawValues, &t_packet.RawValues, sizeof(TRawValues));
00120          }
00121          return *this;
00122       }
00123 
00124       friend std::ostream& operator<<(std::ostream& os,
00125                                       const SRABPacket& t_packet) {
00126          os << "RANGE_AND_BEARING_RECEIVED_DATA < id = " << t_packet.RobotId
00127             << ", range = " << t_packet.Range
00128             << ", bearing horizontal = " << t_packet.BearingHorizontal
00129             << ", bearing vertical = " << t_packet.BearingVertical
00130             << ", data = ["
00131             << t_packet.Data[0] << ":"
00132             << t_packet.Data[1] << ":"
00133             << t_packet.Data[2] << ":"
00134             << t_packet.Data[3] << ":"
00135             << t_packet.Data[4] << ":"
00136             << t_packet.Data[5] << ":"
00137             << t_packet.Data[6] << ":"
00138             << t_packet.Data[7] << ":"
00139             << t_packet.Data[8] << ":"
00140             << t_packet.Data[9]
00141             << "], id = " << t_packet.Id
00142             << " >";
00143 
00144          return os;
00145       }
00146    };
00147 
00148    class CCI_FootBotRangeAndBearingSensor : public CCI_Sensor {
00149 
00150    public:
00151 
00152       /* Type for the map that holds the latest received packet for each robot. */
00153       typedef std::map<std::string, SRABPacket> TLastReceivedPackets;
00154 
00155    public:
00156 
00157       CCI_FootBotRangeAndBearingSensor() :
00158          m_unLatestPacketId(0) {
00159       }
00160 
00161       virtual ~CCI_FootBotRangeAndBearingSensor() {}
00162 
00163       /* Clears the messages received from the range and bearing. Call this at the end of your
00164        * time-step to be sure that at each time-step you only have the most recently received packets*/
00165       inline void ClearRABReceivedPackets() {
00166          m_tLastReceivedPackets.clear();
00167       }
00168 
00169       /* Get the latest packet for all robots. Might contain empty packets. */
00170       inline const TLastReceivedPackets& GetLastReceivedPackets() const {
00171          return m_tLastReceivedPackets;
00172       }
00173 
00174       /* Get the id of the packet received most recently. */
00175       inline UInt64 GetLatestPacketId() const {
00176          return m_unLatestPacketId;
00177       }
00178 
00179    protected:
00180 
00181       /* Stores the last received packet from each robot. */
00182       TLastReceivedPackets m_tLastReceivedPackets;
00183 
00184       /* Id of the latest received packet. Can be used to get the freshly arrived packages only. */
00185       UInt64 m_unLatestPacketId;
00186 
00187    };
00188 
00189 }
00190 
00191 #endif