ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/actuators/differential_steering_default_actuator.cpp
Go to the documentation of this file.
00001 
00007 #include "differential_steering_default_actuator.h"
00008 #include <argos3/core/utility/logging/argos_log.h>
00009 #include <argos3/core/utility/plugins/factory.h>
00010 
00011 namespace argos {
00012 
00013    /****************************************/
00014    /****************************************/
00015 
00016    CDifferentialSteeringDefaultActuator::CDifferentialSteeringDefaultActuator() :
00017       m_pcWheeledEntity(NULL),
00018       m_pcRNG(NULL),
00019       m_fNoiseStdDeviation(0.0f) {
00020       m_fCurrentVelocity[LEFT_WHEEL] = 0.0f;
00021       m_fCurrentVelocity[RIGHT_WHEEL] = 0.0f;
00022    }
00023    
00024    /****************************************/
00025    /****************************************/
00026    
00027    void CDifferentialSteeringDefaultActuator::SetRobot(CComposableEntity& c_entity) {
00028       try {
00029          m_pcWheeledEntity = &(c_entity.GetComponent<CWheeledEntity>("wheels"));
00030          if(m_pcWheeledEntity->GetNumWheels() != 2) {
00031             THROW_ARGOSEXCEPTION("The differential steering actuator can be associated only to a robot with 2 wheels");
00032          }
00033       }
00034       catch(CARGoSException& ex) {
00035          THROW_ARGOSEXCEPTION_NESTED("Error setting differential steering actuator to entity \"" << c_entity.GetId() << "\"", ex);
00036       }
00037    }
00038 
00039    /****************************************/
00040    /****************************************/
00041 
00042    void CDifferentialSteeringDefaultActuator::Init(TConfigurationNode& t_tree) {
00043       try {
00044          CCI_DifferentialSteeringActuator::Init(t_tree);
00045          GetNodeAttributeOrDefault<Real>(t_tree, "noise_std_dev", m_fNoiseStdDeviation, 0.0f);
00046          if(m_fNoiseStdDeviation > 0.0f) {
00047             m_pcRNG = CRandom::CreateRNG("argos");
00048          }
00049       }
00050       catch(CARGoSException& ex) {
00051          THROW_ARGOSEXCEPTION_NESTED("Initialization error in foot-bot steering actuator.", ex);
00052       }
00053    }
00054 
00055    /****************************************/
00056    /****************************************/
00057    
00058    void CDifferentialSteeringDefaultActuator::SetLinearVelocity(Real f_left_velocity,
00059                                                   Real f_right_velocity) {
00060       /* Convert speeds in m/s */
00061       m_fCurrentVelocity[LEFT_WHEEL] = f_left_velocity * 0.01f;
00062       m_fCurrentVelocity[RIGHT_WHEEL] = f_right_velocity * 0.01f;
00063       /* Apply noise */
00064       if(m_fNoiseStdDeviation > 0.0f) {
00065          AddGaussianNoise();
00066       }
00067    }
00068    
00069    /****************************************/
00070    /****************************************/
00071    
00072    void CDifferentialSteeringDefaultActuator::Update() {
00073       m_pcWheeledEntity->SetVelocities(m_fCurrentVelocity);
00074    }
00075 
00076    /****************************************/
00077    /****************************************/
00078    
00079    void CDifferentialSteeringDefaultActuator::Reset() {
00080       m_fCurrentVelocity[LEFT_WHEEL]  = 0.0f;
00081       m_fCurrentVelocity[RIGHT_WHEEL] = 0.0f;
00082    }
00083    
00084    /****************************************/
00085    /****************************************/
00086    
00087    void CDifferentialSteeringDefaultActuator::AddGaussianNoise() {
00088       m_fCurrentVelocity[LEFT_WHEEL]  += m_fCurrentVelocity[LEFT_WHEEL] * m_pcRNG->Gaussian(m_fNoiseStdDeviation);
00089       m_fCurrentVelocity[RIGHT_WHEEL] += m_fCurrentVelocity[RIGHT_WHEEL] * m_pcRNG->Gaussian(m_fNoiseStdDeviation);
00090    }
00091 
00092    /****************************************/
00093    /****************************************/
00094 
00095 }
00096 
00097 REGISTER_ACTUATOR(CDifferentialSteeringDefaultActuator,
00098                   "differential_steering", "default",
00099                   "Carlo Pinciroli [ilpincy@gmail.com]",
00100                   "1.0",
00101                   "The differential steering actuator.",
00102                   "This actuator controls the two wheels a differential steering robot. For a\n"
00103                   "complete description of its usage, refer to the\n"
00104                   "ci_differential_steering_actuator.h file.\n\n"
00105                   "REQUIRED XML CONFIGURATION\n\n"
00106                   "  <controllers>\n"
00107                   "    ...\n"
00108                   "    <my_controller ...>\n"
00109                   "      ...\n"
00110                   "      <actuators>\n"
00111                   "        ...\n"
00112                   "        <differential_steering implementation=\"default\" />\n"
00113                   "        ...\n"
00114                   "      </actuators>\n"
00115                   "      ...\n"
00116                   "    </my_controller>\n"
00117                   "    ...\n"
00118                   "  </controllers>\n\n"
00119                   "OPTIONAL XML CONFIGURATION\n\n"
00120                   "It is possible to specify noisy speed in order to match the characteristics\n"
00121                   "of the real robot. This can be done with the attribute: 'noise_std_dev',\n" 
00122                   "which indicates the standard deviation of a gaussian noise applied to the\n"
00123                   "desired velocity of the steering:\n\n"
00124                   "  <controllers>\n"
00125                   "    ...\n"
00126                   "    <my_controller ...>\n"
00127                   "      ...\n"
00128                   "      <actuators>\n"
00129                   "        ...\n"
00130                   "        <differential_steering implementation=\"default\"\n"
00131                   "                               noise_std_dev=\"1\" />\n"
00132                   "        ...\n"
00133                   "      </actuators>\n"
00134                   "      ...\n"
00135                   "    </my_controller>\n"
00136                   "    ...\n"
00137                   "  </controllers>\n",
00138                   "Usable"
00139    );
00140