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