ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/sensors/differential_steering_default_sensor.cpp
Go to the documentation of this file.
00001 
00007 #include <argos3/core/simulator/simulator.h>
00008 #include <argos3/plugins/simulator/entities/wheeled_entity.h>
00009 #include <argos3/core/simulator/entity/composable_entity.h>
00010 
00011 #include "differential_steering_default_sensor.h"
00012 
00013 namespace argos {
00014 
00015    /****************************************/
00016    /****************************************/
00017 
00018    CDifferentialSteeringDefaultSensor::CDifferentialSteeringDefaultSensor() :
00019       m_pcWheeledEntity(NULL),
00020       m_pcRNG(NULL),
00021       m_bAddNoise(false) {}
00022 
00023    /****************************************/
00024    /****************************************/
00025 
00026    void CDifferentialSteeringDefaultSensor::SetRobot(CComposableEntity& c_entity) {
00027       try {
00028          m_pcWheeledEntity = &(c_entity.GetComponent<CWheeledEntity>("wheels"));
00029          if(m_pcWheeledEntity->GetNumWheels() != 2) {
00030             THROW_ARGOSEXCEPTION("The differential steering sensor can be associated only to a robot with 2 wheels");
00031          }
00032          m_pfWheelVelocities = m_pcWheeledEntity->GetWheelVelocities();
00033          m_sReading.WheelAxisLength = 100.0f * Distance(m_pcWheeledEntity->GetWheelPosition(0),
00034                                                         m_pcWheeledEntity->GetWheelPosition(1));
00035       }
00036       catch(CARGoSException& ex) {
00037          THROW_ARGOSEXCEPTION_NESTED("Error setting differential steering sensor to entity \"" << c_entity.GetId() << "\"", ex);
00038       }
00039    }
00040 
00041    /****************************************/
00042    /****************************************/
00043 
00044    void CDifferentialSteeringDefaultSensor::Init(TConfigurationNode& t_tree) {
00045       try {
00046          CCI_DifferentialSteeringSensor::Init(t_tree);
00047          /* Parse noise range */
00048          GetNodeAttributeOrDefault(t_tree, "vel_noise_range",  m_cVelNoiseRange,  m_cVelNoiseRange);
00049          GetNodeAttributeOrDefault(t_tree, "dist_noise_range", m_cDistNoiseRange, m_cDistNoiseRange);
00050          if(m_cVelNoiseRange.GetSpan() != 0 ||
00051             m_cDistNoiseRange.GetSpan() != 0) {
00052             m_bAddNoise = true;
00053             m_pcRNG = CRandom::CreateRNG("argos");
00054          }
00055       }
00056       catch(CARGoSException& ex) {
00057          THROW_ARGOSEXCEPTION_NESTED("Initialization error in default differential steering sensor", ex);
00058       }
00059    }
00060 
00061    /****************************************/
00062    /****************************************/
00063    
00064    void CDifferentialSteeringDefaultSensor::Update() {
00065       m_sReading.VelocityLeftWheel = m_pfWheelVelocities[0] * 100.0f;
00066       m_sReading.VelocityRightWheel = m_pfWheelVelocities[1] * 100.0f;
00067       m_sReading.CoveredDistanceLeftWheel = m_sReading.VelocityLeftWheel * CPhysicsEngine::GetSimulationClockTick();
00068       m_sReading.CoveredDistanceRightWheel = m_sReading.VelocityRightWheel * CPhysicsEngine::GetSimulationClockTick();
00069       if(m_bAddNoise) {
00070          m_sReading.VelocityLeftWheel  += m_pcRNG->Uniform(m_cVelNoiseRange);
00071          m_sReading.VelocityRightWheel += m_pcRNG->Uniform(m_cVelNoiseRange);
00072          m_sReading.CoveredDistanceLeftWheel  += m_pcRNG->Uniform(m_cDistNoiseRange);
00073          m_sReading.CoveredDistanceRightWheel += m_pcRNG->Uniform(m_cDistNoiseRange);
00074       }
00075    }
00076 
00077    /****************************************/
00078    /****************************************/
00079 
00080    void CDifferentialSteeringDefaultSensor::Reset() {
00081       m_sReading.VelocityLeftWheel         = 0.0f;
00082       m_sReading.VelocityRightWheel        = 0.0f;
00083       m_sReading.CoveredDistanceLeftWheel  = 0.0f;
00084       m_sReading.CoveredDistanceRightWheel = 0.0f;
00085    }
00086 
00087    /****************************************/
00088    /****************************************/
00089 
00090    REGISTER_SENSOR(CDifferentialSteeringDefaultSensor,
00091                    "differential_steering", "default",
00092                    "Carlo Pinciroli [ilpincy@gmail.com]",
00093                    "1.0",
00094                    "A generic differential steering sensor.",
00095                    "This sensor returns the current position and orientation of a robot. This sensor\n"
00096                    "can be used with any robot, since it accesses only the body component. In\n"
00097                    "controllers, you must include the ci_differential_steering_sensor.h header.\n\n"
00098                    "REQUIRED XML CONFIGURATION\n\n"
00099                    "  <controllers>\n"
00100                    "    ...\n"
00101                    "    <my_controller ...>\n"
00102                    "      ...\n"
00103                    "      <sensors>\n"
00104                    "        ...\n"
00105                    "        <differential_steering implementation=\"default\" />\n"
00106                    "        ...\n"
00107                    "      </sensors>\n"
00108                    "      ...\n"
00109                    "    </my_controller>\n"
00110                    "    ...\n"
00111                    "  </controllers>\n\n"
00112                    "OPTIONAL XML CONFIGURATION\n\n"
00113                    "It is possible to add uniform noise to the sensor, thus matching the\n"
00114                    "characteristics of a real robot better. You can add noise through the\n"
00115                    "attributes 'vel_noise_range' and 'dist_noise_range'.\n"
00116                    "Attribute 'vel_noise_range' regulates the noise range on the velocity returned\n"
00117                    "by the sensor. Attribute 'dist_noise_range' sets the noise range on the\n"
00118                    "distance covered by the wheels.\n"
00119                    "value for rotation.\n\n"
00120                    "  <controllers>\n"
00121                    "    ...\n"
00122                    "    <my_controller ...>\n"
00123                    "      ...\n"
00124                    "      <sensors>\n"
00125                    "        ...\n"
00126                    "        <differential_steering implementation=\"default\"\n"
00127                    "                               vel_noise_range=\"-0.1:0.2\"\n"
00128                    "                               dist_noise_range=\"-10.5:13.7\" />\n"
00129                    "        ...\n"
00130                    "      </sensors>\n"
00131                    "      ...\n"
00132                    "    </my_controller>\n"
00133                    "    ...\n"
00134                    "  </controllers>\n\n"
00135                    "OPTIONAL XML CONFIGURATION\n\n"
00136                    "None.\n",
00137                    "Usable"
00138                   );
00139 
00140 }