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