ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
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