ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/sensors/positioning_default_sensor.cpp
Go to the documentation of this file.
00001 
00007 #include <argos3/core/simulator/simulator.h>
00008 #include <argos3/core/simulator/entity/embodied_entity.h>
00009 #include <argos3/core/simulator/entity/composable_entity.h>
00010 
00011 #include "positioning_default_sensor.h"
00012 
00013 namespace argos {
00014 
00015    /****************************************/
00016    /****************************************/
00017 
00018    CPositioningDefaultSensor::CPositioningDefaultSensor() :
00019       m_pcEmbodiedEntity(NULL),
00020       m_pcRNG(NULL),
00021       m_bAddNoise(false) {}
00022 
00023    /****************************************/
00024    /****************************************/
00025 
00026    void CPositioningDefaultSensor::SetRobot(CComposableEntity& c_entity) {
00027       m_pcEmbodiedEntity = &(c_entity.GetComponent<CEmbodiedEntity>("body"));
00028    }
00029 
00030    /****************************************/
00031    /****************************************/
00032 
00033    void CPositioningDefaultSensor::Init(TConfigurationNode& t_tree) {
00034       try {
00035          CCI_PositioningSensor::Init(t_tree);
00036          /* Parse noise range */
00037          GetNodeAttributeOrDefault(t_tree, "pos_noise_range", m_cPosNoiseRange, m_cPosNoiseRange);
00038          GetNodeAttributeOrDefault(t_tree, "angle_noise_range", m_cAngleNoiseRange, m_cAngleNoiseRange);
00039          GetNodeAttributeOrDefault(t_tree, "axis_noise_range", m_cAxisNoiseRange, m_cAxisNoiseRange);
00040          if(m_cPosNoiseRange.GetSpan() != 0 ||
00041             m_cAngleNoiseRange.GetSpan() != CRadians::ZERO ||
00042             m_cAxisNoiseRange.GetSpan() != 0) {
00043             m_bAddNoise = true;
00044             m_pcRNG = CRandom::CreateRNG("argos");
00045          }
00046       }
00047       catch(CARGoSException& ex) {
00048          THROW_ARGOSEXCEPTION_NESTED("Initialization error in default positioning sensor", ex);
00049       }
00050    }
00051 
00052    /****************************************/
00053    /****************************************/
00054    
00055    void CPositioningDefaultSensor::Update() {
00056       m_sReading.Position = m_pcEmbodiedEntity->GetPosition();
00057       if(m_bAddNoise) {
00058          m_sReading.Position += CVector3(m_pcRNG->Uniform(m_cPosNoiseRange),
00059                                          m_pcRNG->Uniform(m_cPosNoiseRange),
00060                                          m_pcRNG->Uniform(m_cPosNoiseRange));
00061          m_pcEmbodiedEntity->GetOrientation().ToAngleAxis(m_cAngle, m_cAxis);
00062          m_cAngle += CRadians(m_pcRNG->Uniform(m_cAngleNoiseRange));
00063          m_cAxis += CVector3(m_pcRNG->Uniform(m_cAxisNoiseRange),
00064                              m_pcRNG->Uniform(m_cAxisNoiseRange),
00065                              m_pcRNG->Uniform(m_cAxisNoiseRange));
00066          m_sReading.Orientation.FromAngleAxis(m_cAngle, m_cAxis);
00067       }
00068       else {
00069          m_sReading.Orientation = m_pcEmbodiedEntity->GetOrientation();
00070       }
00071    }
00072 
00073    /****************************************/
00074    /****************************************/
00075 
00076    void CPositioningDefaultSensor::Reset() {
00077       m_sReading.Position = CVector3();
00078       m_sReading.Orientation = CQuaternion();
00079    }
00080 
00081    /****************************************/
00082    /****************************************/
00083 
00084    REGISTER_SENSOR(CPositioningDefaultSensor,
00085                    "positioning", "default",
00086                    "Carlo Pinciroli [ilpincy@gmail.com]",
00087                    "1.0",
00088                    "A generic positioning sensor.",
00089                    "This sensor returns the current position and orientation of a robot. This sensor\n"
00090                    "can be used with any robot, since it accesses only the body component. In\n"
00091                    "controllers, you must include the ci_positioning_sensor.h header.\n\n"
00092                    "REQUIRED XML CONFIGURATION\n\n"
00093                    "  <controllers>\n"
00094                    "    ...\n"
00095                    "    <my_controller ...>\n"
00096                    "      ...\n"
00097                    "      <sensors>\n"
00098                    "        ...\n"
00099                    "        <positioning implementation=\"default\" />\n"
00100                    "        ...\n"
00101                    "      </sensors>\n"
00102                    "      ...\n"
00103                    "    </my_controller>\n"
00104                    "    ...\n"
00105                    "  </controllers>\n\n"
00106                    "OPTIONAL XML CONFIGURATION\n\n"
00107                    "It is possible to add uniform noise to the sensor, thus matching the\n"
00108                    "characteristics of a real robot better. You can add noise through the\n"
00109                    "attributes 'pos_noise_range', 'angle_noise_range', and 'axis_noise_range'.\n"
00110                    "Attribute 'pos_noise_range' regulates the noise range on the position returned\n"
00111                    "by the sensor. Attribute 'angle_noise_range' sets the noise range on the angle\n"
00112                    "(values expressed in degrees). Attribute 'axis_noise_range' sets the noise for\n"
00113                    "the rotation axis. Angle and axis are used to calculate a quaternion, which is\n"
00114                    "the actual returned value for rotation.\n\n"
00115                    "  <controllers>\n"
00116                    "    ...\n"
00117                    "    <my_controller ...>\n"
00118                    "      ...\n"
00119                    "      <sensors>\n"
00120                    "        ...\n"
00121                    "        <positioning implementation=\"default\"\n"
00122                    "                     pos_noise_range=\"-0.1:0.2\"\n"
00123                    "                     angle_noise_range=\"-10.5:13.7\"\n"
00124                    "                     axis_noise_range=\"-0.3:0.4\" />\n"
00125                    "        ...\n"
00126                    "      </sensors>\n"
00127                    "      ...\n"
00128                    "    </my_controller>\n"
00129                    "    ...\n"
00130                    "  </controllers>\n\n"
00131                    "OPTIONAL XML CONFIGURATION\n\n"
00132                    "None.\n",
00133                    "Usable"
00134                   );
00135 
00136 }