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