camera_sensor_led_detector_algorithm.cpp
Go to the documentation of this file.
1 
8 
9 #include <argos3/core/simulator/simulator.h>
10 #include <argos3/core/utility/math/matrix/squarematrix.h>
11 #include <argos3/core/utility/math/matrix/transformationmatrix3.h>
12 
13 #include <argos3/plugins/simulator/media/led_medium.h>
14 #include <argos3/plugins/simulator/entities/led_entity.h>
15 
16 namespace argos {
17 
18  /****************************************/
19  /****************************************/
20 
22  m_bShowRays(false),
23  m_pcLEDIndex(nullptr) {}
24 
25  /****************************************/
26  /****************************************/
27 
29  try {
30  /* Parent class init */
32  /* Show rays? */
33  GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
34  /* Get LED medium from id specified in the XML */
35  std::string strMedium;
36  GetNodeAttribute(t_tree, "medium", strMedium);
37  m_pcLEDIndex = &(CSimulator::GetInstance().GetMedium<CLEDMedium>(strMedium).GetIndex());
38  }
39  catch(CARGoSException& ex) {
40  THROW_ARGOSEXCEPTION_NESTED("Error initializing the LED detector algorithm", ex);
41  }
42  }
43 
44  /****************************************/
45  /****************************************/
46 
48  const std::array<CPlane, 6>& arr_frustum_planes,
49  const CTransformationMatrix3& c_camera_to_world_transform,
50  const CVector3& c_camera_location,
51  const CVector3& c_bounding_box_position,
52  const CVector3& c_bounding_box_half_extents) {
53  /* Define a class for the update operation */
54  CUpdateOperation cUpdateOperation(c_projection_matrix, arr_frustum_planes,
55  c_camera_to_world_transform, c_camera_location,
56  *this);
57  /* Clear out readings from last update */
58  m_vecReadings.clear();
59  /* Clear out checked rays from last update */
60  m_vecCheckedRays.clear();
61  /* Run the operation */
62  m_pcLEDIndex->ForEntitiesInBoxRange(c_bounding_box_position,
63  c_bounding_box_half_extents,
64  cUpdateOperation);
65  }
66 
67  /****************************************/
68  /****************************************/
69 
71  "led_detector",
72  "Michael Allwright [allsey87@gmail.com]",
73  "1.0",
74  "This algorithm detects nearby LEDs seen by the camera and\n"
75  "returns the X and Y coordinates on the sensor",
76  "This algorithm detects nearby LEDs seen by the camera and\n"
77  "returns the X and Y coordinates on the sensor",
78  "Under development");
79 }
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception.
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12
REGISTER_CAMERA_SENSOR_ALGORITHM(CCameraSensorDirectionalLEDDetectorAlgorithm, "directional_led_detector", "Michael Allwright [allsey87@gmail.com]", "1.0", "This algorithm detects nearby LEDs seen by the camera and\n" "returns the X and Y coordinates on the sensor", "This algorithm detects nearby LEDs seen by the camera and\n" "returns the X and Y coordinates on the sensor", "Under development")
void GetNodeAttributeOrDefault(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer, const T &t_default)
Returns the value of a node's attribute, or the passed default value.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node's attribute.
T & GetMedium(const std::string &str_id)
Returns a reference to a medium.
Definition: simulator.h:129
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:78
The exception that wraps all errors in ARGoS.
A 3D vector class.
Definition: vector3.h:31
virtual void Init(TConfigurationNode &t_node)
Initializes the resource.
std::vector< std::pair< bool, CRay3 > > m_vecCheckedRays
This class provides the most general interface to a camera.
virtual void Update(const CSquareMatrix< 3 > &c_projection_matrix, const std::array< CPlane, 6 > &arr_frustum_planes, const CTransformationMatrix3 &c_camera_to_world_transform, const CVector3 &c_camera_location, const CVector3 &c_bounding_box_position, const CVector3 &c_bounding_box_half_extents)
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.