00001
00007 #include <argos3/core/simulator/entity/embodied_entity.h>
00008 #include <argos3/core/simulator/entity/composable_entity.h>
00009 #include <argos3/core/simulator/simulator.h>
00010
00011 #include "footbot_proximity_default_sensor.h"
00012
00013 namespace argos {
00014
00015
00016
00017
00018 class CProximitySensorImpl : public CProximityDefaultSensor {
00019
00020 public:
00021
00022 virtual Real CalculateReading(Real f_distance) {
00023 if(f_distance < 0.009889556) {
00024 return 1.0;
00025 }
00026 else {
00027 return 0.0100527 / (f_distance + 0.000163144);
00028 }
00029 }
00030
00031 };
00032
00033
00034
00035
00036 CFootBotProximityDefaultSensor::CFootBotProximityDefaultSensor() :
00037 m_pcProximityImpl(new CProximitySensorImpl()) {}
00038
00039
00040
00041
00042 CFootBotProximityDefaultSensor::~CFootBotProximityDefaultSensor() {
00043 delete m_pcProximityImpl;
00044 }
00045
00046
00047
00048
00049 void CFootBotProximityDefaultSensor::SetRobot(CComposableEntity& c_entity) {
00050 try {
00051 m_pcProximityImpl->SetRobot(c_entity);
00052 }
00053 catch(CARGoSException& ex) {
00054 THROW_ARGOSEXCEPTION_NESTED("Can't set robot for the foot-bot proximity default sensor", ex);
00055 }
00056 }
00057
00058
00059
00060
00061 void CFootBotProximityDefaultSensor::Init(TConfigurationNode& t_tree) {
00062 m_pcProximityImpl->Init(t_tree);
00063 }
00064
00065
00066
00067
00068 void CFootBotProximityDefaultSensor::Update() {
00069 m_pcProximityImpl->Update();
00070 for(size_t i = 0; i < m_pcProximityImpl->GetReadings().size(); ++i) {
00071 m_tReadings[i].Value = m_pcProximityImpl->GetReadings()[i];
00072 }
00073 }
00074
00075
00076
00077
00078 void CFootBotProximityDefaultSensor::Reset() {
00079 m_pcProximityImpl->Reset();
00080 }
00081
00082
00083
00084
00085 REGISTER_SENSOR(CFootBotProximityDefaultSensor,
00086 "footbot_proximity", "default",
00087 "Carlo Pinciroli [ilpincy@gmail.com]",
00088 "1.0",
00089 "The foot-bot proximity sensor.",
00090 "This sensor accesses the foot-bot proximity sensor. For a complete description\n"
00091 "of its usage, refer to the ci_footbot_proximity_sensor.h interface. For the XML\n"
00092 "configuration, refer to the default proximity sensor.\n",
00093 "Usable"
00094 );
00095
00096 }